Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: Sanity check DLVR raw data before accumulating it

Browse Source 
This is intended to help with really bad RF interference flipping bits on the I2C bus which can lead to some intresting results
This commit is contained in:
Michael du Breuil 2021-12-28 17:21:31 -07:00 committed by Peter Barker
parent 6e684b3496
commit 382cbb9b4d
1 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
View File

@ -104,7 +104,7 @@ void AP_Airspeed_DLVR::timer()
if ((data >> STATUS_SHIFT)) {
// anything other then 00 in the status bits is an error
Debug("DLVR: Bad status read %d", data >> STATUS_SHIFT);
Debug("DLVR: Bad status read %u", (unsigned int)(data >> STATUS_SHIFT));
return;
}
@ -112,15 +112,31 @@ void AP_Airspeed_DLVR::timer()
uint32_t temp_raw = (data >> TEMPERATURE_SHIFT) & TEMPERATURE_MASK;
float press_h2o = 1.25f * 2.0f * range_inH2O * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
if ((press_h2o > range_inH2O) || (press_h2o < -range_inH2O)) {
Debug("DLVR: Out of range pressure %f", press_h2o);
return;
}
float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;
WITH_SEMAPHORE(sem);
const uint32_t now = AP_HAL::millis();
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as we are only worried about the case where we had never read
if ((temperature != 0) && (fabsf(temperature - temp) > 10) && ((now - last_sample_time_ms) < 2000)) {
Debug("DLVR: Temperature swing %f", temp);
return;
}
#pragma GCC diagnostic pop
pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
temperature_sum += temp;
press_count++;
temp_count++;
last_sample_time_ms = AP_HAL::millis();
last_sample_time_ms = now;
}
// return the current differential_pressure in Pascal