AP_Airspeed: added logging of airspeed calibration internals

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -131,11 +131,15 @@ float AP_Airspeed::get_pressure(void)
         float sum = 0;
         uint16_t count = 0;
         struct differential_pressure_s report;
+        static uint64_t last_timestamp;
 
-        while (::read(_ets_fd, &report, sizeof(report)) == sizeof(report)) {
+        while (::read(_ets_fd, &report, sizeof(report)) == sizeof(report) &&
+               report.timestamp != last_timestamp) {
             sum += report.differential_pressure_pa;
             count++;
+            last_timestamp = report.timestamp;
         }
+        // hal.console->printf("count=%u\n", (unsigned)count);
         if (count == 0) {
             return _last_pressure;
         }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -6,11 +6,14 @@
 #include <AP_Common.h>
 #include <AP_HAL.h>
 #include <AP_Param.h>
+#include <GCS_MAVLink.h>
+#include <AP_SpdHgtControl.h>
 
 class Airspeed_Calibration {
 public:
+    friend class AP_Airspeed;
     // constructor
-    Airspeed_Calibration(void);
+    Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms);
 
     // initialise the calibration
     void init(float initial_ratio);
@@ -26,15 +29,17 @@ private:
     const float Q1; // process noise matrix bottom right element
     Vector3f state; // state vector
     const float DT; // time delta
+    const AP_SpdHgtControl::AircraftParameters &aparm;
 };
 
 class AP_Airspeed
 {
 public:
     // constructor
-    AP_Airspeed() : 
+    AP_Airspeed(const AP_SpdHgtControl::AircraftParameters &parms) : 
         _ets_fd(-1),
-        _EAS2TAS(1.0f)
+        _EAS2TAS(1.0f),
+        _calibration(parms)
     {
 		AP_Param::setup_object_defaults(this, var_info);
     };
@@ -110,10 +115,14 @@ public:
     }
 
     // update airspeed ratio calibration
-    void update_calibration(Vector3f vground);
+    void update_calibration(const Vector3f &vground);
+
+	// log data to MAVLink
+	void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
 
     static const struct AP_Param::GroupInfo var_info[];
 
+
 private:
     AP_HAL::AnalogSource *_source;
     AP_Float        _offset;

libraries/AP_Airspeed/Airspeed_Calibration.cpp

@@ -14,14 +14,15 @@
 extern const AP_HAL::HAL& hal;
 
 // constructor - fill in all the initial values
-Airspeed_Calibration::Airspeed_Calibration() :
+Airspeed_Calibration::Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms) :
     P(100,   0,         0,
       0,   100,         0,
       0,     0,  0.000001f),
     Q0(0.01f),
     Q1(0.000001f),
     state(0, 0, 0),
-    DT(1)
+    DT(1),
+    aparm(parms)
 {
 }
 
@@ -108,15 +109,17 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
 /*
   called once a second to do calibration update
  */
-void AP_Airspeed::update_calibration(Vector3f vground)
+void AP_Airspeed::update_calibration(const Vector3f &vground)
 {
     if (!_autocal) {
         // auto-calibration not enabled
         return;
     }
     // calculate true airspeed, assuming a airspeed ratio of 1.0
-    float true_airspeed = sqrtf(get_differential_pressure()) * _EAS2TAS;
+    float dpress = get_differential_pressure();
+    float true_airspeed = sqrtf(dpress) * _EAS2TAS;
     float ratio = _calibration.update(true_airspeed, vground);
+
     if (isnan(ratio) || isinf(ratio)) {
         return;
     }
@@ -134,3 +137,21 @@ void AP_Airspeed::update_calibration(Vector3f vground)
         _counter++;
     }
 }
+
+// log airspeed calibration data to MAVLink
+void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
+{
+    mavlink_msg_airspeed_autocal_send(chan,
+                                      vground.x,
+                                      vground.y,
+                                      vground.z,
+                                      get_differential_pressure(),
+                                      _EAS2TAS,
+                                      _ratio.get(),
+                                      _calibration.state.x,
+                                      _calibration.state.y,
+                                      _calibration.state.z,
+                                      _calibration.P.a.x,
+                                      _calibration.P.b.y,
+                                      _calibration.P.c.z);
+}