From c25ce2cee864c9827714ccf369686753bc458bec Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 23 Nov 2018 10:43:22 +1100
Subject: [PATCH] AP_AirSpeed: move sending of airspeed_autocal into
 AP_AirSpeed

---
 libraries/AP_Airspeed/AP_Airspeed.h           |  5 +--
 .../AP_Airspeed/Airspeed_Calibration.cpp      | 42 ++++++++++++-------
 2 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h
index 3fa4758614..2bb695f5a8 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.h
+++ b/libraries/AP_Airspeed/AP_Airspeed.h
@@ -134,9 +134,6 @@ public:
     // update airspeed ratio calibration
     void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
 
-	// log data to MAVLink
-	void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
-
     // return health status of sensor
     bool healthy(uint8_t i) const {
         return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i);
@@ -243,9 +240,11 @@ private:
     float get_pressure(uint8_t i);
     void update_calibration(uint8_t i, float raw_pressure);
     void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
+    void send_airspeed_calibration(const Vector3f &vg);
 
     void check_sensor_failures();
     void check_sensor_ahrs_wind_max_failures(uint8_t i);
 
     AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
+
 };
diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
index d259e07abd..512137be9c 100644
--- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp
+++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
@@ -8,6 +8,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
 
 #include "AP_Airspeed.h"
 
@@ -157,22 +158,33 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
     for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
         update_calibration(i, vground, max_airspeed_allowed_during_cal);
     }
+    send_airspeed_calibration(vground);
 }
 
-// log airspeed calibration data to MAVLink
-void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
+
+void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
 {
-    mavlink_msg_airspeed_autocal_send(chan,
-                                      vground.x,
-                                      vground.y,
-                                      vground.z,
-                                      get_differential_pressure(primary),
-                                      state[primary].EAS2TAS,
-                                      param[primary].ratio.get(),
-                                      state[primary].calibration.state.x,
-                                      state[primary].calibration.state.y,
-                                      state[primary].calibration.state.z,
-                                      state[primary].calibration.P.a.x,
-                                      state[primary].calibration.P.b.y,
-                                      state[primary].calibration.P.c.z);
+    for (uint8_t i=0; i<gcs().num_gcs(); i++) {
+        if (!gcs().chan(i).initialised) {
+            continue;
+        }
+        const mavlink_channel_t chan = (mavlink_channel_t)i;
+        if (!HAVE_PAYLOAD_SPACE(chan, AIRSPEED_AUTOCAL)) {
+            continue;
+        }
+        mavlink_msg_airspeed_autocal_send(
+            chan,
+            vground.x,
+            vground.y,
+            vground.z,
+            get_differential_pressure(primary),
+            state[primary].EAS2TAS,
+            param[primary].ratio.get(),
+            state[primary].calibration.state.x,
+            state[primary].calibration.state.y,
+            state[primary].calibration.state.z,
+            state[primary].calibration.P.a.x,
+            state[primary].calibration.P.b.y,
+            state[primary].calibration.P.c.z);
+    }
 }