dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

bmi088: Do not use invalid values

This commit is contained in:
muramura 2023-04-29 13:08:29 +09:00 committed by Daniel Agar
parent f0bea542d5
commit b23eee40f0
2 changed files with 22 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
View File

@ -427,6 +427,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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;
}

15
src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
View File

@ -346,6 +346,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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;
}