CXOF flow sensor (UART)

Enable opflow on sparky2
Optical flow MSP updates

Makefile

@@ -682,8 +682,10 @@ HIGHEND_SRC = \
             drivers/rangefinder/rangefinder_hcsr04_i2c.c \
             drivers/rangefinder/rangefinder_srf10.c \
             drivers/rangefinder/rangefinder_vl53l0x.c \
-            drivers/opflow_fake.c \
+            drivers/opflow/opflow_fake.c \
+            drivers/opflow/opflow_virtual.c \
             drivers/vtx_common.c \
+            io/opflow_cxof.c \
             io/dashboard.c \
             io/displayport_max7456.c \
             io/displayport_msp.c \

src/main/build/debug.h

@@ -54,5 +54,6 @@ typedef enum {
     DEBUG_RANGEFINDER_QUALITY,
     DEBUG_PITOT,
     DEBUG_AGL,
+    DEBUG_FLOW_RAW,
     DEBUG_COUNT
 } debugType_e;

src/main/drivers/opflow/opflow_virtual.c

@@ -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/.
+ */
+ 
+/*
+ * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB)
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform.h>
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "drivers/opflow/opflow_virtual.h"
+
+static const virtualOpflowVTable_t * highLevelDeviceVTable = NULL;
+
+static bool virtualOpflowInit(opflowDev_t * dev)
+{
+    UNUSED(dev);
+    return highLevelDeviceVTable->init();
+}
+
+static bool virtualOpflowUpdate(opflowDev_t * dev)
+{
+    UNUSED(dev);
+    return highLevelDeviceVTable->update(&dev->rawData);
+}
+
+bool virtualOpflowDetect(opflowDev_t * dev, const virtualOpflowVTable_t * vtable)
+{
+    if (vtable && vtable->detect()) {
+        highLevelDeviceVTable = vtable;
+        dev->initFn = &virtualOpflowInit;
+        dev->updateFn = &virtualOpflowUpdate;
+        memset(&dev->rawData, 0, sizeof(opflowData_t));
+        return true;
+    }
+
+    return false;
+}

src/main/drivers/opflow/opflow_virtual.h

@@ -0,0 +1,36 @@
+/*
+ * 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 "drivers/opflow/opflow.h"
+
+typedef struct virtualOpflowVTable_s {
+    bool (*detect)(void);
+    bool (*init)(void);
+    bool (*update)(opflowData_t * data);
+} virtualOpflowVTable_t;
+
+bool virtualOpflowDetect(opflowDev_t * dev, const virtualOpflowVTable_t * vtable);
+

src/main/fc/fc_msp.c

@@ -485,6 +485,22 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
         break;
 
+    case MSP2_INAV_OPTICAL_FLOW:
+#ifdef USE_OPTICAL_FLOW
+        sbufWriteU8(dst, opflow.rawQuality);
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
+#else
+        sbufWriteU8(dst, opflow.rawQuality);
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
+        sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
+#endif
+        break;
+
     case MSP_ANALOG:
         sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
         sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery

src/main/fc/fc_tasks.c

@@ -574,7 +574,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [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
+        .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif

src/main/fc/settings.yaml

@@ -13,7 +13,7 @@ tables:
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "FAKE"]
     enum: magSensor_e
   - name: opflow_hardware
-    values: ["NONE", "FAKE"]
+    values: ["NONE", "FAKE", "CXOF", "PMW3901"]
     enum: opticalFlowSensor_e
   - name: baro_hardware
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "FAKE"]
@@ -66,7 +66,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "RFIND", "RFIND_Q", "PITOT", "AGL", "FLOW_RAW"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator

src/main/io/opflow.h

@@ -0,0 +1,32 @@
+/*
+ * 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 <stdbool.h>
+
+#include "sensors/opflow.h"
+#include "drivers/opflow/opflow_virtual.h"
+
+extern virtualOpflowVTable_t opflowCxofVtable;

src/main/io/opflow_cxof.c

@@ -0,0 +1,145 @@
+/*
+ * 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 <ctype.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "io/serial.h"
+
+#if defined(USE_OPFLOW_CXOF)
+#include "drivers/opflow/opflow_virtual.h"
+#include "drivers/time.h"
+#include "io/opflow.h"
+
+#define CXOF_PACKET_SIZE    9
+static serialPort_t * flowPort = NULL;
+static serialPortConfig_t * portConfig;
+static uint8_t  buffer[10];
+static int      bufferPtr;
+
+typedef struct __attribute__((packed)) {
+    uint8_t     header;
+    uint8_t     res0;
+    int16_t     motionX;
+    int16_t     motionY;
+    int8_t      motionT;
+    uint8_t     squal;
+    uint8_t     footer;
+} cxofPacket_t;
+
+static bool cxofOpflowDetect(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_OPTICAL_FLOW);
+    if (!portConfig) {
+        return false;
+    }
+
+    return true;
+}
+
+static bool cxofOpflowInit(void)
+{
+    if (!portConfig) {
+        return false;
+    }
+
+    flowPort = openSerialPort(portConfig->identifier, FUNCTION_OPTICAL_FLOW, NULL, NULL, baudRates[BAUD_19200], MODE_RX, SERIAL_NOT_INVERTED);
+    if (!flowPort) {
+        return false;
+    }
+
+    bufferPtr = 0;
+
+    return true;
+}
+
+static bool cxofOpflowUpdate(opflowData_t * data)
+{
+    static timeUs_t previousTimeUs = 0;
+    const timeUs_t currentTimeUs = micros();
+
+    bool newPacket = false;
+    opflowData_t tmpData = { 0 };
+
+    while (serialRxBytesWaiting(flowPort) > 0) {
+        uint8_t c = serialRead(flowPort);
+
+        // Wait for header
+        if (bufferPtr == 0) {
+            if (c != 0xFE) {
+                break;
+            }
+        }
+
+        // Consume received byte
+        if (bufferPtr < CXOF_PACKET_SIZE) {
+            buffer[bufferPtr++] = c;
+
+            if (bufferPtr == CXOF_PACKET_SIZE) {
+                cxofPacket_t * pkt = (cxofPacket_t *)&buffer[0];
+
+                if (pkt->header == 0xFE && pkt->footer == 0xAA) {
+                    // Valid packet
+                    tmpData.deltaTime += (currentTimeUs - previousTimeUs);
+                    tmpData.flowRateRaw[0] +=  pkt->motionX;
+                    tmpData.flowRateRaw[1] += -pkt->motionY;    // Flow sensor is facing down, apply 180 roll rotation
+                    tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;
+
+                    previousTimeUs = currentTimeUs;
+                    newPacket = true;
+                }
+
+                // Reset the decoder
+                bufferPtr = 0;
+            }
+        }
+        else {
+            // In case of buffer overflow - reset the decoder
+            bufferPtr = 0;
+        }
+    }
+    
+    if (newPacket) {
+        *data = tmpData;
+    }
+
+    return newPacket;
+}
+
+virtualOpflowVTable_t opflowCxofVtable = {
+    .detect = cxofOpflowDetect,
+    .init = cxofOpflowInit,
+    .update = cxofOpflowUpdate
+};
+
+#endif

src/main/io/serial.h

@@ -45,6 +45,7 @@ typedef enum {
     FUNCTION_VTX_SMARTAUDIO      = (1 << 11), // 2048
     FUNCTION_VTX_TRAMP           = (1 << 12), // 4096
     FUNCTION_UAV_INTERCONNECT    = (1 << 13), // 8192
+    FUNCTION_OPTICAL_FLOW        = (1 << 14), // 16384
 } serialPortFunction_e;
 
 typedef enum {

src/main/msp/msp_protocol_v2_inav.h

@@ -19,3 +19,4 @@
 // See https://github.com/iNavFlight/inav/wiki/MSP-V2#msp-v2-message-catalogue
 
 #define MSP2_INAV_STATUS            0x2000
+#define MSP2_INAV_OPTICAL_FLOW      0x2001

src/main/sensors/opflow.c

@@ -43,18 +43,22 @@
 #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 "drivers/opflow/opflow.h"
+#include "drivers/opflow/opflow_fake.h"
+#include "drivers/opflow/opflow_virtual.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
+#include "sensors/gyro.h"
 #include "sensors/sensors.h"
 #include "sensors/opflow.h"
 
 #include "scheduler/scheduler.h"
 
+#include "io/opflow.h"
+
 #include "build/debug.h"
 
 opflow_t opflow;
@@ -85,6 +89,14 @@ static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
 #endif
             break;
 
+        case OPFLOW_CXOF:
+#if defined(USE_OPFLOW_CXOF)
+            if (virtualOpflowDetect(dev, &opflowCxofVtable)) {   // FIXME: Do actual detection if HC-SR04 is plugged in
+                opflowHardware = OPFLOW_CXOF;
+            }
+#endif
+            break;
+
         case OPFLOW_NONE:
             opflowHardware = OPFLOW_NONE;
             break;
@@ -115,7 +127,11 @@ bool opflowInit(void)
         return false;
     }
 
-    opflow.dev.initFn(&opflow.dev);
+    if (!opflow.dev.initFn(&opflow.dev)) {
+        sensorsClear(SENSOR_OPFLOW);
+        return false;
+    }
+
     opflowZeroBodyGyroAcc();
 
     return true;
@@ -133,6 +149,7 @@ void opflowUpdate(timeUs_t currentTimeUs)
         // Indicate valid update
         opflow.lastValidUpdate = currentTimeUs;
         opflow.isHwHealty = true;
+        opflow.rawQuality = opflow.dev.rawData.quality;
 
         // Handle state switching
         switch (opflow.flowQuality) {
@@ -156,6 +173,11 @@ void opflowUpdate(timeUs_t currentTimeUs)
 
             opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs);
             opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs);
+
+            DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.flowRate[X]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 2, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 3, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
         }
         else {
             // Opflow updated but invalid - zero out flow rates and body 
@@ -175,6 +197,7 @@ void opflowUpdate(timeUs_t currentTimeUs)
             opflow.isHwHealty = false;
 
             opflow.flowQuality = OPFLOW_QUALITY_INVALID;
+            opflow.rawQuality = 0;
 
             opflow.flowRate[X] = 0;
             opflow.flowRate[Y] = 0;

src/main/sensors/opflow.h

@@ -26,11 +26,13 @@
 
 #include <stdint.h>
 #include "config/parameter_group.h"
-#include "drivers/opflow.h"
+#include "drivers/opflow/opflow.h"
 
 typedef enum {
     OPFLOW_NONE         = 0,
     OPFLOW_FAKE         = 1,
+    OPFLOW_CXOF         = 2,
+    OPFLOW_PMW3901      = 3
 } opticalFlowSensor_e;
 
 typedef enum {
@@ -56,6 +58,8 @@ typedef struct opflow_s {
 
     float           gyroBodyRateAcc[2];
     timeUs_t        gyroBodyRateTimeUs;    
+
+    uint8_t         rawQuality;
 } opflow_t;
 
 extern opflow_t opflow;

src/main/target/REVO/target.h

@@ -67,8 +67,8 @@
 //#define USE_PITOT_MS4525
 //#define PITOT_I2C_BUS           BUS_I2C2
 
-// #define USE_OPTICAL_FLOW
-// #define USE_OPFLOW_FAKE
+#define USE_OPTICAL_FLOW
+#define USE_OPFLOW_CXOF
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_UIB

src/main/target/SPARKY2/target.h

@@ -79,7 +79,6 @@
 #define USE_UART1
 #define UART1_RX_PIN            PA10
 #define UART1_TX_PIN            PA9
-#define UART1_AHB1_PERIPHERALS  RCC_AHB1Periph_DMA2
 
 #define USE_UART3
 #define UART3_RX_PIN            PB11
@@ -111,25 +110,20 @@
 #define I2C_DEVICE_2_SHARES_UART3
 
 #define USE_ADC
-// PC2 shared with HC-SR04
 #define ADC_CHANNEL_1_PIN               PC2
 #define ADC_CHANNEL_2_PIN               PC1
 #define VBAT_ADC_CHANNEL                ADC_CHN_2
 #define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
 
-#define CURRENT_METER_ADC_PIN   PC2
-#define VBAT_ADC_PIN            PC1
-
-#define USE_LED_STRIP
-#define LED_STRIP_TIMER         TIM5
+#define LED_STRIP
+#define LED_STRIP_TIMER                 TIM5
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_VL53L0X
-#define VL53L0X_I2C_BUS                         BUS_I2C2
-// #define USE_RANGEFINDER_HCSR04
-// #define RANGEFINDER_HCSR04_TRIGGER_PIN       PC2
-// #define RANGEFINDER_HCSR04_ECHO_PIN          PC3
-// #define USE_RANGEFINDER_SRF10
+#define VL53L0X_I2C_BUS                 BUS_I2C2
+
+#define USE_OPTICAL_FLOW
+#define USE_OPFLOW_CXOF
 
 #define DEFAULT_FEATURES        FEATURE_BLACKBOX
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL