Merge pull request #3795 from iNavFlight/de_opflow_fixes

digitalentity · web-flow · commit 16faa4399f2d · 2019-05-02T13:13:31.000+02:00
Optic flow &amp; surface improvements
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
@@ -188,7 +188,7 @@ static const char * const *sensorHardwareNames[] = {
 #else
         NULL,
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
         table_opflow_hardware,
 #else
         NULL,
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -635,7 +635,7 @@ void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) {
     /* Update actual hardware readings */
     gyroUpdate();
 
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
     if (sensors(SENSOR_OPFLOW)) {
         opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));
     }
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -542,7 +542,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
     case MSP2_INAV_OPTICAL_FLOW:
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
         sbufWriteU8(dst, opflow.rawQuality);
         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X]));
         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
@@ -1062,7 +1062,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, gyroConfig()->gyro_align);
         sbufWriteU8(dst, accelerometerConfig()->acc_align);
         sbufWriteU8(dst, compassConfig()->mag_align);
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
         sbufWriteU8(dst, opticalFlowConfig()->opflow_align);
 #else
         sbufWriteU8(dst, 0);
@@ -1178,7 +1178,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #else
         sbufWriteU8(dst, 0);
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
         sbufWriteU8(dst, opticalFlowConfig()->opflow_hardware);
 #else
         sbufWriteU8(dst, 0);
@@ -1252,6 +1252,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);
     #endif
+
+    #ifdef USE_OPFLOW
+        sbufWriteU16(dst, opticalFlowConfig()->opflow_scale * 256);
+    #else
+        sbufWriteU16(dst, 0);
+    #endif
         break;
 
     case MSP_POSITION_ESTIMATION_CONFIG:
@@ -1948,7 +1954,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #else
             sbufReadU8(src);
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
             opticalFlowConfigMutable()->opflow_align = sbufReadU8(src);
 #else
             sbufReadU8(src);
@@ -2080,7 +2086,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #else
             sbufReadU8(src);        // rangefinder hardware
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
             opticalFlowConfigMutable()->opflow_hardware = sbufReadU8(src);
 #else
             sbufReadU8(src);        // optical flow hardware
@@ -2163,6 +2169,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             sbufReadU16(src);
             sbufReadU16(src);
             sbufReadU16(src);
+#endif
+#ifdef USE_OPFLOW
+            if (dataSize >= 20) {
+                opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f;
+            }
 #endif
         } else
             return MSP_RESULT_ERROR;
@@ -2205,6 +2216,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         break;
 
