Browse Source

AP_Airspeed: ensure we have at least 10 samples for airspeed cal

thanks to Michael for pointing out this issue
master
Andrew Tridgell 9 years ago
parent
commit
689595080a
  1. 10
      libraries/AP_Airspeed/AP_Airspeed.cpp
  2. 1
      libraries/AP_Airspeed/AP_Airspeed.h

10
libraries/AP_Airspeed/AP_Airspeed.cpp

@ -186,11 +186,18 @@ void AP_Airspeed::calibrate(bool in_startup) @@ -186,11 +186,18 @@ void AP_Airspeed::calibrate(bool in_startup)
_cal.start_ms = AP_HAL::millis();
_cal.count = 0;
_cal.sum = 0;
_cal.read_count = 0;
}
/*
update async airspeed calibration
*/
void AP_Airspeed::update_calibration(float raw_pressure)
{
if (AP_HAL::millis() - _cal.start_ms >= 1000) {
// consider calibration complete when we have at least 10 samples
// over at least 1 second
if (AP_HAL::millis() - _cal.start_ms >= 1000 &&
_cal.read_count > 10) {
if (_cal.count == 0) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy");
} else {
@ -204,6 +211,7 @@ void AP_Airspeed::update_calibration(float raw_pressure) @@ -204,6 +211,7 @@ void AP_Airspeed::update_calibration(float raw_pressure)
_cal.sum += raw_pressure;
_cal.count++;
}
_cal.read_count++;
}
// read the airspeed sensor

1
libraries/AP_Airspeed/AP_Airspeed.h

@ -176,6 +176,7 @@ private: @@ -176,6 +176,7 @@ private:
uint32_t start_ms;
uint16_t count;
float sum;
uint16_t read_count;
} _cal;
Airspeed_Calibration _calibration;

Loading…
Cancel
Save