Commit Graph
2 Commits
Author SHA1 Message Date
Jacob Dahl c3f3442c12 [BACKPORT 1.18] fix(drivers/imu): InvenSense FIFO scale, invalid samples and clipping (#29529)
* fix(drivers/invensense): always publish full-scale FIFO data (#28134)

Publish the 16 bit FIFO registers directly and set the scale once in
Configure(), removing the dual decode pass and the per-batch scale
switch. The 20 bit extension nibble is no longer used.

Also corrects the IIM42653 range, which reported +/-16 g and +/-2000 dps
while ACCEL_CONFIG0/GYRO_CONFIG0 program +/-32 g and +/-4000 dps.

(cherry picked from commit 7eede5e670)

* fix(drivers/invensense): drop invalid gyro FIFO samples (#28308)

With FIFO_HOLD_LAST_DATA_EN clear the sensor inserts -32768 for accel and gyro samples that carry no data (power-on until the first ODR sample, a disabled sensor, mismatched ODRs) and limits valid samples to -32766..+32767. The accel path already drops these; the gyro path published them as negative full scale.

(cherry picked from commit 243936abe2)

* fix(drivers/imu): clip FIFO samples against the declared range, not the int16 rail (#28310)

* fix(drivers/imu): clip FIFO samples against the declared range, not the int16 rail

updateFIFO() counted a sample as clipped only within 1 LSB of INT16_MIN
or INT16_MAX, while update() compares against _clip_limit (range/scale).
The two agree only for sensors whose sensitivity is exactly range/32768.
ST parts leave headroom in the word: the LSM6DSV80X high-g channel at
+/-80 g and 3.904 mg/LSB saturates at 20492 counts, its gyro at
+/-4000 dps and 140 mdps/LSB at 28571, the LSM9DS1 and ADIS16607 are
similar, and the BMI055's 12-bit accel word never reaches the rail at
all. None of them could report a clip on the FIFO path, so the EKF's
delta-velocity clipping handling never engaged on those sensors.

Compare against _clip_limit on the FIFO path as well. For rail-scaled
sensors the threshold moves from 32766 to 32735 counts (the existing
0.999 margin), which is what the non-FIFO path already used.

* docs(msg): SensorGyroFifo/SensorAccelFifo counts are raw, not SI

x/y/z are int16 counts; SI is count * scale. The comments called them
rad/s and m/s^2, which is what sensor_gyro/sensor_accel carry.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

---------

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
(cherry picked from commit b4f9388cd9)

---------

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
2026-11-05 09:53:10 -07:00
Daniel Agar cea185268e msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge
- update all msgs to be directly compatible with ROS2
 - microdds_client improvements
   - timesync
   - reduced code size
   - add to most default builds if we can afford it
   - lots of other little changes
 - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
2022-10-19 19:36:47 -04:00