AP_Airspeed: use non-blocking airspeed calibration

this prevents the EKF from being upset by a time jump

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -22,6 +22,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -174,8 +175,6 @@ bool AP_Airspeed::get_temperature(float &temperature)
 // the get_airspeed() interface can be used
 void AP_Airspeed::calibrate(bool in_startup)
 {
-    float sum = 0;
-    uint8_t count = 0;
     if (!_enable) {
         return;
     }
@@ -184,24 +183,27 @@ void AP_Airspeed::calibrate(bool in_startup)
     }
     // discard first reading
     get_pressure();
-    for (uint8_t i = 0; i < 10; i++) {
-        hal.scheduler->delay(100);
-        float p = get_pressure();
-        if (_healthy) {
-            sum += p;
-            count++;
+    _cal.start_ms = AP_HAL::millis();
+    _cal.count = 0;
+    _cal.sum = 0;
+}
+
+void AP_Airspeed::update_calibration(float raw_pressure)
+{
+    if (AP_HAL::millis() - _cal.start_ms >= 1000) {
+        if (_cal.count == 0) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy");
+        } else {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor calibrated");
+            _offset.set_and_save(_cal.sum / _cal.count);
         }
-    }
-    if (count == 0) {
-        // unhealthy sensor
-        hal.console->println("Airspeed sensor unhealthy");
-        _offset.set(0);
+        _cal.start_ms = 0;
         return;
     }
-    float raw = sum/count;
-    _offset.set_and_save(raw);
-    _airspeed = 0;
-    _raw_airspeed = 0;
+    if (_healthy) {
+        _cal.sum += raw_pressure;
+        _cal.count++;
+    }
 }
 
 // read the airspeed sensor
@@ -211,10 +213,15 @@ void AP_Airspeed::read(void)
     if (!_enable) {
         return;
     }
-    airspeed_pressure = get_pressure() - _offset;
+    float raw_pressure = get_pressure();
+    if (_cal.start_ms != 0) {
+        update_calibration(raw_pressure);
+    }
+    
+    airspeed_pressure = raw_pressure - _offset;
 
     // remember raw pressure for logging
-    _raw_pressure     = airspeed_pressure;
+    _corrected_pressure = airspeed_pressure;
 
     /*
       we support different pitot tube setups so used can choose if

libraries/AP_Airspeed/AP_Airspeed.h

@@ -115,9 +115,9 @@ public:
         return _offset;
     }
 
-    // return the current raw pressure
-    float get_raw_pressure(void) const {
-        return _raw_pressure;
+    // return the current corrected pressure
+    float get_corrected_pressure(void) const {
+        return _corrected_pressure;
     }
 
     // set the apparent to true airspeed ratio
@@ -164,18 +164,26 @@ private:
     float           _raw_airspeed;
     float           _airspeed;
     float			_last_pressure;
-    float			_raw_pressure;
+    float			_corrected_pressure;
     float           _EAS2TAS;
     bool		    _healthy:1;
     bool		    _hil_set:1;
     float           _hil_pressure;
     uint32_t        _last_update_ms;
 
+    // state of runtime calibration
+    struct {
+        uint32_t        start_ms;
+        uint16_t        count;
+        float           sum;
+    } _cal;
+
     Airspeed_Calibration _calibration;
     float _last_saved_ratio;
     uint8_t _counter;
 
     float get_pressure(void);
+    void update_calibration(float raw_pressure);
 
     AP_Airspeed_Analog analog;
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN