From 4f1f8aeb1ce83b825c537aba9edc85c8de4b1888 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 1 Dec 2024 14:18:15 +1100
Subject: [PATCH] AP_Airspeed: don't send AIRSPEED_AUTOCAL message when
 disabled

we were wasting bandwidth sending this all the time

also fixed to support calibration messages for 2nd airspeed sensor
---
 .../AP_Airspeed/Airspeed_Calibration.cpp      | 34 ++++++++++++-------
 1 file changed, 22 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
index 0a416d2150e1f..bea01f02fc239 100644
--- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp
+++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
@@ -171,23 +171,33 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
 #if HAL_GCS_ENABLED
 void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
 {
+    /*
+      the AIRSPEED_AUTOCAL message doesn't have an instance number
+      so we can only send it for one sensor at a time
+     */
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        if (!param[i].autocal && !calibration_enabled) {
+            // auto-calibration not enabled on this sensor
+            continue;
+        }
 #if AP_AIRSPEED_AUTOCAL_ENABLE
-    const mavlink_airspeed_autocal_t packet{
+        const mavlink_airspeed_autocal_t packet{
         vx: vground.x,
         vy: vground.y,
         vz: vground.z,
-        diff_pressure: get_differential_pressure(primary),
+        diff_pressure: get_differential_pressure(i),
         EAS2TAS: AP::ahrs().get_EAS2TAS(),
-        ratio: param[primary].ratio.get(),
-        state_x: state[primary].calibration.state.x,
-        state_y: state[primary].calibration.state.y,
-        state_z: state[primary].calibration.state.z,
-        Pax: state[primary].calibration.P.a.x,
-        Pby: state[primary].calibration.P.b.y,
-        Pcz: state[primary].calibration.P.c.z
-    };
-    gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
-                                  (const char *)&packet);
+        ratio: param[i].ratio.get(),
+        state_x: state[i].calibration.state.x,
+        state_y: state[i].calibration.state.y,
+        state_z: state[i].calibration.state.z,
+        Pax: state[i].calibration.P.a.x,
+        Pby: state[i].calibration.P.b.y,
+        Pcz: state[i].calibration.P.c.z
+        };
+        gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
+                                      (const char *)&packet);
+    }
 #endif // AP_AIRSPEED_AUTOCAL_ENABLE
 }
 #endif  // HAL_GCS_ENABLED