From 75333dcd2f68fae17657aaed4c7beb31f32efa1d Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Sat, 24 Oct 2020 10:52:29 +0200
Subject: [PATCH] AP_Periph: support output of MSP airspeed sensor

---
 Tools/AP_Periph/AP_Periph.h     |  2 ++
 Tools/AP_Periph/msp.cpp         | 32 ++++++++++++++++++++++++++++++++
 libraries/AP_MSP/msp_protocol.h |  1 +
 libraries/AP_MSP/msp_sensors.h  |  7 +++++++
 4 files changed, 42 insertions(+)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index ebf14bff76..9f07a931ac 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -78,6 +78,7 @@ public:
         uint32_t last_gps_ms;
         uint32_t last_baro_ms;
         uint32_t last_mag_ms;
+        uint32_t last_airspeed_ms;
     } msp;
     void msp_init(AP_HAL::UARTDriver *_uart);
     void msp_sensor_update(void);
@@ -85,6 +86,7 @@ public:
     void send_msp_GPS(void);
     void send_msp_compass(void);
     void send_msp_baro(void);
+    void send_msp_airspeed(void);
 #endif
     
 #ifdef HAL_PERIPH_ENABLE_ADSB
diff --git a/Tools/AP_Periph/msp.cpp b/Tools/AP_Periph/msp.cpp
index 6719a66f4c..6fe7e75599 100644
--- a/Tools/AP_Periph/msp.cpp
+++ b/Tools/AP_Periph/msp.cpp
@@ -48,6 +48,9 @@ void AP_Periph_FW::msp_sensor_update(void)
 #ifdef HAL_PERIPH_ENABLE_MAG
     send_msp_compass();
 #endif
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    send_msp_airspeed();
+#endif
 }
 
 
@@ -160,4 +163,33 @@ void AP_Periph_FW::send_msp_compass(void)
 }
 #endif // HAL_PERIPH_ENABLE_MAG
 
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+/*
+  send MSP airspeed packet
+ */
+void AP_Periph_FW::send_msp_airspeed(void)
+{
+    MSP::msp_airspeed_data_message_t p;
+
+    const uint32_t last_update_ms = airspeed.last_update_ms();
+    if (msp.last_airspeed_ms == last_update_ms) {
+        return;
+    }
+    msp.last_airspeed_ms = last_update_ms;
+
+    p.instance = 0;
+    p.time_ms = msp.last_airspeed_ms;
+    p.pressure = airspeed.get_corrected_pressure();
+    float temp;
+    if (!airspeed.get_temperature(temp)) {
+        p.temp = INT16_MIN; //invalid temperature
+    } else {
+        p.temp = temp * 100;
+    }
+
+    send_msp_packet(MSP2_SENSOR_AIRSPEED, &p, sizeof(p));
+}
+#endif // HAL_PERIPH_ENABLE_AIRSPEED
+
+
 #endif // HAL_PERIPH_ENABLE_MSP
diff --git a/libraries/AP_MSP/msp_protocol.h b/libraries/AP_MSP/msp_protocol.h
index c942b57a83..7b26549061 100644
--- a/libraries/AP_MSP/msp_protocol.h
+++ b/libraries/AP_MSP/msp_protocol.h
@@ -349,3 +349,4 @@
 #define MSP2_SENSOR_GPS             0x1F03
 #define MSP2_SENSOR_COMPASS         0x1F04
 #define MSP2_SENSOR_BAROMETER       0x1F05
+#define MSP2_SENSOR_AIRSPEED        0x1F06
diff --git a/libraries/AP_MSP/msp_sensors.h b/libraries/AP_MSP/msp_sensors.h
index e8d7f07b73..e3b6885bed 100644
--- a/libraries/AP_MSP/msp_sensors.h
+++ b/libraries/AP_MSP/msp_sensors.h
@@ -54,4 +54,11 @@ typedef struct PACKED {
     int16_t magY; // mGauss, right
     int16_t magZ; // mGauss, down
 } msp_compass_data_message_t;
+
+typedef struct PACKED {
+    uint8_t instance;
+    uint32_t time_ms;
+    float pressure;
+    int16_t temp; // centi-degrees C
+} msp_airspeed_data_message_t;
 }