[OPFLOW] Refactor USE_* to make sure opflow is enabled for all target…

…s; Implement opflow calibration
iNavFlight · May 1, 2019 · 1271e4d · 1271e4d
1 parent d2368ff
commit 1271e4d
Show file tree

Hide file tree

Showing 16 changed files with 67 additions and 73 deletions.
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);
@@ -1253,7 +1253,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, 0);
     #endif
 
-    #ifdef USE_OPTICAL_FLOW
+    #ifdef USE_OPFLOW
         sbufWriteU16(dst, opticalFlowConfig()->opflow_scale * 256);
     #else
         sbufWriteU16(dst, 0);
@@ -1954,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);
@@ -2086,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
@@ -2170,7 +2170,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             sbufReadU16(src);
             sbufReadU16(src);
 #endif
-#ifdef USE_OPTICAL_FLOW
+#ifdef USE_OPFLOW
             if (dataSize >= 20) {
                 opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f;
             }

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/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"
 
@@ -74,7 +75,7 @@ static float opflowCalibrationFlowAcc;
 #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
+#ifdef USE_OPFLOW
 PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1);
 
 PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
@@ -171,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
@@ -190,51 +191,64 @@ void opflowUpdate(timeUs_t currentTimeUs)
                 break;
         }
 
-        if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) {
+        // 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);
 
             // Calculate flow rate and accumulated body rate
-            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);
+            opflow.flowRate[X] = opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler;
+            opflow.flowRate[Y] = opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler;
+
+            // 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]));
+        }
 
-            // Handle opflow calibration
-            if (opflowIsCalibrating) {
-                if ((millis() - opflowCalibrationStartedAt) > OPFLOW_CALIBRATE_TIME_MS) {
-                    // Finish calibration
-                    if (opflowCalibrationBodyAcc > 1000.0f) {
-                        opticalFlowConfigMutable()->opflow_scale = opflowCalibrationFlowAcc / opflowCalibrationBodyAcc;
-                    }
+        // Process calibration
+        if (opflowIsCalibrating) {
+            // Blink LED
+            LED0_TOGGLE;
 
-                    opflowIsCalibrating = 0;
+            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();
                 }
-                else {
-                    // Ongoing calibration - accumulate body and flow rates
-                    opflowCalibrationBodyAcc += ABS(opflow.bodyRate[X]);
-                    opflowCalibrationBodyAcc += ABS(opflow.bodyRate[Y]);
-                    opflowCalibrationFlowAcc += ABS(opflow.dev.rawData.flowRateRaw[X]) * (1.0e6 / opflow.dev.rawData.deltaTime);
-                    opflowCalibrationFlowAcc += ABS(opflow.dev.rawData.flowRateRaw[Y]) * (1.0e6 / opflow.dev.rawData.deltaTime);
-                }
-            }
 
-            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]));
+                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;
+            }
         }
-        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;
-        }
+        // 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/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
@@ -126,9 +126,6 @@
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS             BUS_I2C2
 
-#define USE_OPTICAL_FLOW
-#define USE_OPFLOW_CXOF
-
 #define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS