[BARO/MAG] Move MSP sensor payload structures to a separate file; Implement MSP-based BARO and MAG

src/main/drivers/barometer/barometer_msp.c

@@ -0,0 +1,102 @@
+/*
+ * 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 "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/barometer/barometer.h"
+#include "drivers/barometer/barometer_msp.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_BARO_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
+
+static int32_t mspBaroPressure;
+static int32_t mspBaroTemperature;
+static timeMs_t mspBaroLastUpdateMs;
+
+static bool mspBaroStartGet(baroDev_t * baro)
+{
+    UNUSED(baro);
+    return true;
+}
+
+static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature)
+{
+    UNUSED(baro);
+
+    if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) {
+        return false;
+    }
+
+    if (pressure)
+        *pressure = mspBaroPressure;
+
+    if (temperature)
+        *temperature = mspBaroTemperature;
+
+    return true;
+}
+
+void mspBaroReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorBaroDataMessage_t * pkt = (const mspSensorBaroDataMessage_t *)bufferPtr;
+
+    mspBaroPressure = pkt->pressurePa;
+    mspBaroTemperature = pkt->temp;
+
+    mspBaroLastUpdateMs = millis();
+}
+
+bool mspBaroDetect(baroDev_t *baro)
+{
+    mspBaroPressure = 101325;    // pressure in Pa (0m MSL)
+    mspBaroTemperature = 2500;   // temperature in 0.01 C = 25 deg
+
+    // these are dummy as temperature is measured as part of pressure
+    baro->ut_delay = 10000;
+    baro->get_ut = mspBaroStartGet;
+    baro->start_ut = mspBaroStartGet;
+
+    // only _up part is executed, and gets both temperature and pressure
+    baro->up_delay = 10000;
+    baro->start_up = mspBaroStartGet;
+    baro->get_up = mspBaroStartGet;
+
+    baro->calculate = mspBaroCalculate;
+
+    return true;
+}
+
+#endif

src/main/drivers/barometer/barometer_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 baroDev_s;
+bool mspBaroDetect(struct baroDev_s *baro);
+void mspBaroReceiveNewData(uint8_t * bufferPtr);

src/main/drivers/compass/compass_msp.c

@@ -0,0 +1,92 @@
+/*
+ * 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_MAG_MSP)
+
+#include "build/build_config.h"
+
+#include "common/axis.h"
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/compass/compass.h"
+#include "drivers/compass/compass_msp.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_MAG_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
+
+static int16_t mspMagData[XYZ_AXIS_COUNT];
+static timeMs_t mspMagLastUpdateMs;
+
+static bool mspMagInit(magDev_t *magDev)
+{
+    UNUSED(magDev);
+    mspMagData[X] = 0;
+    mspMagData[Y] = 0;
+    mspMagData[Z] = 0;
+    mspMagLastUpdateMs = 0;
+    return true;
+}
+
+void mspMagReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;
+
+    mspMagData[X] = pkt->magX;
+    mspMagData[Y] = pkt->magY;
+    mspMagData[Z] = pkt->magZ;
+
+    mspMagLastUpdateMs = millis();
+}
+
+static bool mspMagRead(magDev_t *magDev)
+{
+    UNUSED(magDev);
+
+    if ((millis() - mspMagLastUpdateMs) > MSP_MAG_TIMEOUT_MS) {
+        return false;
+    }
+
+    magDev->magADCRaw[X] = mspMagData[X];
+    magDev->magADCRaw[Y] = mspMagData[Y];
+    magDev->magADCRaw[Z] = mspMagData[Z];
+
+    return true;
+}
+
+bool mspMagDetect(magDev_t *mag)
+{
+    mag->init = mspMagInit;
+    mag->read = mspMagRead;
+    return true;
+}
+
+#endif

src/main/drivers/compass/compass_msp.h

@@ -0,0 +1,28 @@
+/*
+ * 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
+
+bool mspMagDetect(struct magDev_s *mag);
+void mspMagReceiveNewData(uint8_t * bufferPtr);

src/main/fc/fc_msp.c

@@ -40,8 +40,10 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/accgyro/accgyro.h"
-#include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
+#include "drivers/compass/compass_msp.h"
+#include "drivers/barometer/barometer_msp.h"
+#include "drivers/bus_i2c.h"
 #include "drivers/display.h"
 #include "drivers/flash.h"
 #include "drivers/osd.h"
@@ -3222,9 +3224,9 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
             break;
 #endif
 
-#if defined(USE_COMPASS_MSP)
+#if defined(USE_MAG_MSP)
         case MSP2_SENSOR_COMPASS:
-            mspCompassReceiveNewData(sbufPtr(src));
+            mspMagReceiveNewData(sbufPtr(src));
             break;
 #endif
 

src/main/fc/settings.yaml

@@ -10,13 +10,13 @@ tables:
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
     enum: rangefinderType_e
   - name: mag_hardware
-    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"]
+    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
     enum: magSensor_e
   - name: opflow_hardware
     values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
     enum: opticalFlowSensor_e
   - name: baro_hardware
-    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "FAKE"]
+    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"]

src/main/io/gps.h

@@ -165,4 +165,4 @@ bool isGPSHealthy(void);
 bool isGPSHeadingValid(void);
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
-void mspGPSReceiveNewData(uint8_t * bufferPtr);
+void mspGPSReceiveNewData(const uint8_t * bufferPtr);

src/main/io/gps_msp.c

@@ -44,33 +44,7 @@
 #include "io/gps_private.h"
 #include "io/serial.h"
 
-#include "scheduler/protothreads.h"
-
-typedef struct __attribute__((packed)) {
-    uint8_t  instance;                  // sensor instance number to support multi-sensor setups
-    uint16_t gpsWeek;                   // GPS week, 0xFFFF if not available
-    uint32_t msTOW;
-    uint8_t  fixType;
-    uint8_t  satellitesInView;
-    uint16_t horizontalPosAccuracy;     // [cm]
-    uint16_t verticalPosAccuracy;       // [cm]
-    uint16_t horizontalVelAccuracy;     // [cm/s]
-    uint16_t hdop;
-    int32_t  longitude;
-    int32_t  latitude;
-    int32_t  mslAltitude;       // cm
-    int32_t  nedVelNorth;       // cm/s
-    int32_t  nedVelEast;
-    int32_t  nedVelDown;
-    int16_t  groundCourse;      // deg * 100
-    int16_t  trueYaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
-    uint16_t year;
-    uint8_t  month;
-    uint8_t  day;
-    uint8_t  hour;
-    uint8_t  min;
-    uint8_t  sec;
-} mspGpsDataMessage_t;
+#include "msp/msp_protocol_v2_sensor_msg.h"
 
 static bool newDataReady;
 
@@ -96,9 +70,9 @@ static uint8_t gpsMapFixType(uint8_t mspFixType)
     return GPS_NO_FIX;
 }
 
-void mspGPSReceiveNewData(uint8_t * bufferPtr)
+void mspGPSReceiveNewData(const uint8_t * bufferPtr)
 {
-    mspGpsDataMessage_t * pkt = (mspGpsDataMessage_t *)bufferPtr;
+    const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
 
     gpsSol.fixType   = gpsMapFixType(pkt->fixType);
     gpsSol.numSat    = pkt->satellitesInView;

src/main/io/opflow_msp.c

@@ -37,15 +37,13 @@
 #include "io/serial.h"
 
 #if defined(USE_OPFLOW_MSP)
+
 #include "drivers/opflow/opflow_virtual.h"
 #include "drivers/time.h"
 #include "io/opflow.h"
 
-typedef struct __attribute__((packed)) {
-    uint8_t quality;    // [0;255]
-    int32_t motionX;
-    int32_t motionY;
-} mspOpflowSensor_t;
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
 
 static bool hasNewData = false;
 static timeUs_t updatedTimeUs = 0;
@@ -76,7 +74,7 @@ static bool mspOpflowUpdate(opflowData_t * data)
 void mspOpflowReceiveNewData(uint8_t * bufferPtr)
 {
     const timeUs_t currentTimeUs = micros();
-    const mspOpflowSensor_t * pkt = (const mspOpflowSensor_t *)bufferPtr;
+    const mspSensorOpflowDataMessage_t * pkt = (const mspSensorOpflowDataMessage_t *)bufferPtr;
 
     sensorData.deltaTime = currentTimeUs - updatedTimeUs;
     sensorData.flowRateRaw[0] = pkt->motionX;

src/main/io/rangefinder_msp.c

@@ -37,14 +37,12 @@
 #include "io/serial.h"
 
 #if defined(USE_RANGEFINDER_MSP)
+
 #include "drivers/rangefinder/rangefinder_virtual.h"
 #include "drivers/time.h"
 #include "io/rangefinder.h"
+#include "msp/msp_protocol_v2_sensor_msg.h"
 
-typedef struct __attribute__((packed)) {
-    uint8_t quality;    // [0;255]
-    int32_t distanceMm; // Negative value for out of range
-} mspRangefinderSensor_t;
 
 static bool hasNewData = false;
 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
@@ -76,7 +74,7 @@ static int32_t mspRangefinderGetDistance(void)
 
 void mspRangefinderReceiveNewData(uint8_t * bufferPtr)
 {
-    const mspRangefinderSensor_t * pkt = (const mspRangefinderSensor_t *)bufferPtr;
+    const mspSensorRangefinderDataMessage_t * pkt = (const mspSensorRangefinderDataMessage_t *)bufferPtr;
 
     sensorData = pkt->distanceMm / 10;
     hasNewData = true;

src/main/msp/msp_protocol_v2_sensor_msg.h

@@ -0,0 +1,77 @@
+/*
+ * 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
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t motionX;
+    int32_t motionY;
+} mspSensorOpflowDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t distanceMm; // Negative value for out of range
+} mspSensorRangefinderDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;                  // sensor instance number to support multi-sensor setups
+    uint16_t gpsWeek;                   // GPS week, 0xFFFF if not available
+    uint32_t msTOW;
+    uint8_t  fixType;
+    uint8_t  satellitesInView;
+    uint16_t horizontalPosAccuracy;     // [cm]
+    uint16_t verticalPosAccuracy;       // [cm]
+    uint16_t horizontalVelAccuracy;     // [cm/s]
+    uint16_t hdop;
+    int32_t  longitude;
+    int32_t  latitude;
+    int32_t  mslAltitude;       // cm
+    int32_t  nedVelNorth;       // cm/s
+    int32_t  nedVelEast;
+    int32_t  nedVelDown;
+    uint16_t groundCourse;      // deg * 100, 0..36000
+    uint16_t trueYaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
+    uint16_t year;
+    uint8_t  month;
+    uint8_t  day;
+    uint8_t  hour;
+    uint8_t  min;
+    uint8_t  sec;
+} mspSensorGpsDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    float    pressurePa;
+    int16_t  temp; // centi-degrees C
+} mspSensorBaroDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    int16_t  magX; // mGauss, front
+    int16_t  magY; // mGauss, right
+    int16_t  magZ; // mGauss, down
+} mspSensorCompassDataMessage_t;

src/main/sensors/barometer.c

@@ -40,6 +40,7 @@
 #include "drivers/barometer/barometer_ms56xx.h"
 #include "drivers/barometer/barometer_spl06.h"
 #include "drivers/barometer/barometer_dps310.h"
+#include "drivers/barometer/barometer_msp.h"
 #include "drivers/time.h"
 
 #include "fc/runtime_config.h"
@@ -187,6 +188,19 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
         }
         FALLTHROUGH;
 
+    case BARO_MSP:
+#ifdef USE_BARO_MSP
+        if (mspBaroDetect(dev)) {
+            baroHardware = BARO_MSP;
+            break;
+        }
+#endif
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (baroHardwareToUse != BARO_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+
     case BARO_FAKE:
 #ifdef USE_FAKE_BARO
         if (fakeBaroDetect(dev)) {

src/main/sensors/barometer.h

@@ -32,7 +32,8 @@ typedef enum {
     BARO_SPL06  = 7,
     BARO_BMP388 = 8,
     BARO_DPS310 = 9,
-    BARO_FAKE   = 10,
+    BARO_MSP    = 10,
+    BARO_FAKE   = 11,
     BARO_MAX = BARO_FAKE
 } baroSensor_e;
 

src/main/sensors/compass.c

@@ -40,6 +40,7 @@
 #include "drivers/compass/compass_qmc5883l.h"
 #include "drivers/compass/compass_mpu9250.h"
 #include "drivers/compass/compass_lis3mdl.h"
+#include "drivers/compass/compass_msp.h"
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/time.h"
@@ -249,6 +250,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
         }
         FALLTHROUGH;
 
+    case MAG_MSP:
+#ifdef USE_MAG_MSP
+        if (mspMagDetect(dev)) {
+            magHardware = MAG_MSP;
+            break;
+        }
+#endif
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (magHardwareToUse != MAG_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+
     case MAG_FAKE:
 #ifdef USE_FAKE_MAG
         if (fakeMagDetect(dev)) {

src/main/sensors/compass.h

@@ -41,7 +41,8 @@ typedef enum {
     MAG_MPU9250 = 9,
     MAG_IST8308 = 10,
     MAG_LIS3MDL = 11,
-    MAG_FAKE = 12,
+    MAG_MSP = 12,
+    MAG_FAKE = 13,
     MAG_MAX = MAG_FAKE
 } magSensor_e;
 

src/main/target/common_post.h

@@ -35,6 +35,15 @@
 #define USE_CANVAS
 #endif
 
+// Enable MSP BARO & MAG drivers if BARO and MAG sensors are compiled in
+#if defined(USE_MAG)
+#define USE_MAG_MSP
+#endif
+
+#if defined(USE_BARO)
+#define USE_BARO_MSP
+#endif
+
 #ifdef USE_ESC_SENSOR
     #define USE_RPM_FILTER
 #endif