[MSP SENSOR] Add support for MSP Airspeed sensor

src/main/CMakeLists.txt

@@ -118,6 +118,8 @@ main_sources(COMMON_SRC
     drivers/barometer/barometer_ms56xx.h
     drivers/barometer/barometer_spl06.c
     drivers/barometer/barometer_spl06.h
+    drivers/barometer/barometer_msp.c
+    drivers/barometer/barometer_msp.h
 
     drivers/buf_writer.c
     drivers/buf_writer.h
@@ -148,6 +150,8 @@ main_sources(COMMON_SRC
     drivers/compass/compass_mpu9250.h
     drivers/compass/compass_qmc5883l.c
     drivers/compass/compass_qmc5883l.h
+    drivers/compass/compass_msp.c
+    drivers/compass/compass_msp.h
 
     drivers/display.c
     drivers/display.h
@@ -195,12 +199,14 @@ main_sources(COMMON_SRC
     drivers/osd.h
     drivers/persistent.c
     drivers/persistent.h
-    drivers/pitotmeter_adc.c
-    drivers/pitotmeter_adc.h
-    drivers/pitotmeter_ms4525.c
-    drivers/pitotmeter_ms4525.h
-    drivers/pitotmeter_virtual.c
-    drivers/pitotmeter_virtual.h
+    drivers/pitotmeter/pitotmeter_adc.c
+    drivers/pitotmeter/pitotmeter_adc.h
+    drivers/pitotmeter/pitotmeter_ms4525.c
+    drivers/pitotmeter/pitotmeter_ms4525.h
+    drivers/pitotmeter/pitotmeter_msp.c
+    drivers/pitotmeter/pitotmeter_msp.h
+    drivers/pitotmeter/pitotmeter_virtual.c
+    drivers/pitotmeter/pitotmeter_virtual.h
     drivers/pwm_esc_detect.c
     drivers/pwm_esc_detect.h
     drivers/pwm_mapping.c
@@ -499,6 +505,7 @@ main_sources(COMMON_SRC
     io/gps_ublox.c
     io/gps_nmea.c
     io/gps_naza.c
+    io/gps_msp.c
     io/gps_private.h
     io/ledstrip.c
     io/ledstrip.h

src/main/drivers/pitotmeter_adc.c → src/main/drivers/pitotmeter/pitotmeter_adc.c

@@ -24,9 +24,9 @@
 
 #include "common/utils.h"
 
-#include "pitotmeter.h"
-#include "pitotmeter_adc.h"
-#include "adc.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_adc.h"
+#include "drivers/adc.h"
 
 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
 

src/main/drivers/pitotmeter_ms4525.c → src/main/drivers/pitotmeter/pitotmeter_ms4525.c

@@ -24,8 +24,8 @@
 #include "common/utils.h"
 #include "common/maths.h"
 #include "drivers/bus_i2c.h"
-#include "drivers/pitotmeter.h"
 #include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
 
 // MS4525, Standard address 0x28
 #define MS4525_ADDR                 0x28

src/main/drivers/pitotmeter/pitotmeter_msp.c

@@ -0,0 +1,97 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#if defined(USE_BARO_MSP)
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_PITOT_TIMEOUT_MS      500     // Less than 2Hz updates is considered a failure
+
+static int32_t mspPitotPressure;
+static int32_t mspPitotTemperature;
+static timeMs_t mspPitotLastUpdateMs;
+
+static bool mspPitotStart(pitotDev_t *pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static bool mspPitotRead(pitotDev_t *pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static void mspPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature)
+{
+    UNUSED(pitot);
+
+    if (pressure) {
+        *pressure = mspPitotPressure;
+    }
+
+    if (temperature) {
+        *temperature = (mspPitotTemperature - 27315) / 100.0f;  // Pitot expects temp in Kelvin
+    }
+}
+
+void mspPitotmeterReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorAirspeedDataMessage_t * pkt = (const mspSensorAirspeedDataMessage_t *)bufferPtr;
+
+    mspPitotPressure = pkt->diffPressurePa;
+    mspPitotTemperature = pkt->temp;
+    mspPitotLastUpdateMs = millis();
+}
+
+bool mspPitotmeterDetect(pitotDev_t *pitot)
+{
+    mspPitotPressure = 0;
+    mspPitotTemperature = 27315;    // 0 deg/c
+
+    pitot->delay = 10000;
+    pitot->start = mspPitotStart;
+    pitot->get = mspPitotRead;
+    pitot->calculate = mspPitotCalculate;
+
+    return true;
+}
+
+#endif

src/main/drivers/pitotmeter/pitotmeter_msp.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+struct pitotDev_s;
+bool mspPitotmeterDetect(struct pitotDev_s *pitot);
+void mspPitotmeterReceiveNewData(uint8_t * bufferPtr);

src/main/drivers/pitotmeter_virtual.c → src/main/drivers/pitotmeter/pitotmeter_virtual.c

@@ -42,8 +42,8 @@
 
 #include "sensors/pitotmeter.h"
 
-#include "pitotmeter.h"
-#include "pitotmeter_virtual.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_virtual.h"
 
 #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) 
 

src/main/fc/fc_msp.c

@@ -43,6 +43,7 @@
 #include "drivers/compass/compass.h"
 #include "drivers/compass/compass_msp.h"
 #include "drivers/barometer/barometer_msp.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/display.h"
 #include "drivers/flash.h"
@@ -3235,6 +3236,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
             mspBaroReceiveNewData(sbufPtr(src));
             break;
 #endif
+
+#if defined(USE_PITOT_MSP)
+        case MSP2_SENSOR_AIRSPEED:
+            mspPitotmeterReceiveNewData(sbufPtr(src));
+            break;
+#endif
     }
 
     return MSP_RESULT_NO_REPLY;

src/main/fc/settings.yaml

@@ -19,7 +19,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]

src/main/msp/msp_protocol_v2_sensor.h

@@ -21,4 +21,5 @@
 #define MSP2_SENSOR_OPTIC_FLOW      0x1F02
 #define MSP2_SENSOR_GPS             0x1F03
 #define MSP2_SENSOR_COMPASS         0x1F04
-#define MSP2_SENSOR_BAROMETER       0x1F05
+#define MSP2_SENSOR_BAROMETER       0x1F05
+#define MSP2_SENSOR_AIRSPEED        0x1F06

src/main/msp/msp_protocol_v2_sensor_msg.h

@@ -68,6 +68,13 @@ typedef struct __attribute__((packed)) {
     int16_t  temp; // centi-degrees C
 } mspSensorBaroDataMessage_t;
 
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    float    diffPressurePa;
+    int16_t  temp;              // centi-degrees C
+} mspSensorAirspeedDataMessage_t;
+
 typedef struct __attribute__((packed)) {
     uint8_t  instance;
     uint32_t timeMs;

src/main/sensors/pitotmeter.c

@@ -29,10 +29,11 @@
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
-#include "drivers/pitotmeter.h"
-#include "drivers/pitotmeter_ms4525.h"
-#include "drivers/pitotmeter_adc.h"
-#include "drivers/pitotmeter_virtual.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_ms4525.h"
+#include "drivers/pitotmeter/pitotmeter_adc.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
+#include "drivers/pitotmeter/pitotmeter_virtual.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -108,6 +109,20 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             }
             FALLTHROUGH;
 
+        case PITOT_MSP:
+#ifdef USE_PITOT_MSP
+            // Skip autodetection for MSP baro, only allow manual config
+            if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) {
+                pitotHardware = PITOT_MSP;
+                break;
+            }
+#endif
+            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+            if (pitotHardwareToUse != PITOT_AUTODETECT) {
+                break;
+            }
+            FALLTHROUGH;
+
         case PITOT_FAKE:
 #ifdef USE_PITOT_FAKE
             if (fakePitotDetect(dev)) {

src/main/sensors/pitotmeter.h

@@ -21,7 +21,7 @@
 #include "common/filter.h"
 #include "common/calibration.h"
 
-#include "drivers/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter.h"
 
 typedef enum {
     PITOT_NONE = 0,
@@ -30,6 +30,7 @@ typedef enum {
     PITOT_ADC = 3,
     PITOT_VIRTUAL = 4,
     PITOT_FAKE = 5,
+    PITOT_MSP = 6,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE

src/main/target/common.h

@@ -94,8 +94,10 @@
 #define USE_OPFLOW_CXOF
 #define USE_OPFLOW_MSP
 
+// Allow default airspeed sensors
 #define USE_PITOT
 #define USE_PITOT_MS4525
+#define USE_PITOT_MSP
 
 #define USE_1WIRE
 #define USE_1WIRE_DS2482