diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 8b13ff6e56..5c8714712f 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -427,6 +427,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; + int index = 0; + for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; @@ -436,15 +438,20 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + if (!(gyro_x == INT16_MIN && gyro_y == INT16_MIN && gyro_z == INT16_MIN)) { + gyro.x[index] = gyro_x; + gyro.y[index] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[index] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + ++index; + } } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - _px4_gyro.updateFIFO(gyro); + if (index > 0) { + _px4_gyro.updateFIFO(gyro); + } return true; } diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index ad177c68e5..0592ec2750 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -346,6 +346,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; + int index = 0; + for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; @@ -355,15 +357,20 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + if (!(gyro_x == INT16_MIN && gyro_y == INT16_MIN && gyro_z == INT16_MIN)) { + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + ++index; + } } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - _px4_gyro.updateFIFO(gyro); + if (index > 0) { + _px4_gyro.updateFIFO(gyro); + } return true; }