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

AP_InertialSensor: correctly detect bad frames on regular ICM42xxx

Browse Source
This commit is contained in:
Andy Piper 2022-08-06 20:23:38 +01:00 committed by Andrew Tridgell
parent 2b7bb84838
commit a6312ac893
1 changed files with 4 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -119,7 +119,7 @@ struct PACKED FIFOData {
}; };
#define INV3_SAMPLE_SIZE sizeof(FIFOData) #define INV3_SAMPLE_SIZE sizeof(FIFOData)
#define INV3_FIFO_BUFFER_LEN 2 #define INV3_FIFO_BUFFER_LEN 8
AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev, AP_HAL::OwnPtr<AP_HAL::Device> _dev,
@ -300,17 +300,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
// we have a header to confirm we don't have FIFO corruption! no more mucking // we have a header to confirm we don't have FIFO corruption! no more mucking
// about with the temperature registers // about with the temperature registers
if (inv3_type == Invensensev3_Type::ICM42670) { if ((d.header & 0xFC) != 0x68) {
if ((d.header & 0xFC) != 0x68) { // no or bad data
// no or bad data return false;
return false;
}
} else {
if ((d.header & 0xF8) != 0x68) {
// no or bad data
return false;
}
} }
Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])}; Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])};