+#ifdef USE_OPFLOW
+    case MSP2_INAV_OPFLOW_CALIBRATION:
+        if (!ARMING_FLAG(ARMED))
+            opflowStartCalibration();
+        else
+            return MSP_RESULT_ERROR;
+        break;
+#endif
+
     case MSP_EEPROM_WRITE:
         if (!ARMING_FLAG(ARMED)) {
             writeEEPROM();
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
@@ -203,7 +203,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 {
     if (!sensors(SENSOR_OPFLOW))
@@ -324,7 +324,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
 #endif
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
     setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
 #endif
 #ifdef USE_VTX_CONTROL
@@ -511,7 +511,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
     [TASK_OPFLOW] = {
         .taskName = "OPFLOW",
         .taskFunc = taskUpdateOpticalFlow,
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -261,7 +261,7 @@ groups:
   - name: PG_OPFLOW_CONFIG
     type: opticalFlowConfig_t
     headers: ["sensors/opflow.h"]
-    condition: USE_OPTICAL_FLOW
+    condition: USE_OPFLOW
     members:
       - name: opflow_hardware
         table: opflow_hardware
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
@@ -60,3 +60,5 @@
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
+
+#define MSP2_INAV_OPFLOW_CALIBRATION            0x2032
diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c
@@ -45,7 +45,7 @@
 
 extern navigationPosEstimator_t posEstimator;
 
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
 /**
  * Read optical flow topic
  *  Function is called by OPFLOW task as soon as new update is available
@@ -63,7 +63,7 @@ void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
 
 bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
 {
-#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW)
+#if defined(USE_RANGEFINDER) && defined(USE_OPFLOW)
     if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {
         return false;
     }
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
@@ -98,7 +98,7 @@ typedef enum {
 #ifdef USE_CMS
     TASK_CMS,
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
     TASK_OPFLOW,
 #endif
 #ifdef USE_UAV_INTERCONNECT
diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
@@ -192,7 +192,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
 
 hardwareSensorStatus_e getHwOpticalFlowStatus(void)
 {
-#if defined(USE_OPTICAL_FLOW)
+#if defined(USE_OPFLOW)
     if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
         if (opflowIsHealthy()) {
             return HW_SENSOR_OK;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
@@ -74,7 +74,7 @@ bool sensorsAutodetect(void)
     rangefinderInit();
 #endif
 
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
     opflowInit();
 #endif
 
diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c
@@ -41,6 +41,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/io.h"
+#include "drivers/light_led.h"
 #include "drivers/logging.h"
 #include "drivers/time.h"
 
@@ -64,17 +65,23 @@
 
 opflow_t opflow;
 
+#ifdef USE_OPFLOW
+static bool opflowIsCalibrating = false;
+static timeMs_t opflowCalibrationStartedAt;
+static float opflowCalibrationBodyAcc;
+static float opflowCalibrationFlowAcc;
+
 #define OPFLOW_SQUAL_THRESHOLD_HIGH     35      // TBD
 #define OPFLOW_SQUAL_THRESHOLD_LOW      10      // TBD
 #define OPFLOW_UPDATE_TIMEOUT_US        200000  // At least 5Hz updates required
+#define OPFLOW_CALIBRATE_TIME_MS        30000   // 30 second calibration time
 
-#ifdef USE_OPTICAL_FLOW
 PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1);
 
 PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
     .opflow_hardware = OPFLOW_NONE,
     .opflow_align = CW0_DEG_FLIP,
-    .opflow_scale = 1.0f,
+    .opflow_scale = 10.5f,
 );
 
 static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
@@ -147,6 +154,14 @@ bool opflowInit(void)
     return true;
 }
 
+void opflowStartCalibration(void)
+{
+    opflowCalibrationStartedAt = millis();
+    opflowIsCalibrating = true;
+    opflowCalibrationBodyAcc = 0;
+    opflowCalibrationFlowAcc = 0;
+}
+
 /*
  * This is called periodically by the scheduler
  */
@@ -157,8 +172,8 @@ void opflowUpdate(timeUs_t currentTimeUs)
 
     if (opflow.dev.updateFn(&opflow.dev)) {
         // Indicate valid update
-        opflow.lastValidUpdate = currentTimeUs;
         opflow.isHwHealty = true;
+        opflow.lastValidUpdate = currentTimeUs;
         opflow.rawQuality = opflow.dev.rawData.quality;
 
         // Handle state switching
@@ -176,33 +191,65 @@ void opflowUpdate(timeUs_t currentTimeUs)
                 break;
         }
 
-        if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) {
-            const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale;
+        // Opflow updated. Assume zero valus unless further processing sets otherwise
+        opflow.flowRate[X] = 0;
+        opflow.flowRate[Y] = 0;
+        opflow.bodyRate[X] = 0;
+        opflow.bodyRate[Y] = 0;
+
+        // In the following code we operate deg/s and do conversion to rad/s in the last step
+        // Calculate body rates
+        if (opflow.gyroBodyRateTimeUs > 0) {
+            opflow.bodyRate[X] = opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs;
+            opflow.bodyRate[Y] = opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs;
+        }
+
+        // If quality of the flow from the sensor is good - process further
+        if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
+            const float integralToRateScaler = (opticalFlowConfig()->opflow_scale > 0.01f) ? (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale : 0.0f;
 
             // Apply sensor alignment
             applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align);
-            //applyBoardAlignment(opflow.dev.rawData.flowRateRaw);
 
-            opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler);
-            opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler);
+            // Calculate flow rate and accumulated body rate
+            opflow.flowRate[X] = opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler;
+            opflow.flowRate[Y] = 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);
-
-            DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[X]));
-            DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[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]));
+            // Only update DEBUG_FLOW_RAW if flow is good
+            DEBUG_SET(DEBUG_FLOW_RAW, 0, (opflow.flowRate[X]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 1, (opflow.flowRate[Y]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 2, (opflow.bodyRate[X]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 3, (opflow.bodyRate[Y]));
         }
-        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;
+        // Process calibration
+        if (opflowIsCalibrating) {
+            // Blink LED
+            LED0_TOGGLE;
+
+            if ((millis() - opflowCalibrationStartedAt) > OPFLOW_CALIBRATE_TIME_MS) {
+                // Finish calibration if we accumulated more than 3600 deg of rotation over 30 seconds
+                if (opflowCalibrationBodyAcc > 3600.0f) {
+                    opticalFlowConfigMutable()->opflow_scale = opflowCalibrationFlowAcc / opflowCalibrationBodyAcc;
+                    saveConfigAndNotify();
+                }
+
+                opflowIsCalibrating = 0;
+            }
+            else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
+                // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough
+                const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime;
+                opflowCalibrationBodyAcc += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
+                opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
+            }
         }
 
+        // Convert to radians so NAV doesn't have to do the conversion
+        opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.bodyRate[X]);
+        opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.bodyRate[Y]);
+        opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.flowRate[X]);
+        opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.flowRate[Y]);
+
         // Zero out gyro accumulators to calculate rotation per flow update
         opflowZeroBodyGyroAcc();
     }
diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h
@@ -69,4 +69,5 @@ extern opflow_t opflow;
 void opflowGyroUpdateCallback(timeUs_t gyroUpdateDeltaUs);
 bool opflowInit(void);
 void opflowUpdate(timeUs_t currentTimeUs);
-bool opflowIsHealthy(void);
+bool opflowIsHealthy(void);
+void opflowStartCalibration(void);
diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h
@@ -134,9 +134,6 @@
 
 #define TEMPERATURE_I2C_BUS         DEFAULT_I2C_BUS
 
-#define USE_OPTICAL_FLOW
-#define USE_OPFLOW_MSP
-
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
 #define USE_RANGEFINDER_HCSR04_I2C
diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h
@@ -169,9 +169,6 @@
 
 #define TEMPERATURE_I2C_BUS         DEFAULT_I2C_BUS
 
-#define USE_OPTICAL_FLOW
-#define USE_OPFLOW_MSP
-
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
 #define USE_RANGEFINDER_HCSR04_I2C
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
@@ -131,9 +131,6 @@
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     I2C_EXT_BUS
 
-#define USE_OPTICAL_FLOW
-#define USE_OPFLOW_CXOF
-
 #define USE_VCP
 #define VBUS_SENSING_PIN        PC5
 #define VBUS_SENSING_ENABLED
diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h
@@ -56,12 +56,6 @@
 //#define USE_PITOT_MS4525
 //#define PITOT_I2C_BUS           BUS_I2C2
 
-// #define USE_OPTICAL_FLOW
-// #define USE_OPFLOW_CXOF
-
-// #define USE_RANGEFINDER
-// #define RANGEFINDER_I2C_BUS    BUS_I2C2
-
 #define USE_VCP
 #define VBUS_SENSING_PIN        PA9
 #define VBUS_SENSING_ENABLED
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
@@ -70,8 +70,6 @@
 
 #define PITOT_I2C_BUS           BUS_I2C2
 
-#define USE_OPTICAL_FLOW
-
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h

Original file line number	Diff line number	Diff line change
`@@ -635,7 +635,7 @@ void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) {`
`635`	`635`	`/* Update actual hardware readings */`
`636`	`636`	`gyroUpdate();`
`637`	`637`
`638`		`-#ifdef USE_OPTICAL_FLOW`
	`638`	`+#ifdef USE_OPFLOW`
`639`	`639`	`if (sensors(SENSOR_OPFLOW)) {`
`640`	`640`	`opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));`
`641`	`641`	`}`
Original file line number	Diff line number	Diff line change
`@@ -45,7 +45,7 @@`
`45`	`45`
`46`	`46`	`extern navigationPosEstimator_t posEstimator;`
`47`	`47`
`48`		`-#ifdef USE_OPTICAL_FLOW`
	`48`	`+#ifdef USE_OPFLOW`
`49`	`49`	`/**`
`50`	`50`	`* Read optical flow topic`
`51`	`51`	`* Function is called by OPFLOW task as soon as new update is available`
`@@ -63,7 +63,7 @@ void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)`
`63`	`63`
`64`	`64`	`bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)`
`65`	`65`	`{`
`66`		`-#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW)`
	`66`	`+#if defined(USE_RANGEFINDER) && defined(USE_OPFLOW)`
`67`	`67`	`if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {`
`68`	`68`	`return false;`
`69`	`69`	`}`
Original file line number	Diff line number	Diff line change
`@@ -192,7 +192,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)`
`192`	`192`
`193`	`193`	`hardwareSensorStatus_e getHwOpticalFlowStatus(void)`
`194`	`194`	`{`
`195`		`-#if defined(USE_OPTICAL_FLOW)`
	`195`	`+#if defined(USE_OPFLOW)`
`196`	`196`	`if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {`
`197`	`197`	`if (opflowIsHealthy()) {`
`198`	`198`	`return HW_SENSOR_OK;`