From 953f542ec5491fda3ee3158cb41d0291c9e689d9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Oct 2022 19:36:02 +1100
Subject: [PATCH] Plane: send HYGROMETER_SENSOR data if available

---
 ArduPlane/GCS_Mavlink.cpp | 50 +++++++++++++++++++++++++++++++++++++++
 ArduPlane/GCS_Mavlink.h   |  6 +++++
 2 files changed, 56 insertions(+)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 97ae814ee7..bf2dae5a3c 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -2,6 +2,7 @@
 
 #include "Plane.h"
 #include <AP_RPM/AP_RPM_config.h>
+#include <AP_Airspeed/AP_Airspeed_config.h>
 
 MAV_TYPE GCS_Plane::frame_type() const
 {
@@ -417,12 +418,58 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
     case MSG_LANDING:
         plane.landing.send_landing_message(chan);
         break;
+
+    case MSG_HYGROMETER:
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+        CHECK_PAYLOAD_SIZE(HYGROMETER_SENSOR);
+        send_hygrometer();
+#endif
+        break;
+
     default:
         return GCS_MAVLINK::try_send_message(id);
     }
     return true;
 }
 
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+void GCS_MAVLINK_Plane::send_hygrometer()
+{
+    if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
+        return;
+    }
+
+    const auto *airspeed = AP::airspeed();
+    if (airspeed == nullptr) {
+        return;
+    } 
+    const uint32_t now = AP_HAL::millis();
+
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        uint8_t idx = (i+last_hygrometer_send_idx+1) % AIRSPEED_MAX_SENSORS;
+        float temperature, humidity;
+        uint32_t last_sample_ms;
+        if (!airspeed->get_hygrometer(idx, last_sample_ms, temperature, humidity)) {
+            continue;
+        }
+        if (now - last_sample_ms > 2000) {
+            // not updating, stop sending
+            continue;
+        }
+        if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
+            return;
+        }
+
+        mavlink_msg_hygrometer_sensor_send(
+            chan,
+            idx,
+            int16_t(temperature*100),
+            uint16_t(humidity*100));
+        last_hygrometer_send_idx = idx;
+    }
+}
+#endif // AP_AIRSPEED_HYGROMETER_ENABLE
+
 
 /*
   default stream rates to 1Hz
@@ -576,6 +623,9 @@ static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_LANDING,
     MSG_ESC_TELEMETRY,
     MSG_EFI_STATUS,
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+    MSG_HYGROMETER,
+#endif
 };
 static const ap_message STREAM_EXTRA2_msgs[] = {
     MSG_VFR_HUD
diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
index 8518baccf5..4b7304bb92 100644
--- a/ArduPlane/GCS_Mavlink.h
+++ b/ArduPlane/GCS_Mavlink.h
@@ -2,6 +2,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Airspeed/AP_Airspeed_config.h>
 
 class GCS_MAVLINK_Plane : public GCS_MAVLINK
 {
@@ -71,6 +72,11 @@ private:
     uint8_t high_latency_wind_direction() const override;
 #endif // HAL_HIGH_LATENCY2_ENABLED
 
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+    void send_hygrometer();
+    uint8_t last_hygrometer_send_idx;
+#endif
+
     MAV_VTOL_STATE vtol_state() const override;
     MAV_LANDED_STATE landed_state() const override;