From ae2a310b9cba9b9d555eb45c7dcaf9fa52d7e39d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 16 Nov 2020 21:35:15 +1100
Subject: [PATCH] AP_MSP: added airspeed support

---
 libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 16 ++++++++++++++++
 libraries/AP_MSP/AP_MSP_Telem_Backend.h   |  1 +
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
index 9e8f744fdf..518e331882 100644
--- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
+++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
@@ -15,6 +15,7 @@
 
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_BLHeli/AP_BLHeli.h>
 #include <RC_Channel/RC_Channel.h>
@@ -492,6 +493,11 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_m
         msp_handle_baro(*pkt);
     }
     break;
+    case MSP2_SENSOR_AIRSPEED: {
+        const MSP::msp_airspeed_data_message_t *pkt = (const MSP::msp_airspeed_data_message_t *)src->ptr;
+        msp_handle_airspeed(*pkt);
+    }
+    break;
     }
 
     return MSP_RESULT_NO_REPLY;
@@ -540,6 +546,16 @@ void AP_MSP_Telem_Backend::msp_handle_baro(const MSP::msp_baro_data_message_t &p
 #endif
 }
 
+void AP_MSP_Telem_Backend::msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt)
+{
+#if HAL_MSP_AIRSPEED_ENABLED
+    auto *airspeed = AP::airspeed();
+    if (airspeed) {
+        airspeed->handle_msp(pkt);
+    }
+#endif
+}
+
 MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst)
 {
 #if OSD_ENABLED
diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.h b/libraries/AP_MSP/AP_MSP_Telem_Backend.h
index 771eef76ee..0dfdfdd4ed 100644
--- a/libraries/AP_MSP/AP_MSP_Telem_Backend.h
+++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.h
@@ -164,6 +164,7 @@ protected:
     void msp_handle_gps(const MSP::msp_gps_data_message_t &pkt);
     void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt);
     void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt);
+    void msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt);
 
     // implementation specific helpers
     // custom masks are needed for vendor specific settings