AP_Airspeed: added MSP backend

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -35,9 +35,12 @@
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 #include "AP_Airspeed_UAVCAN.h"
 #endif
-#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
+#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
 #include "AP_Airspeed_NMEA.h"
 #endif
+#if HAL_MSP_AIRSPEED_ENABLED
+#include "AP_Airspeed_MSP.h"
+#endif
 extern const AP_HAL::HAL &hal;
 
 #ifdef HAL_AIRSPEED_TYPE_DEFAULT
@@ -80,7 +83,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),
 
@@ -187,7 +190,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second Airspeed type
     // @Description: Type of 2nd airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP
     // @User: Standard
     AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE),
 
@@ -274,7 +277,7 @@ AP_Airspeed::AP_Airspeed()
 }
 
 void AP_Airspeed::init()
-{
+{   
     if (sensor[0] != nullptr) {
         // already initialised
         return;
@@ -361,10 +364,14 @@ void AP_Airspeed::init()
         case TYPE_NMEA_WATER:
 #if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
             sensor[i] = new AP_Airspeed_NMEA(*this, i);
+#endif
+            break;
+        case TYPE_MSP:
+#if HAL_MSP_AIRSPEED_ENABLED
+            sensor[i] = new AP_Airspeed_MSP(*this, i, 0);
 #endif
             break;
         }
-
         if (sensor[i] && !sensor[i]->init()) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u init failed", i + 1);
             delete sensor[i];
@@ -564,6 +571,25 @@ void AP_Airspeed::update(bool log)
 #endif
 }
 
+#if HAL_MSP_AIRSPEED_ENABLED
+/*
+  handle MSP airspeed data
+ */
+void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
+{
+    
+    if (pkt.instance > 1) {
+        return; //supporting 2 airspeed sensors at most
+    }
+
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        if (sensor[i]) {
+            sensor[i]->handle_msp(pkt);
+        }
+    }
+}
+#endif 
+
 void AP_Airspeed::Log_Airspeed()
 {
     const uint64_t now = AP_HAL::micros64();

libraries/AP_Airspeed/AP_Airspeed.h

@@ -5,6 +5,7 @@
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_MSP/msp.h>
 
 class AP_Airspeed_Backend;
 
@@ -16,6 +17,9 @@ class AP_Airspeed_Backend;
 #define AP_AIRSPEED_AUTOCAL_ENABLE !defined(HAL_BUILD_AP_PERIPH)
 #endif
 
+#ifndef HAL_MSP_AIRSPEED_ENABLED
+#define HAL_MSP_AIRSPEED_ENABLED HAL_MSP_SENSORS_ENABLED
+#endif
 class Airspeed_Calibration {
 public:
     friend class AP_Airspeed;
@@ -162,6 +166,7 @@ public:
         TYPE_I2C_DLVR_30IN=11,
         TYPE_I2C_DLVR_60IN=12,
         TYPE_NMEA_WATER=13,
+        TYPE_MSP=14,
     };
 
     // get current primary sensor
@@ -179,6 +184,10 @@ public:
     float get_corrected_pressure(void) const {
         return get_corrected_pressure(primary);
     }
+
+#if HAL_MSP_AIRSPEED_ENABLED
+    void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
+#endif
     
 private:
     static AP_Airspeed *_singleton;

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -42,6 +42,10 @@ public:
     // return airspeed in m/s if available
     virtual bool get_airspeed(float& airspeed) {return false;}
 
+#if HAL_MSP_AIRSPEED_ENABLED
+    virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {}
+#endif 
+
 protected:
     int8_t get_pin(void) const;
     float get_psi_range(void) const;

libraries/AP_Airspeed/AP_Airspeed_MSP.cpp

@@ -0,0 +1,63 @@
+#include "AP_Airspeed_MSP.h"
+
+#if HAL_MSP_AIRSPEED_ENABLED
+
+AP_Airspeed_MSP::AP_Airspeed_MSP(AP_Airspeed &_frontend, uint8_t _instance, uint8_t _msp_instance) :
+    AP_Airspeed_Backend(_frontend, _instance),
+    msp_instance(_msp_instance)
+{
+}
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_MSP::get_differential_pressure(float &pressure)
+{
+    WITH_SEMAPHORE(sem);
+    if (press_count == 0) {
+        return false;
+    }
+    pressure = sum_pressure/press_count;
+    press_count = 0;
+    sum_pressure = 0;
+    return true;
+}
+
+// get last temperature
+bool AP_Airspeed_MSP::get_temperature(float &temperature)
+{
+    WITH_SEMAPHORE(sem);
+    if (temp_count == 0) {
+        return false;
+    }
+    temperature = sum_temp/temp_count;
+    temp_count = 0;
+    sum_temp = 0;
+    return true;
+}
+
+void AP_Airspeed_MSP::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
+{
+    if (pkt.instance != msp_instance) {
+        // not for us
+        return;
+    }
+    WITH_SEMAPHORE(sem);
+
+    sum_pressure += pkt.pressure;
+    press_count++;
+    if (press_count > 100) {
+        // prevent overflow
+        sum_pressure /= 2;
+        press_count /= 2;
+    }
+
+    sum_temp += pkt.temp*0.01;
+    temp_count++;
+    if (temp_count > 100) {
+        // prevent overflow
+        sum_temp /= 2;
+        temp_count /= 2;
+    }
+    
+}
+
+#endif // HAL_MSP_AIRSPEED_ENABLED

libraries/AP_Airspeed/AP_Airspeed_MSP.h

@@ -0,0 +1,35 @@
+/*
+  MSP airspeed backend
+ */
+#pragma once
+
+#include "AP_Airspeed_Backend.h"
+
+#if HAL_MSP_AIRSPEED_ENABLED
+
+class AP_Airspeed_MSP : public AP_Airspeed_Backend
+{
+public:
+    AP_Airspeed_MSP(AP_Airspeed &airspeed, uint8_t instance, uint8_t msp_instance);
+
+    bool init(void) override {
+        return true;
+    }
+
+    void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) override;
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure) override;
+
+    // temperature not available via analog backend
+    bool get_temperature(float &temperature) override;
+    
+private:
+    const uint8_t msp_instance;
+    float sum_pressure;
+    uint8_t press_count;
+    float sum_temp;
+    uint8_t temp_count;
+};
+
+#endif // HAL_MSP_AIRSPEED_ENABLED