MSP Range finder added (betaflight#13980)

* MSP Range Finder added (configured and tested on the MTF-01P Lidar) * MSP Range Finder added (configured and tested on the MTF-01P Lidar) * fix the license of the added files from INAV * Update src/main/drivers/rangefinder/rangefinder_virtual.c Co-authored-by: Mark Haslinghuis <[email protected]> * Update src/main/msp/msp.c Co-authored-by: Mark Haslinghuis <[email protected]> * Update src/main/io/rangefinder.h Co-authored-by: Mark Haslinghuis <[email protected]> * Update src/main/msp/msp.c Co-authored-by: Mark Haslinghuis <[email protected]> * refactored the code of INAV for the MSP rangefinder, to be extendable to other MSP rangefinder and more specific about the parameters of the supported ones * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <[email protected]> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <[email protected]> * refactored the code for readability * mt lidar msp address * us the delay MS from the dev directly * todo * MT device type datatype * refactored the code for readability * spacing * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * refactoring Co-authored-by: Petr Ledvina <[email protected]> * after the edits fix * rm idx * typo fixed * i think this shouldn't be here * spacing * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <[email protected]> * fix mt models ranges * remove mt01p model, it doesn't map * rm "MT01P", it doesn't support map * rm mt01p, it doesn't support msp * Update rangefinder_lidarmt.h * update mt lidar deadzone * Update rangefinder_lidarmt.c * Update rangefinder_lidarmt.c * rm unused variable mtfConnected --------- Co-authored-by: Mark Haslinghuis <[email protected]> Co-authored-by: Petr Ledvina <[email protected]>
HThuren · Nov 11, 2024 · b70e98e · b70e98e
1 parent 778f93f
commit b70e98e
Show file tree

Hide file tree

Showing 8 changed files with 177 additions and 2 deletions.
diff --git a/mk/source.mk b/mk/source.mk
@@ -235,6 +235,7 @@ COMMON_SRC = \
             drivers/light_ws2811strip.c \
             drivers/rangefinder/rangefinder_hcsr04.c \
             drivers/rangefinder/rangefinder_lidartf.c \
+            drivers/rangefinder/rangefinder_lidarmt.c \
             drivers/serial_escserial.c \
             drivers/vtx_common.c \
             drivers/vtx_table.c \

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -166,7 +166,7 @@ const char * const lookupTableMagHardware[] = {
 #endif
 #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
 const char * const lookupTableRangefinderHardware[] = {
-    "NONE", "HCSR04", "TFMINI", "TF02"
+    "NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P"
 };
 #endif
 

diff --git a/src/main/drivers/rangefinder/rangefinder_lidarmt.c b/src/main/drivers/rangefinder/rangefinder_lidarmt.c
@@ -0,0 +1,108 @@
+/*
+ * This file is part of Betaflight and INAV
+ *
+ * Betaflight and INAV are free software. You can
+ * redistribute this software and/or modify this software 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.
+ *
+ * Betaflight and INAV are distributed in the hope that
+ * they 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART)
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_RANGEFINDER_MT
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "drivers/rangefinder/rangefinder_lidarmt.h"
+#include "sensors/rangefinder.h"
+
+static bool hasNewData = false;
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+
+// Initialize the table with values for each rangefinder type
+static const MTRangefinderConfig rangefinderConfigs[] = {
+    { .deviceType = RANGEFINDER_MTF01,  .delayMs = 10, .maxRangeCm = 800  },
+    { .deviceType = RANGEFINDER_MTF02,  .delayMs = 20, .maxRangeCm = 250  },
+    { .deviceType = RANGEFINDER_MTF01P, .delayMs = 10, .maxRangeCm = 1200 },
+    { .deviceType = RANGEFINDER_MTF02P, .delayMs = 20, .maxRangeCm = 600  },
+};
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t distanceMm; // Negative value for out of range
+} mspSensorRangefinderLidarMtDataMessage_t;
+
+static void mtRangefinderInit(rangefinderDev_t * dev) {
+    UNUSED(dev);
+}
+
+static void mtRangefinderUpdate(rangefinderDev_t * dev) {
+    UNUSED(dev);
+}
+
+static int32_t mtRangefinderGetDistance(rangefinderDev_t * dev) {
+    UNUSED(dev);
+    if (hasNewData) {
+        hasNewData = false;
+        return (sensorData >= 0.0f) ? sensorData : RANGEFINDER_OUT_OF_RANGE;
+    } else {
+        return RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse) {   
+    const MTRangefinderConfig* deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse);
+    if (!deviceConf) {
+        return false;
+    }
+    dev->delayMs    = deviceConf->delayMs;
+    dev->maxRangeCm = deviceConf->maxRangeCm;
+
+    dev->detectionConeDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES;
+    dev->detectionConeExtendedDeciDegrees = RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES;
+
+    dev->init = &mtRangefinderInit;
+    dev->update = &mtRangefinderUpdate;
+    dev->read = &mtRangefinderGetDistance;
+
+    return true;
+}
+
+void mtRangefinderReceiveNewData(const uint8_t * bufferPtr) {   
+    const mspSensorRangefinderLidarMtDataMessage_t * pkt = (const mspSensorRangefinderLidarMtDataMessage_t *)bufferPtr;
+
+    sensorData = pkt->distanceMm / 10;
+    hasNewData = true;
+}
+
+const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse){
+    for (const MTRangefinderConfig* cfg =  rangefinderConfigs; cfg < ARRAYEND(rangefinderConfigs); cfg++) {
+        if (cfg->deviceType == mtRangefinderToUse) {
+            return cfg;
+        }
+    }
+    return NULL;
+}
+
+#endif // USE_RANGEFINDER_MT
diff --git a/src/main/drivers/rangefinder/rangefinder_lidarmt.h b/src/main/drivers/rangefinder/rangefinder_lidarmt.h
@@ -0,0 +1,36 @@
+/*
+ * This file is part of Betaflight and INAV
+ *
+ * Betaflight and INAV are free software. You can
+ * redistribute this software and/or modify this software 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.
+ *
+ * Betaflight and INAV are distributed in the hope that
+ * they 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/rangefinder/rangefinder.h"
+#include "sensors/rangefinder.h"
+
+#define RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES  900 
+
+typedef struct {
+    rangefinderType_e deviceType;
+    uint8_t delayMs;
+    uint16_t maxRangeCm;
+} MTRangefinderConfig;
+
+bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse);
+void mtRangefinderReceiveNewData(const uint8_t * bufferPtr);
+const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
@@ -71,6 +71,7 @@
 #include "drivers/usb_msc.h"
 #include "drivers/vtx_common.h"
 #include "drivers/vtx_table.h"
+#include "drivers/rangefinder/rangefinder_lidarmt.h"
 
 #include "fc/board_info.h"
 #include "fc/controlrate_profile.h"
@@ -3294,8 +3295,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 #else
         sbufReadU8(src);
 #endif
-        break;
 
+#ifdef USE_RANGEFINDER
+        rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src);
+#else
+        sbufReadU8(src);        // rangefinder hardware
+#endif
+        break;
 #ifdef USE_ACC
     case MSP_ACC_CALIBRATION:
         if (!ARMING_FLAG(ARMED))
@@ -3646,6 +3652,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         break;
 #endif
 
+#if defined(USE_RANGEFINDER_MT)
+    case MSP2_SENSOR_RANGEFINDER_LIDARMT:
+        mtRangefinderReceiveNewData(sbufPtr(src));
+        break;
+#endif
 #ifdef USE_GPS
     case MSP2_SENSOR_GPS:
         (void)sbufReadU8(src);              // instance

diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
@@ -23,3 +23,5 @@
 
 // Sensors
 #define MSP2_SENSOR_GPS                 0x1F03
+// TODO: implement new, extensible rangefinder protocol
+#define MSP2_SENSOR_RANGEFINDER_LIDARMT 0x1F01
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
@@ -41,6 +41,7 @@
 #include "drivers/rangefinder/rangefinder.h"
 #include "drivers/rangefinder/rangefinder_hcsr04.h"
 #include "drivers/rangefinder/rangefinder_lidartf.h"
+#include "drivers/rangefinder/rangefinder_lidarmt.h"
 #include "drivers/time.h"
 
 #include "fc/runtime_config.h"
@@ -125,6 +126,18 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
             }
 #endif
             break;
+
+#if defined(USE_RANGEFINDER_MT)
+        case RANGEFINDER_MTF01:
+        case RANGEFINDER_MTF02:
+        case RANGEFINDER_MTF01P:
+        case RANGEFINDER_MTF02P:
+            if (mtRangefinderDetect(dev, rangefinderHardwareToUse)) {
+                rangefinderHardware = rangefinderHardwareToUse;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(dev->delayMs));
+            }
+            break;
+#endif
 
         case RANGEFINDER_NONE:
             rangefinderHardware = RANGEFINDER_NONE;

diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
@@ -31,6 +31,10 @@ typedef enum {
     RANGEFINDER_HCSR04      = 1,
     RANGEFINDER_TFMINI      = 2,
     RANGEFINDER_TF02        = 3,
+    RANGEFINDER_MTF01       = 4,
+    RANGEFINDER_MTF02       = 5,
+    RANGEFINDER_MTF01P      = 6,
+    RANGEFINDER_MTF02P      = 7,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {