Optical flow sensor code initial implementation (phase 1)

Makefile

@@ -670,6 +670,7 @@ HIGHEND_SRC = \
             drivers/rangefinder_hcsr04.c \
             drivers/rangefinder_hcsr04_i2c.c \
             drivers/rangefinder_srf10.c \
+            drivers/opflow_fake.c \
             drivers/rangefinder_vl53l0x.c \
             drivers/vtx_common.c \
             io/dashboard.c \
@@ -692,10 +693,11 @@ HIGHEND_SRC = \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \
+            sensors/opflow.c \
             telemetry/crsf.c \
             telemetry/frsky.c \
             telemetry/hott.c \
-	    telemetry/ibus_shared.c \
+            telemetry/ibus_shared.c \
             telemetry/ibus.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \

src/main/config/parameter_group_ids.h

@@ -94,7 +94,8 @@
 #define PG_STATS_CONFIG 1009
 #define PG_ADC_CHANNEL_CONFIG 1010
 #define PG_TIME_CONFIG 1011
-#define PG_INAV_END 1011
+#define PG_OPFLOW_CONFIG 1012
+#define PG_INAV_END 1012
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/logging.c

@@ -57,6 +57,7 @@ static const char *     eventDescription[BOOT_EVENT_CODE_COUNT] = {
     [BOOT_EVENT_TIMER_CH_MAPPED]            = "TIMER_CHANNEL_MAPPED",
     [BOOT_EVENT_PITOT_DETECTION]            = "PITOT_DETECTION",
     [BOOT_EVENT_HARDWARE_IO_CONFLICT]       = "HARDWARE_CONFLICT",
+    [BOOT_EVENT_OPFLOW_DETECTION]           = "OPFLOW_DETECTION",
 };
 
 const char * getBootlogEventDescription(bootLogEventCode_e eventCode)

src/main/drivers/logging_codes.h

@@ -49,6 +49,7 @@ typedef enum {
     BOOT_EVENT_TIMER_CH_MAPPED          = 19,   // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO
     BOOT_EVENT_PITOT_DETECTION          = 20,
     BOOT_EVENT_HARDWARE_IO_CONFLICT     = 21,   // Hardware IO resource conflict, parameters: #1 - current owner, #2 - requested owner
+    BOOT_EVENT_OPFLOW_DETECTION         = 22,
 
     BOOT_EVENT_CODE_COUNT
 } bootLogEventCode_e;

src/main/drivers/opflow.h

@@ -0,0 +1,42 @@
+/*
+ * 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
+
+#include "common/time.h"
+#include "drivers/sensor.h"
+
+struct opflowDev_s;
+
+typedef struct opflowData_s {
+    timeDelta_t deltaTime;      // Integration timeframe of motionX/Y
+    int32_t     flowRateRaw[2]; // Flow rotation in raw sensor uints (per deltaTime interval)
+    int16_t     quality;
+} opflowData_t;
+
+typedef struct opflowDev_s {
+    sensorOpflowInitFuncPtr initFn;
+    sensorOpflowUpdateFuncPtr updateFn;
+    opflowData_t rawData;
+} opflowDev_t;

src/main/drivers/opflow_fake.c

@@ -0,0 +1,70 @@
+/*
+ * 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 <string.h>
+
+#include <platform.h>
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "opflow.h"
+#include "opflow_fake.h"
+
+#ifdef USE_OPFLOW_FAKE
+static opflowData_t fakeData;
+
+void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY, int16_t quality)
+{
+    fakeData.deltaTime = deltaTime;
+    fakeData.flowRateRaw[0] = flowRateX;
+    fakeData.flowRateRaw[1] = flowRateY;
+    fakeData.quality = quality;
+}
+
+static bool fakeOpflowInit(opflowDev_t * dev)
+{
+    UNUSED(dev);
+    return true;
+}
+
+static bool fakeOpflowUpdate(opflowDev_t * dev)
+{
+    memcpy(&dev->rawData, &fakeData, sizeof(opflowData_t));
+    return true;
+}
+
+bool fakeOpflowDetect(opflowDev_t * dev)
+{
+    dev->initFn = &fakeOpflowInit;
+    dev->updateFn = &fakeOpflowUpdate;
+
+    memset(&dev->rawData, 0, sizeof(opflowData_t));
+
+    return true;
+}
+#endif

src/main/drivers/opflow_fake.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 fakeOpflowDetect(opflowDev_t * dev);
+void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY, int16_t quality);

src/main/drivers/sensor.h

@@ -50,3 +50,6 @@ typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
 struct magDev_s;
 typedef bool (*sensorMagInitFuncPtr)(struct magDev_s *mag);
 typedef bool (*sensorMagReadFuncPtr)(struct magDev_s *mag);
+struct opflowDev_s;
+typedef bool (*sensorOpflowInitFuncPtr)(struct opflowDev_s *mag);
+typedef bool (*sensorOpflowUpdateFuncPtr)(struct opflowDev_s *mag);

src/main/fc/cli.c

@@ -109,6 +109,7 @@ extern uint8_t __config_end;
 #include "sensors/gyro.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
 #include "sensors/sensors.h"
 
 #include "telemetry/frsky.h"
@@ -168,10 +169,10 @@ static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D",
 
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
-    "GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "PITOT", "GPS", "GPS+MAG", NULL
+    "GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "PITOT", "OPFLOW", "GPS", "GPS+MAG", NULL
 };
 
-#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER | SENSOR_PITOT)
+#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER | SENSOR_PITOT | SENSOR_OPFLOW)
 
 static const char * const hardwareSensorStatusNames[] = {
     "NONE", "OK", "UNAVAILABLE", "FAILING"
@@ -190,7 +191,10 @@ static const char * const *sensorHardwareNames[] = {
         table_rangefinder_hardware,
 #endif
 #ifdef PITOT
-        table_pitot_hardware
+        table_pitot_hardware,
+#endif
+#ifdef USE_OPTICAL_FLOW
+        table_opflow_hardware,
 #endif
 };
 
@@ -2349,12 +2353,13 @@ static void cliStatus(char *cmdline)
     cliPrintLinef("  PCLK2  = %d MHz", clocks.PCLK2_Frequency / 1000000);
 #endif
 
-    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, GPS=%s",
+    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s",
         hardwareSensorStatusNames[getHwGyroStatus()],
         hardwareSensorStatusNames[getHwAccelerometerStatus()],
         hardwareSensorStatusNames[getHwCompassStatus()],
         hardwareSensorStatusNames[getHwBarometerStatus()],
         hardwareSensorStatusNames[getHwRangefinderStatus()],
+        hardwareSensorStatusNames[getHwOpticalFlowStatus()],
         hardwareSensorStatusNames[getHwGPSStatus()]
     );
 #endif

src/main/fc/fc_core.c

@@ -45,6 +45,7 @@
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
 
 #include "fc/fc_core.h"
 #include "fc/cli.h"
@@ -653,10 +654,12 @@ void taskGyro(timeUs_t currentTimeUs) {
     // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
     // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
     const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+    timeUs_t gyroUpdateUs;
 
     if (gyroConfig()->gyroSync) {
         while (true) {
-            if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(micros(), currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
+            gyroUpdateUs = micros();
+            if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
                 break;
             }
         }
@@ -667,7 +670,13 @@ void taskGyro(timeUs_t currentTimeUs) {
 
 #ifdef ASYNC_GYRO_PROCESSING
     /* Update IMU for better accuracy */
-    imuUpdateGyroscope((timeUs_t)currentDeltaTime + (micros() - currentTimeUs));
+    imuUpdateGyroscope((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));
+#endif
+
+#ifdef USE_OPTICAL_FLOW
+    if (sensors(SENSOR_OPFLOW)) {
+        opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));
+    }
 #endif
 }
 

src/main/fc/fc_msp.c

@@ -94,6 +94,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/opflow.h"
 
 #include "telemetry/telemetry.h"
 
@@ -341,7 +342,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, getHwGPSStatus());
         sbufWriteU8(dst, getHwRangefinderStatus());
         sbufWriteU8(dst, getHwPitotmeterStatus());
-        sbufWriteU8(dst, HW_SENSOR_NONE);                   // Optical flow
+        sbufWriteU8(dst, getHwOpticalFlowStatus());
         break;
 
     case MSP_ACTIVEBOXES:

src/main/fc/fc_tasks.c

@@ -73,6 +73,7 @@
 #include "sensors/gyro.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
 
 #include "telemetry/telemetry.h"
 
@@ -195,6 +196,19 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_OPTICAL_FLOW
+void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (!sensors(SENSOR_OPFLOW))
+        return;
+
+    opflowUpdate(currentTimeUs);
+    updatePositionEstimator_OpticalFlowTopic(currentTimeUs);
+}
+#endif
+
 #ifdef USE_DASHBOARD
 void taskDashboardUpdate(timeUs_t currentTimeUs)
 {
@@ -347,6 +361,9 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
 #endif
 #endif
+#ifdef USE_OPTICAL_FLOW
+    setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
+#endif
 #ifdef VTX_CONTROL
 #if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
     setTaskEnabled(TASK_VTXCTRL, true);
@@ -545,6 +562,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_OPTICAL_FLOW
+    [TASK_OPFLOW] = {
+        .taskName = "OPFLOW",
+        .taskFunc = taskUpdateOpticalFlow,
+        .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UIB sensor will work at lower rate w/o accumulation
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 #ifdef USE_RCSPLIT
     [TASK_RCSPLIT] = {
         .taskName = "RCSPLIT",

src/main/fc/settings.yaml

@@ -12,6 +12,9 @@ tables:
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "FAKE"]
     enum: magSensor_e
+  - name: opflow_hardware
+    values: ["NONE", "FAKE"]
+    enum: opticalFlowSensor_e
   - name: baro_hardware
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "FAKE"]
     enum: baroSensor_e
@@ -201,6 +204,17 @@ groups:
       - name: rangefinder_hardware
         table: rangefinder_hardware
 
+  - name: PG_OPFLOW_CONFIG
+    type: opticalFlowConfig_t
+    headers: ["sensors/opflow.h"]
+    condition: USE_OPTICAL_FLOW
+    members:
+      - name: opflow_hardware
+        table: opflow_hardware
+      - name: opflow_scale
+        min: 0
+        max: 10000
+
   - name: PG_COMPASS_CONFIG
     type: compassConfig_t
     headers: ["sensors/compass.h"]

src/main/navigation/navigation.h

@@ -248,6 +248,7 @@ void navigationInit(void);
 
 /* Position estimator update functions */
 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
+void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs);
 void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt);
 
 /* Navigation system updates */

src/main/navigation/navigation_pos_estimator.c

@@ -53,6 +53,7 @@
 #include "sensors/compass.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
 #include "sensors/sensors.h"
 
 
@@ -113,6 +114,14 @@ typedef struct {
     float       alt;            // Raw altitude measurement (cm)
 } navPositionEstimatorSURFACE_t;
 
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    bool        isValid;
+    float       quality;
+    float       flowRate[2];
+    float       bodyRate[2];
+} navPositionEstimatorFLOW_t;
+
 typedef struct {
     timeUs_t    lastUpdateTime; // Last update time (us)
     // 3D position, velocity and confidence
@@ -150,6 +159,7 @@ typedef struct {
     navPositionEstimatorBARO_t  baro;
     navPositionEstimatorSURFACE_t surface;
     navPositionEstimatorPITOT_t pitot;
+    navPositionEstimatorFLOW_t  flow;
 
     // IMU data
     navPosisitonEstimatorIMU_t  imu;
@@ -483,6 +493,22 @@ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfa
 }
 #endif
 
+#ifdef USE_OPTICAL_FLOW
+/**
+ * Read optical flow topic
+ *  Function is called by OPFLOW task as soon as new update is available
+ */
+void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
+{
+    posEstimator.flow.lastUpdateTime = currentTimeUs;
+    posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
+    posEstimator.flow.flowRate[X] = opflow.flowRate[X];
+    posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
+    posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
+    posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
+}
+#endif
+
 /**
  * Update IMU topic
  *  Function is called at main loop rate

src/main/scheduler/scheduler.h

@@ -101,6 +101,9 @@ typedef enum {
 #ifdef CMS
     TASK_CMS,
 #endif
+#ifdef USE_OPTICAL_FLOW
+    TASK_OPFLOW,
+#endif
 #ifdef USE_RCSPLIT
     TASK_RCSPLIT,
 #endif

src/main/sensors/diagnostics.c

@@ -23,6 +23,7 @@
 #include "sensors/barometer.h"
 #include "sensors/rangefinder.h"
 #include "sensors/pitotmeter.h"
+#include "sensors/opflow.h"
 
 extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
 extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
@@ -189,6 +190,32 @@ hardwareSensorStatus_e getHwGPSStatus(void)
 #endif
 }
 
+hardwareSensorStatus_e getHwOpticalFlowStatus(void)
+{
+#if defined(USE_OPTICAL_FLOW)
+    if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
+        if (opflowIsHealthy()) {
+            return HW_SENSOR_OK;
+        }
+        else {
+            return HW_SENSOR_UNHEALTHY;
+        }
+    }
+    else {
+        if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
+            // Selected but not detected
+            return HW_SENSOR_UNAVAILABLE;
+        }
+        else {
+            // Not selected and not detected
+            return HW_SENSOR_NONE;
+        }
+    }
+#else
+    return HW_SENSOR_NONE;
+#endif
+}
+
 bool isHardwareHealthy(void)
 {
     const hardwareSensorStatus_e gyroStatus = getHwGyroStatus();
@@ -198,6 +225,7 @@ bool isHardwareHealthy(void)
     const hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus();
     const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
     const hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
+    const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus();
 
     // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings)
     if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY)
@@ -221,5 +249,8 @@ bool isHardwareHealthy(void)
     if (gpsStatus == HW_SENSOR_UNAVAILABLE || gpsStatus == HW_SENSOR_UNHEALTHY)
         return false;
 
+    if (opflowStatus == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY)
+        return false;
+
     return true;
 }

src/main/sensors/diagnostics.h

@@ -23,6 +23,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void);
 hardwareSensorStatus_e getHwGPSStatus(void);
 hardwareSensorStatus_e getHwRangefinderStatus(void);
 hardwareSensorStatus_e getHwPitotmeterStatus(void);
+hardwareSensorStatus_e getHwOpticalFlowStatus(void);
 
 bool isHardwareHealthy(void);
 

src/main/sensors/initialisation.c

@@ -36,10 +36,11 @@
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
 #include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
 #include "sensors/initialisation.h"
 
-uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE };
+uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
-uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE };
+uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
 
 
 bool sensorsAutodetect(void)
@@ -70,6 +71,10 @@ bool sensorsAutodetect(void)
     rangefinderInit();
 #endif
 
+#ifdef USE_OPTICAL_FLOW
+    opflowInit();
+#endif
+
     if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
         accelerometerConfigMutable()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC];
         eepromUpdatePending = true;
@@ -82,10 +87,12 @@ bool sensorsAutodetect(void)
     }
 #endif
 
+#ifdef MAG
     if (compassConfig()->mag_hardware == MAG_AUTODETECT) {
         compassConfigMutable()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
         eepromUpdatePending = true;
     }
+#endif
 
 #ifdef PITOT
     if (pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {

src/main/sensors/opflow.c

@@ -0,0 +1,206 @@
+/*
+ * 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 <string.h>
+#include <math.h>
+
+#include <platform.h>
+
+#include "build/build_config.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/io.h"
+#include "drivers/logging.h"
+#include "drivers/time.h"
+#include "sensors/gyro.h"
+#include "drivers/opflow.h"
+#include "drivers/opflow_fake.h"
+
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+
+#include "sensors/sensors.h"
+#include "sensors/opflow.h"
+
+#include "scheduler/scheduler.h"
+
+#include "build/debug.h"
+
+opflow_t opflow;
+
+#define OPFLOW_SQUAL_THRESHOLD_HIGH     50      // TBD
+#define OPFLOW_SQUAL_THRESHOLD_LOW      15      // TBD
+#define OPFLOW_UPDATE_TIMEOUT_US        100000  // At least 10Hz updates required
+
+#ifdef USE_OPTICAL_FLOW
+PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 0);
+
+PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
+    .opflow_hardware = OPFLOW_NONE,
+    .opflow_scale = 1.0f,
+);
+
+static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
+{
+    opticalFlowSensor_e opflowHardware = OPFLOW_NONE;
+    requestedSensors[SENSOR_INDEX_OPFLOW] = opflowHardwareToUse;
+
+    switch (opflowHardwareToUse) {
+        case OPFLOW_FAKE:
+#if defined(USE_OPFLOW_FAKE)
+            if (fakeOpflowDetect(dev)) {   // FIXME: Do actual detection if HC-SR04 is plugged in
+                opflowHardware = OPFLOW_FAKE;
+            }
+#endif
+            break;
+
+        case OPFLOW_NONE:
+            opflowHardware = OPFLOW_NONE;
+            break;
+    }
+
+    addBootlogEvent6(BOOT_EVENT_OPFLOW_DETECTION, BOOT_EVENT_FLAGS_NONE, opflowHardware, 0, 0, 0);
+
+    if (opflowHardware == OPFLOW_NONE) {
+        sensorsClear(SENSOR_OPFLOW);
+        return false;
+    }
+
+    detectedSensors[SENSOR_INDEX_OPFLOW] = opflowHardware;
+    sensorsSet(SENSOR_OPFLOW);
+    return true;
+}
+
+static void opflowZeroBodyGyroAcc(void)
+{
+    opflow.gyroBodyRateTimeUs = 0;
+    opflow.gyroBodyRateAcc[X] = 0;
+    opflow.gyroBodyRateAcc[Y] = 0;
+}
+
+bool opflowInit(void)
+{
+    if (!opflowDetect(&opflow.dev, opticalFlowConfig()->opflow_hardware)) {
+        return false;
+    }
+
+    opflow.dev.initFn(&opflow.dev);
+    opflowZeroBodyGyroAcc();
+
+    return true;
+}
+
+/*
+ * This is called periodically by the scheduler
+ */
+void opflowUpdate(timeUs_t currentTimeUs)
+{
+    if (!opflow.dev.updateFn)
+        return;
+
+    if (opflow.dev.updateFn(&opflow.dev)) {
+        // Indicate valid update
+        opflow.lastValidUpdate = currentTimeUs;
+        opflow.isHwHealty = true;
+
+        // Handle state switching
+        switch (opflow.flowQuality) {
+            case OPFLOW_QUALITY_INVALID:
+                if (opflow.dev.rawData.quality >= OPFLOW_SQUAL_THRESHOLD_HIGH) {
+                    opflow.flowQuality = OPFLOW_QUALITY_VALID;
+                }
+                break;
+
+            case OPFLOW_QUALITY_VALID:
+                if (opflow.dev.rawData.quality <= OPFLOW_SQUAL_THRESHOLD_LOW) {
+                    opflow.flowQuality = OPFLOW_QUALITY_INVALID;
+                }
+                break;
+        }
+
+        if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) {
+            const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale;
+            opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler);
+            opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler);
+
+            opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs);
+            opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs);
+        }
+        else {
+            // Opflow updated but invalid - zero out flow rates and body 
+            opflow.flowRate[X] = 0;
+            opflow.flowRate[Y] = 0;
+
+            opflow.bodyRate[X] = 0;
+            opflow.bodyRate[Y] = 0;
+        }
+
+        // Zero out gyro accumulators to calculate rotation per flow update
+        opflowZeroBodyGyroAcc();
+    }
+    else {
+        // No new data available
+        if (opflow.isHwHealty && ((currentTimeUs - opflow.lastValidUpdate) > OPFLOW_UPDATE_TIMEOUT_US)) {
+            opflow.isHwHealty = false;
+
+            opflow.flowQuality = OPFLOW_QUALITY_INVALID;
+
+            opflow.flowRate[X] = 0;
+            opflow.flowRate[Y] = 0;
+            opflow.bodyRate[X] = 0;
+            opflow.bodyRate[Y] = 0;
+
+            opflowZeroBodyGyroAcc();
+        }
+    }
+}
+
+/* Run a simple gyro update integrator to estimate average body rate between two optical flow updates */
+void opflowGyroUpdateCallback(timeUs_t gyroUpdateDeltaUs)
+{
+    if (!opflow.isHwHealty)
+        return;
+
+    for (int axis = 0; axis < 2; axis++) {
+        opflow.gyroBodyRateAcc[axis] += gyro.gyroADCf[axis] * gyroUpdateDeltaUs;
+    }
+
+    opflow.gyroBodyRateTimeUs += gyroUpdateDeltaUs;
+}
+
+bool opflowIsHealthy(void)
+{
+    return opflow.isHwHealty;
+}
+#endif

src/main/sensors/opflow.h

@@ -0,0 +1,66 @@
+/*
+ * 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
+
+#include <stdint.h>
+#include "config/parameter_group.h"
+#include "drivers/opflow.h"
+
+typedef enum {
+    OPFLOW_NONE         = 0,
+    OPFLOW_FAKE         = 1,
+} opticalFlowSensor_e;
+
+typedef enum {
+    OPFLOW_QUALITY_INVALID,
+    OPFLOW_QUALITY_VALID
+} opflowQuality_e;
+
+typedef struct opticalFlowConfig_s  {
+    uint8_t opflow_hardware;
+    float   opflow_scale;       // Scaler value to convert between raw sensor units to [deg/s]
+} opticalFlowConfig_t;
+
+PG_DECLARE(opticalFlowConfig_t, opticalFlowConfig);
+
+typedef struct opflow_s {
+    opflowDev_t dev;
+
+    opflowQuality_e flowQuality;
+    timeUs_t        lastValidUpdate;
+    bool            isHwHealty;
+    float           flowRate[2];    // optical flow angular rate in rad/sec measured about the X and Y body axis
+    float           bodyRate[2];    // body inertial angular rate in rad/sec measured about the X and Y body axis
+
+    float           gyroBodyRateAcc[2];
+    timeUs_t        gyroBodyRateTimeUs;    
+} opflow_t;
+
+extern opflow_t opflow;
+
+void opflowGyroUpdateCallback(timeUs_t gyroUpdateDeltaUs);
+bool opflowInit(void);
+void opflowUpdate(timeUs_t currentTimeUs);
+bool opflowIsHealthy(void);

src/main/sensors/sensors.h

@@ -24,6 +24,7 @@ typedef enum {
     SENSOR_INDEX_MAG,
     SENSOR_INDEX_RANGEFINDER,
     SENSOR_INDEX_PITOT,
+    SENSOR_INDEX_OPFLOW,
     SENSOR_INDEX_COUNT
 } sensorIndex_e;
 
@@ -49,8 +50,9 @@ typedef enum {
     SENSOR_MAG = 1 << 3,
     SENSOR_RANGEFINDER = 1 << 4,
     SENSOR_PITOT = 1 << 5,
-    SENSOR_GPS = 1 << 6,
+    SENSOR_OPFLOW = 1 << 6,
-    SENSOR_GPSMAG = 1 << 7,
+    SENSOR_GPS = 1 << 7,
+    SENSOR_GPSMAG = 1 << 8,
 } sensors_e;
 
 extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];

src/main/target/REVO/target.h

@@ -63,10 +63,14 @@
 //#define USE_PITOT_MS4525
 //#define PITOT_I2C_INSTANCE      I2C_DEVICE_EXT
 
+// #define USE_OPTICAL_FLOW
+// #define USE_OPFLOW_FAKE
+
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_VL53L0X
 #define RANGEFINDER_VL53L0X_INSTANCE    I2C_DEVICE_EXT
 
+
 #define M25P16_CS_PIN           PB3
 #define M25P16_SPI_INSTANCE     SPI3