opticalflow mt sensor added (#14038)

* opticalflow mt sensor added * set the processedFlow to raw when no rotation * use sin and cos approximations Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * refactoring * not needed anymore * update * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * update * rm prototype * clean up * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * update flip_y * Update src/main/drivers/opticalflow/opticalflow.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/rangefinder/rangefinder_lidarmt.h Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/opticalflow.c Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * rm casting * update op rotation --------- Co-authored-by: Petr Ledvina <ledvinap@gmail.com> Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
2025-07-12 19:10:32 +03:00 · 2024-12-20 19:47:36 +01:00 · 2024-12-20 19:47:36 +01:00 · d244239f39
commit d244239f39
parent 2f21754a69
21 changed files with 490 additions and 21 deletions
--- a/mk/source.mk
+++ b/mk/source.mk
@ -252,6 +252,7 @@ COMMON_SRC = \
            osd/osd_warnings.c \
            sensors/barometer.c \
            sensors/rangefinder.c \
+            sensors/opticalflow.c \
            telemetry/telemetry.c \
            telemetry/crsf.c \
            telemetry/ghst.c \
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@ -55,6 +55,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
    [DEBUG_FPORT] = "FPORT",
    [DEBUG_RANGEFINDER] = "RANGEFINDER",
    [DEBUG_RANGEFINDER_QUALITY] = "RANGEFINDER_QUALITY",
+    [DEBUG_OPTICALFLOW] = "OPTICALFLOW",
    [DEBUG_LIDAR_TF] = "LIDAR_TF",
    [DEBUG_ADC_INTERNAL] = "ADC_INTERNAL",
    [DEBUG_RUNAWAY_TAKEOFF] = "RUNAWAY_TAKEOFF",
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@ -57,6 +57,7 @@ typedef enum {
    DEBUG_FPORT,
    DEBUG_RANGEFINDER,
    DEBUG_RANGEFINDER_QUALITY,
+    DEBUG_OPTICALFLOW,
    DEBUG_LIDAR_TF,
    DEBUG_ADC_INTERNAL,
    DEBUG_RUNAWAY_TAKEOFF,
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@ -239,6 +239,7 @@ static const char * const featureNames[] = {
    _R(FEATURE_SOFTSERIAL, "SOFTSERIAL"),
    _R(FEATURE_GPS, "GPS"),
    _R(FEATURE_RANGEFINDER, "RANGEFINDER"),
+    _R(FEATURE_OPTICALFLOW, "OPTICALFLOW"),
    _R(FEATURE_TELEMETRY, "TELEMETRY"),
    _R(FEATURE_3D, "3D"),
    _R(FEATURE_RX_PARALLEL_PWM, "RX_PARALLEL_PWM"),
@ -272,7 +273,7 @@ static const char *const sensorTypeNames[] = {
 #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER)

 static const char * const *sensorHardwareNames[] = {
-    lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware
+    lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware, lookupTableOpticalflowHardware
 };
 #endif // USE_SENSOR_NAMES

--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@ -129,6 +129,7 @@
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opticalflow.h"

 #include "scheduler/scheduler.h"

@ -170,6 +171,11 @@ const char * const lookupTableRangefinderHardware[] = {
    "NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P"
 };
 #endif
+#if defined(USE_SENSOR_NAMES) || defined(USE_OPTICALFLOW)
+const char * const lookupTableOpticalflowHardware[] = {
+    "NONE", "MT"
+};
+#endif

 const char * const lookupTableOffOn[] = {
    "OFF", "ON"
@ -613,6 +619,9 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_RANGEFINDER
    LOOKUP_TABLE_ENTRY(lookupTableRangefinderHardware),
 #endif
+#ifdef USE_OPTICALFLOW
+    LOOKUP_TABLE_ENTRY(lookupTableOpticalflowHardware),
+#endif
 #ifdef USE_GYRO_OVERFLOW_CHECK
    LOOKUP_TABLE_ENTRY(lookupTableGyroOverflowCheck),
 #endif
@ -1710,6 +1719,14 @@ const clivalue_t valueTable[] = {
    { "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
 #endif

+// PG_OPTICALFLOW_CONFIG
+#ifdef USE_OPTICALFLOW
+    { "opticalflow_hardware",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OPTICALFLOW_HARDWARE }, PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, opticalflow_hardware) },
+    { "opticalflow_rotation",     VAR_INT16  | MASTER_VALUE ,              .config.minmaxUnsigned = {0, 359},               PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, rotation) },
+    { "opticalflow_lpf",          VAR_UINT16 | MASTER_VALUE ,              .config.minmaxUnsigned = {0, 10000},             PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, flow_lpf) },
+    { "opticalflow_flip_x",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },               PG_OPTICALFLOW_CONFIG, offsetof(opticalflowConfig_t, flip_x) },
+#endif
+
 // PG_PINIO_CONFIG
 #ifdef USE_PINIO
    { "pinio_config", VAR_UINT8 | HARDWARE_VALUE | MODE_ARRAY, .config.array.length = PINIO_COUNT, PG_PINIO_CONFIG, offsetof(pinioConfig_t, config) },
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@ -82,6 +82,9 @@ typedef enum {
 #ifdef USE_RANGEFINDER
    TABLE_RANGEFINDER_HARDWARE,
 #endif
+#ifdef USE_OPTICALFLOW
+    TABLE_OPTICALFLOW_HARDWARE,
+#endif
 #ifdef USE_GYRO_OVERFLOW_CHECK
    TABLE_GYRO_OVERFLOW_CHECK,
 #endif
@ -260,6 +263,8 @@ extern const char * const lookupTableMagHardware[];

 extern const char * const lookupTableRangefinderHardware[];

+extern const char * const lookupTableOpticalflowHardware[];
+
 extern const char * const lookupTableLedstripColors[];

 extern const char * const lookupTableRescueAltitudeMode[];
--- a/src/main/config/feature.c
+++ b/src/main/config/feature.c
@ -57,6 +57,9 @@ uint32_t featuresSupportedByBuild =
 #ifdef USE_RANGEFINDER
    | FEATURE_RANGEFINDER
 #endif
+#ifdef USE_OPTICALFLOW
+    | FEATURE_OPTICALFLOW
+#endif
 #ifdef USE_TELEMETRY
    | FEATURE_TELEMETRY
 #endif
--- a/src/main/config/feature.h
+++ b/src/main/config/feature.h
@ -50,6 +50,7 @@ typedef enum {
    FEATURE_SERVO_TILT = 1 << 5,
    FEATURE_SOFTSERIAL = 1 << 6,
    FEATURE_GPS = 1 << 7,
+    FEATURE_OPTICALFLOW = 1 << 8,
    FEATURE_RANGEFINDER = 1 << 9,
    FEATURE_TELEMETRY = 1 << 10,
    FEATURE_3D = 1 << 12,
--- a/src/main/drivers/opticalflow/opticalflow.h
+++ b/src/main/drivers/opticalflow/opticalflow.h
@ -0,0 +1,52 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "common/vector.h"
+
+#define OPTICALFLOW_OUT_OF_RANGE        -1
+#define OPTICALFLOW_HARDWARE_FAILURE    -2
+#define OPTICALFLOW_NO_NEW_DATA         -3
+
+#define OPTICALFLOW_HARDWARE_TIMEOUT_US  100000     // 100ms
+
+typedef struct opticalflowData_s {
+    uint32_t timeStampUs;
+    int16_t quality;
+    vector2_t flowRate;
+} opticalflowData_t;
+
+struct opticalflowDev_s;
+typedef void opflowOpInitFunc(struct opticalflowDev_s * dev);
+typedef void opflowOpUpdateFunc(struct opticalflowDev_s * dev);
+typedef void opflowOpReadFunc(struct opticalflowDev_s * dev, opticalflowData_t * result);
+
+typedef struct opticalflowDev_s {
+    unsigned delayMs;
+    int16_t minRangeCm;
+    uint8_t minQualityThreshold;
+    
+    opflowOpInitFunc  *init;
+    opflowOpUpdateFunc *update;
+    opflowOpReadFunc  *read;
+} opticalflowDev_t;
--- a/src/main/drivers/rangefinder/rangefinder_lidarmt.c
+++ b/src/main/drivers/rangefinder/rangefinder_lidarmt.c
@ -24,7 +24,6 @@

 #include <stdbool.h>
 #include <stdint.h>
-#include <string.h>

 #include "platform.h"

@ -34,11 +33,21 @@

 #include "common/utils.h"

+#include "drivers/time.h"
+
 #include "drivers/rangefinder/rangefinder_lidarmt.h"
 #include "sensors/rangefinder.h"

-static bool hasNewData = false;
-static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+#include "drivers/opticalflow/opticalflow.h"
+
+#define MT_OPTICALFLOW_MIN_RANGE 80  // mm
+#define MT_OPFLOW_MIN_QUALITY_THRESHOLD 30
+
+static opticalflowData_t opticalflowSensorData = {0};
+
+static bool hasRFNewData = false;
+static mtRangefinderData_t rfSensorData = {RANGEFINDER_NO_NEW_DATA, 0};
+static const MTRangefinderConfig * deviceConf = NULL;

 // Initialize the table with values for each rangefinder type
 static const MTRangefinderConfig rangefinderConfigs[] = {
@ -63,40 +72,41 @@ static void mtRangefinderUpdate(rangefinderDev_t * dev) {

 static int32_t mtRangefinderGetDistance(rangefinderDev_t * dev) {
    UNUSED(dev);
-    if (hasNewData) {
-        hasNewData = false;
-        return (sensorData >= 0.0f) ? sensorData : RANGEFINDER_OUT_OF_RANGE;
+    if (hasRFNewData) {
+        hasRFNewData = false;
+        return (rfSensorData.distanceMm >= 0) ? (rfSensorData.distanceMm / 10) : RANGEFINDER_OUT_OF_RANGE;
    } else {
        return RANGEFINDER_NO_NEW_DATA;
    }
 }

 bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse) {
-    const MTRangefinderConfig* deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse);
+    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->init   = &mtRangefinderInit;
    dev->update = &mtRangefinderUpdate;
-    dev->read = &mtRangefinderGetDistance;
-
+    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;
+    rfSensorData.distanceMm = pkt->distanceMm;
+    rfSensorData.timestampUs = micros();
+    hasRFNewData = true;
 }

-const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse){
+const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse) {
    for (const MTRangefinderConfig* cfg =  rangefinderConfigs; cfg < ARRAYEND(rangefinderConfigs); cfg++) {
        if (cfg->deviceType == mtRangefinderToUse) {
            return cfg;
@ -105,4 +115,60 @@ const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangef
    return NULL;
 }

+const mtRangefinderData_t * getMTRangefinderData(void) {
+    return &rfSensorData;
+}
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t motionX;
+    int32_t motionY;
+} mtOpticalflowDataMessage_t;
+
+static void mtOpticalflowInit(opticalflowDev_t * dev) {
+    UNUSED(dev);
+}
+
+static void mtOpticalflowUpdate(opticalflowDev_t * dev) {
+    UNUSED(dev);
+}
+
+static void mtOpticalflowGetData(opticalflowDev_t * dev, opticalflowData_t * result) {
+    UNUSED(dev);
+    *result = opticalflowSensorData;
+}
+
+bool mtOpticalflowDetect(opticalflowDev_t * dev, rangefinderType_e mtRangefinderToUse) {
+    deviceConf = getMTRangefinderDeviceConf(mtRangefinderToUse);
+    if (!deviceConf) {
+        return false;
+    }
+
+    dev->delayMs = deviceConf->delayMs;
+    dev->minRangeCm = MT_OPTICALFLOW_MIN_RANGE;
+    dev->minQualityThreshold = MT_OPFLOW_MIN_QUALITY_THRESHOLD;
+
+    dev->init   = &mtOpticalflowInit;
+    dev->update = &mtOpticalflowUpdate;
+    dev->read   = &mtOpticalflowGetData;
+    
+    return true;
+}
+
+void mtOpticalflowReceiveNewData(const uint8_t * bufferPtr) {
+    const mtRangefinderData_t * latestRangefinderData = getMTRangefinderData();
+    
+    const mtOpticalflowDataMessage_t * pkt = (const mtOpticalflowDataMessage_t *)bufferPtr;
+    
+    opticalflowSensorData.timeStampUs = micros();
+    opticalflowSensorData.flowRate.x  = (float)pkt->motionX / 1000.0f; 
+    opticalflowSensorData.flowRate.y  = (float)pkt->motionY / 1000.0f;
+    opticalflowSensorData.quality     = pkt->quality * 100 / 255;
+    
+    if (latestRangefinderData->distanceMm < MT_OPTICALFLOW_MIN_RANGE) {
+        opticalflowSensorData.quality = OPTICALFLOW_OUT_OF_RANGE;
+    } else if (cmp32(micros(), latestRangefinderData->timestampUs) > (5000 * deviceConf->delayMs)) {   // 5 updates missing
+        opticalflowSensorData.quality = OPTICALFLOW_HARDWARE_FAILURE;
+    }
+}
 #endif // USE_RANGEFINDER_MT
--- a/src/main/drivers/rangefinder/rangefinder_lidarmt.h
+++ b/src/main/drivers/rangefinder/rangefinder_lidarmt.h
@ -21,6 +21,7 @@
 #pragma once

 #include "drivers/rangefinder/rangefinder.h"
+#include "drivers/opticalflow/opticalflow.h"
 #include "sensors/rangefinder.h"

 #define RANGEFINDER_MT_DETECTION_CONE_DECIDEGREES  900
@ -31,6 +32,14 @@ typedef struct {
    uint16_t maxRangeCm;
 } MTRangefinderConfig;

+typedef struct {
+    uint32_t timestampUs;
+    int32_t distanceMm;
+} mtRangefinderData_t;
+
 bool mtRangefinderDetect(rangefinderDev_t * dev, rangefinderType_e mtRangefinderToUse);
 void mtRangefinderReceiveNewData(const uint8_t * bufferPtr);
 const MTRangefinderConfig* getMTRangefinderDeviceConf(rangefinderType_e mtRangefinderToUse);
+
+bool mtOpticalflowDetect(opticalflowDev_t * dev, rangefinderType_e mtRangefinderToUse);
+void mtOpticalflowReceiveNewData(const uint8_t * bufferPtr);
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@ -98,6 +98,7 @@
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opticalflow.h"

 #include "telemetry/telemetry.h"
 #include "telemetry/crsf.h"
@ -302,6 +303,20 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif

+#ifdef USE_OPTICALFLOW
+void taskUpdateOpticalflow(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (!sensors(SENSOR_OPTICALFLOW)) {
+        return;
+    }
+
+    opticalflowUpdate();
+    opticalflowProcess();
+}
+#endif
+
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)
 {
@ -449,7 +464,9 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #ifdef USE_RANGEFINDER
    [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST),
 #endif
-
+#ifdef USE_OPTICALFLOW
+    [TASK_OPTICALFLOW] = DEFINE_TASK("OPTICALFLOW", NULL, NULL, taskUpdateOpticalflow, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST),
+#endif
 #ifdef USE_CRSF_V3
    [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 #endif
@ -528,6 +545,12 @@ void tasksInit(void)
    }
 #endif

+#ifdef USE_OPTICALFLOW
+    if (sensors(SENSOR_OPTICALFLOW)) {
+        setTaskEnabled(TASK_OPTICALFLOW, featureIsEnabled(FEATURE_OPTICALFLOW));
+    }
+#endif
+
    setTaskEnabled(TASK_RX, true);

    setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@ -149,6 +149,7 @@
 #include "sensors/gyro.h"
 #include "sensors/gyro_init.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opticalflow.h"

 #include "telemetry/msp_shared.h"
 #include "telemetry/telemetry.h"
@ -2054,7 +2055,7 @@ case MSP_NAME:
        break;

    case MSP_SENSOR_CONFIG:
-        // use sensorIndex_e index: 0:GyroHardware, 1:AccHardware, 2:BaroHardware, 3:MagHardware, 4:RangefinderHardware
+        // use sensorIndex_e index: 0:GyroHardware, 1:AccHardware, 2:BaroHardware, 3:MagHardware, 4:RangefinderHardware 5:OpticalflowHardware
 #if defined(USE_ACC)
        sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
 #else
@ -2076,6 +2077,12 @@ case MSP_NAME:
 #else
        sbufWriteU8(dst, RANGEFINDER_NONE);
 #endif
+
+#ifdef USE_OPTICALFLOW
+        sbufWriteU8(dst, opticalflowConfig()->opticalflow_hardware);
+#else
+        sbufWriteU8(dst, OPTICALFLOW_NONE);
+#endif
        break;

    // Added in MSP API 1.46
@ -2107,6 +2114,11 @@ case MSP_NAME:
        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_RANGEFINDER]);
 #else
        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+#ifdef USE_OPTICAL_FLOW
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_OPTICALFLOW]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
 #endif
        break;

@ -3301,7 +3313,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 #ifdef USE_RANGEFINDER
        rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src);
 #else
-        sbufReadU8(src);        // rangefinder hardware
+        sbufReadU8(src);
+#endif
+
+#ifdef USE_OPTICALFLOW
+        opticalflowConfigMutable()->opticalflow_hardware = sbufReadU8(src);
+#else
+        sbufReadU8(src);
 #endif
        break;
 #ifdef USE_ACC
@ -3658,7 +3676,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
    case MSP2_SENSOR_RANGEFINDER_LIDARMT:
        mtRangefinderReceiveNewData(sbufPtr(src));
        break;
+
+    case MSP2_SENSOR_OPTICALFLOW_MT:
+        mtOpticalflowReceiveNewData(sbufPtr(src));
+        break;
 #endif
+
 #ifdef USE_GPS
    case MSP2_SENSOR_GPS:
        (void)sbufReadU8(src);              // instance
--- a/src/main/msp/msp_protocol_v2_betaflight.h
+++ b/src/main/msp/msp_protocol_v2_betaflight.h
@ -29,6 +29,7 @@
 #define MSP2_GET_LED_STRIP_CONFIG_VALUES    0x3008
 #define MSP2_SET_LED_STRIP_CONFIG_VALUES    0x3009
 #define MSP2_SENSOR_CONFIG_ACTIVE           0x300A
+#define MSP2_SENSOR_OPTICALFLOW             0x300B

 // MSP2_SET_TEXT and MSP2_GET_TEXT variable types
 #define MSP2TEXT_PILOT_NAME                      1
--- a/src/main/msp/msp_protocol_v2_common.h
+++ b/src/main/msp/msp_protocol_v2_common.h
@ -25,3 +25,4 @@
 #define MSP2_SENSOR_GPS                 0x1F03
 // TODO: implement new, extensible rangefinder protocol
 #define MSP2_SENSOR_RANGEFINDER_LIDARMT 0x1F01
+#define MSP2_SENSOR_OPTICALFLOW_MT      0x1F02
--- a/src/main/pg/pg_ids.h
+++ b/src/main/pg/pg_ids.h
@ -158,7 +158,9 @@
 #define PG_MSP_CONFIG               557
 //#define PG_SOFTSERIAL_PIN_CONFIG    558  // removed, merged into SERIAL_PIN_CONFIG
 #define PG_GIMBAL_TRACK_CONFIG      559
-#define PG_BETAFLIGHT_END           559
+#define PG_OPTICALFLOW_CONFIG       560
+#define PG_BETAFLIGHT_END           560
+

 // OSD configuration (subject to change)
 #define PG_OSD_FONT_CONFIG 2047
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@ -134,6 +134,9 @@ typedef enum {
 #ifdef USE_RANGEFINDER
    TASK_RANGEFINDER,
 #endif
+#ifdef USE_OPTICALFLOW
+    TASK_OPTICALFLOW,
+#endif
 #if defined(USE_BARO) || defined(USE_GPS)
    TASK_ALTITUDE,
 #endif
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@ -45,10 +45,11 @@
 #include "sensors/initialisation.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/opticalflow.h"

 // requestedSensors is not actually used
-uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
-uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
+uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, OPTICALFLOW_NONE};
+uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, OPTICALFLOW_NONE};

 void sensorsPreInit(void)
 {
@ -88,6 +89,10 @@ bool sensorsAutodetect(void)
    rangefinderInit();
 #endif

+#ifdef USE_OPTICALFLOW
+    opticalflowInit();
+#endif
+
 #ifdef USE_ADC_INTERNAL
    adcInternalInit();
 #endif
--- a/src/main/sensors/opticalflow.c
+++ b/src/main/sensors/opticalflow.c
@ -0,0 +1,196 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdint.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef USE_OPTICALFLOW
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/time.h"
+#include "common/utils.h"
+#include "common/filter.h"
+
+#include "config/config.h"
+#include "config/feature.h"
+
+#include "fc/runtime_config.h"
+
+#include "scheduler/scheduler.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "drivers/time.h"
+#include "drivers/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/rangefinder_lidarmt.h"
+
+#include "io/beeper.h"
+
+#include "sensors/sensors.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
+#include "sensors/opticalflow.h"
+
+#define OPTICALFLOW_CALIBRATION_DURATION_MS 30000
+#define RATE_SCALE_RESOLUTION (1000.0f)
+
+// static prototypes
+static void applySensorRotation(vector2_t * dst, vector2_t * src);
+static void applyLPF(vector2_t * flowRates);
+
+PG_REGISTER_WITH_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig, PG_OPTICALFLOW_CONFIG, 0);
+
+PG_RESET_TEMPLATE(opticalflowConfig_t, opticalflowConfig,
+    .opticalflow_hardware = OPTICALFLOW_NONE,
+    .rotation = 0,
+    .flow_lpf = 0,
+    .flip_x = 0
+);
+
+static opticalflow_t opticalflow;
+float cosRotAngle = 1.0f;
+float sinRotAngle = 0.0f;
+static pt2Filter_t xFlowLpf, yFlowLpf;
+
+// ======================================================================
+// =================== Opticalflow Main Functions =======================
+// ======================================================================
+static bool opticalflowDetect(opticalflowDev_t * dev, uint8_t opticalflowHardwareToUse) {
+    UNUSED(dev);
+
+    opticalflowType_e opticalflowHardware = OPTICALFLOW_NONE;
+    requestedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardwareToUse;
+
+    switch (opticalflowHardwareToUse) {
+        case OPTICALFLOW_MT:
+#ifdef USE_RANGEFINDER_MT
+            if (mtOpticalflowDetect(dev, rangefinderConfig()->rangefinder_hardware)) {
+                opticalflowHardware = OPTICALFLOW_MT;
+                rescheduleTask(TASK_OPTICALFLOW, TASK_PERIOD_MS(dev->delayMs));
+            }
+#endif
+            break;
+
+        case OPTICALFLOW_NONE:
+            opticalflowHardware = OPTICALFLOW_NONE;
+            break;
+    }
+
+    if (opticalflowHardware == OPTICALFLOW_NONE) {
+        sensorsClear(SENSOR_OPTICALFLOW);
+        return false;
+    }
+
+    detectedSensors[SENSOR_INDEX_OPTICALFLOW] = opticalflowHardware;
+    sensorsSet(SENSOR_OPTICALFLOW);
+    return true;    
+}
+
+bool opticalflowInit(void) {
+    if (!opticalflowDetect(&opticalflow.dev, opticalflowConfig()->opticalflow_hardware)) {
+        return false;
+    }
+
+    opticalflow.dev.init(&opticalflow.dev);
+    opticalflow.quality = OPTICALFLOW_NO_NEW_DATA;
+    opticalflow.rawFlowRates.x = 0;
+    opticalflow.rawFlowRates.y = 0;
+    opticalflow.processedFlowRates.x = 0;
+    opticalflow.processedFlowRates.y = 0;
+    opticalflow.timeStampUs = micros();
+
+    cosRotAngle = cosf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation));
+    sinRotAngle = sinf(DEGREES_TO_RADIANS(opticalflowConfig()->rotation));
+    //low pass filter
+    if (opticalflowConfig()->flow_lpf != 0) {
+        const float flowCutoffHz = (float)opticalflowConfig()->flow_lpf / 100.0f;
+        const float flowGain     = pt2FilterGain(flowCutoffHz, opticalflow.dev.delayMs / 1000.0f);
+
+        pt2FilterInit(&xFlowLpf, flowGain);
+        pt2FilterInit(&yFlowLpf, flowGain);
+    }
+    return true;
+}
+
+void opticalflowUpdate(void) {
+    if (opticalflow.dev.update) {
+        opticalflow.dev.update(&opticalflow.dev);
+    }
+}
+
+void opticalflowProcess(void) {
+    opticalflowData_t data = {0};
+    uint32_t deltaTimeUs = 0;
+    opticalflow.dev.read(&opticalflow.dev, &data);
+    
+    opticalflow.quality = data.quality;
+    deltaTimeUs = cmp32(data.timeStampUs, opticalflow.timeStampUs);
+    
+    if (deltaTimeUs != 0) { // New data
+        vector2_t raw = data.flowRate;
+        vector2_t processed;
+        
+        applySensorRotation(&processed, &raw);
+        applyLPF(&processed);
+        
+        opticalflow.rawFlowRates = raw;
+        opticalflow.processedFlowRates = processed;
+        opticalflow.timeStampUs  = data.timeStampUs;
+
+        // DEBUG SECTION
+        DEBUG_SET(DEBUG_OPTICALFLOW, 0, opticalflow.quality);
+        DEBUG_SET(DEBUG_OPTICALFLOW, 1, lrintf(opticalflow.rawFlowRates.x * 1000));
+        DEBUG_SET(DEBUG_OPTICALFLOW, 2, lrintf(opticalflow.rawFlowRates.y * 1000));
+        DEBUG_SET(DEBUG_OPTICALFLOW, 3, lrintf(opticalflow.processedFlowRates.x * 1000));
+        DEBUG_SET(DEBUG_OPTICALFLOW, 4, lrintf(opticalflow.processedFlowRates.y * 1000));
+        DEBUG_SET(DEBUG_OPTICALFLOW, 5, deltaTimeUs);
+    }
+}
+
+static void applySensorRotation(vector2_t * dst, vector2_t * src) {
+    dst->x = (opticalflowConfig()->flip_x ? -1.0f : 1.0f) * (src->x * cosRotAngle - src->y * sinRotAngle);
+    dst->y = src->x * sinRotAngle + src->y * cosRotAngle;
+}
+
+static void applyLPF(vector2_t * flowRates) {
+    if (opticalflowConfig()->flow_lpf == 0) {
+        return;
+    }    
+
+    flowRates->x = pt2FilterApply(&xFlowLpf, flowRates->x);
+    flowRates->y = pt2FilterApply(&yFlowLpf, flowRates->y);
+}
+
+const opticalflow_t * getLatestFlowOpticalflowData(void) {
+    return &opticalflow;
+}
+
+bool isOpticalflowHealthy(void) {
+    return cmp32(micros(), opticalflow.timeStampUs) < OPTICALFLOW_HARDWARE_TIMEOUT_US;
+}
+#endif // USE_OPTICALFLOW
--- a/src/main/sensors/opticalflow.h
+++ b/src/main/sensors/opticalflow.h
@ -0,0 +1,56 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "drivers/opticalflow/opticalflow.h"
+
+#include "pg/pg.h"
+
+typedef enum {
+    OPTICALFLOW_NONE = 0,
+    OPTICALFLOW_MT = 1,
+} opticalflowType_e;
+
+typedef struct opticalflowConfig_s {
+    uint8_t  opticalflow_hardware;
+    uint16_t rotation;
+    uint8_t  flip_x;
+    uint16_t flow_lpf;
+} opticalflowConfig_t;
+
+PG_DECLARE(opticalflowConfig_t, opticalflowConfig);
+
+typedef struct opticalflow_s {
+    opticalflowDev_t dev;
+    int16_t quality;
+    vector2_t rawFlowRates;
+    vector2_t processedFlowRates;
+    uint32_t timeStampUs;
+} opticalflow_t;
+
+bool opticalflowInit(void);
+
+void opticalflowUpdate(void);
+bool isOpticalflowHealthy(void);
+void opticalflowProcess(void);
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@ -26,6 +26,7 @@ typedef enum {
    SENSOR_INDEX_BARO,
    SENSOR_INDEX_MAG,
    SENSOR_INDEX_RANGEFINDER,
+    SENSOR_INDEX_OPTICALFLOW,
    SENSOR_INDEX_COUNT
 } sensorIndex_e;

@ -52,5 +53,6 @@ typedef enum {
    SENSOR_SONAR = 1 << 4,
    SENSOR_RANGEFINDER = 1 << 4,
    SENSOR_GPS = 1 << 5,
-    SENSOR_GPSMAG = 1 << 6
+    SENSOR_GPSMAG = 1 << 6,
+    SENSOR_OPTICALFLOW = 1 << 7
 } sensors_e;