From a357849d4ccbbbb3de64e4e97ec1b449540a6ebd Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 10 Jul 2026 17:10:35 +0000 Subject: [PATCH] fix(simulation): clamp simulated FIFO accel/gyro samples to int16 range The simulated FIFO emulation for accel 0 and gyro 0 converts SI values to raw counts and assigns them to int16 FIFO fields. Beyond the emulated full-scale range (16 g / 2000 deg/s) the conversion overflows and wraps, so e.g. a 50 rad/s spin is reported as roughly -20 rad/s. Constrain the counts to the int16 range so out-of-range values saturate at full scale and are reported as clipping by the sensor driver, which matches real IMU behavior. Fixes #23469 Signed-off-by: Claude --- .../simulator_mavlink/SimulatorMavlink.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index bf05f60f2579..d79f98bdf92c 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -224,9 +224,11 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil _last_accel_fifo.samples = 1; _last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample; _last_accel_fifo.timestamp_sample = time; - _last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE; - _last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE; - _last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE; + // constrain to the FIFO int16 range: values beyond full scale saturate + // (and get reported as clipping) instead of wrapping around + _last_accel_fifo.x[0] = math::constrain(sensors.xacc / ACCEL_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); + _last_accel_fifo.y[0] = math::constrain(sensors.yacc / ACCEL_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); + _last_accel_fifo.z[0] = math::constrain(sensors.zacc / ACCEL_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); _px4_accel[sensors.id].updateFIFO(_last_accel_fifo); } @@ -267,9 +269,11 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil _last_gyro_fifo.samples = 1; _last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample; _last_gyro_fifo.timestamp_sample = time; - _last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE; - _last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE; - _last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE; + // constrain to the FIFO int16 range: rates beyond full scale saturate + // (and get reported as clipping) instead of wrapping around + _last_gyro_fifo.x[0] = math::constrain(sensors.xgyro / GYRO_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); + _last_gyro_fifo.y[0] = math::constrain(sensors.ygyro / GYRO_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); + _last_gyro_fifo.z[0] = math::constrain(sensors.zgyro / GYRO_FIFO_SCALE, (float)INT16_MIN, (float)INT16_MAX); _px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo); }