aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: reject 0xff frames from BMI088 gyro

Browse Source 
a log on a board with a BMI088 seems to show that this happened
This commit is contained in:
Andrew Tridgell 2023-01-22 10:08:50 +11:00
parent c19554e23a
commit 08d420a08c
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -400,6 +400,7 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void)
}
const float scale = radians(2000.0f) / 32767.0f;
const uint8_t max_frames = 8;
const Vector3i bad_frame{int16_t(0xffff), int16_t(0xffff), int16_t(0xffff)};
Vector3i data[max_frames];
if (num_frames & 0x80) {
@ -427,6 +428,10 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void)
// data is 16 bits with 2000dps range
for (uint8_t i = 0; i < num_frames; i++) {
if (data[i] == bad_frame) {
_inc_gyro_error_count(gyro_instance);
continue;
}
Vector3f gyro(data[i].x, data[i].y, data[i].z);
gyro *= scale;