Refactor position estimation code. Add support for optic flow for position estimation

make/source.mk

@@ -163,6 +163,8 @@ COMMON_SRC = \
             navigation/navigation_geo.c \
             navigation/navigation_multicopter.c \
             navigation/navigation_pos_estimator.c \
+            navigation/navigation_pos_estimator_agl.c \
+            navigation/navigation_pos_estimator_flow.c \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \

src/main/build/debug.h

@@ -54,6 +54,7 @@ typedef enum {
     DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
     DEBUG_AGL,
     DEBUG_FLOW_RAW,
+    DEBUG_FLOW,
     DEBUG_SBUS,
     DEBUG_FPORT,
     DEBUG_ALWAYS,

src/main/common/vector.h

@@ -41,6 +41,13 @@ typedef struct {
 void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
 void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
 
+static inline void vectorZero(fpVector3_t * v)
+{ 
+    v->x = 0.0f;
+    v->y = 0.0f;
+    v->z = 0.0f;
+}
+
 static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
 {
     fpVector3_t r;

src/main/drivers/opflow/opflow.h

@@ -31,7 +31,7 @@ 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)
+    int32_t     flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
     int16_t     quality;
 } opflowData_t;
 

src/main/drivers/opflow/opflow_fake.c

@@ -43,6 +43,7 @@ void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY,
     fakeData.deltaTime = deltaTime;
     fakeData.flowRateRaw[0] = flowRateX;
     fakeData.flowRateRaw[1] = flowRateY;
+    fakeData.flowRateRaw[2] = 0;
     fakeData.quality = quality;
 }
 

src/main/fc/fc_msp_box.c

@@ -182,12 +182,21 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
     }
 
-    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    const bool navReadyQuads = !STATE(FIXED_WING) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
+    if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
+    }
+
+    if (navReadyQuads || navReadyPlanes) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
         activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
         activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
-        activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
+
+        if (feature(FEATURE_GPS)) {
+            activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
+        }
     }
 #endif
 
@@ -323,7 +332,7 @@ uint16_t packSensorStatus(void)
             IS_ENABLED(sensors(SENSOR_MAG))     << 2 |
             IS_ENABLED(sensors(SENSOR_GPS))     << 3 |
             IS_ENABLED(sensors(SENSOR_RANGEFINDER))   << 4 |
-            //IS_ENABLED(sensors(SENSOR_OPFLOW))  << 5 |
+            IS_ENABLED(sensors(SENSOR_OPFLOW))  << 5 |
             IS_ENABLED(sensors(SENSOR_PITOT))   << 6;
 
     // Hardware failure indication bit

src/main/fc/settings.yaml

@@ -67,7 +67,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -231,6 +231,9 @@ groups:
     members:
       - name: opflow_hardware
         table: opflow_hardware
+      - name: opflow_align
+        type: uint8_t
+        table: alignment
       - name: opflow_scale
         min: 0
         max: 10000
@@ -1082,6 +1085,9 @@ groups:
       - name: inav_use_gps_velned
         field: use_gps_velned
         type: bool
+      - name: inav_allow_dead_reckoning
+        field: allow_dead_reckoning
+        type: bool
       - name: inav_reset_altitude
         field: reset_altitude_type
         table: reset_type
@@ -1100,6 +1106,14 @@ groups:
         field: w_z_surface_v
         min: 0
         max: 10
+      - name: inav_w_xy_flow_p
+        field: w_xy_flow_p
+        min: 0
+        max: 10
+      - name: inav_w_xy_flow_v
+        field: w_xy_flow_v
+        min: 0
+        max: 10
       - name: inav_w_z_baro_p
         field: w_z_baro_p
         min: 0

src/main/io/opflow_cxof.c

@@ -113,6 +113,7 @@ static bool cxofOpflowUpdate(opflowData_t * data)
                     tmpData.deltaTime += (currentTimeUs - previousTimeUs);
                     tmpData.flowRateRaw[0] += pkt->motionX;
                     tmpData.flowRateRaw[1] += pkt->motionY;
+                    tmpData.flowRateRaw[2] = 0;
                     tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;
 
                     previousTimeUs = currentTimeUs;

src/main/io/opflow_msp.c

@@ -81,6 +81,7 @@ void mspOpflowReceiveNewData(uint8_t * bufferPtr)
     sensorData.deltaTime = currentTimeUs - updatedTimeUs;
     sensorData.flowRateRaw[0] = pkt->motionX;
     sensorData.flowRateRaw[1] = pkt->motionY;
+    sensorData.flowRateRaw[2] = 0;
     sensorData.quality = (int)pkt->quality * 100 / 255;
     hasNewData = true;
 

src/main/navigation/navigation.c

@@ -173,7 +173,6 @@ static void setupAltitudeController(void);
 static void resetHeadingController(void);
 void resetGCSFlags(void);
 
-static bool posEstimationHasGlobalReference(void);
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
@@ -750,7 +749,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 {
     navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
 
-    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
+    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
         // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@@ -1440,7 +1439,7 @@ bool checkForPositionSensorTimeout(void)
 /*-----------------------------------------------------------
  * Processes an update to XY-position and velocity
  *-----------------------------------------------------------*/
-void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
+void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
 {
     posControl.actualState.abs.pos.x = newX;
     posControl.actualState.abs.pos.y = newY;
@@ -1454,13 +1453,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
 
     posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
 
-    if (estimateValid) {
+    // CASE 1: POS & VEL valid
+    if (estPosValid && estVelValid) {
         posControl.flags.estPosStatus = EST_TRUSTED;
+        posControl.flags.estVelStatus = EST_TRUSTED;
+        posControl.flags.horizontalPositionDataNew = 1;
+        posControl.lastValidPositionTimeMs = millis();
+    }
+    // CASE 1: POS invalid, VEL valid
+    else if (!estPosValid && estVelValid) {
+        posControl.flags.estPosStatus = EST_USABLE;     // Pos usable, but not trusted
+        posControl.flags.estVelStatus = EST_TRUSTED;
         posControl.flags.horizontalPositionDataNew = 1;
         posControl.lastValidPositionTimeMs = millis();
     }
+    // CASE 3: can't use pos/vel data
     else {
         posControl.flags.estPosStatus = EST_NONE;
+        posControl.flags.estVelStatus = EST_NONE;
         posControl.flags.horizontalPositionDataNew = 0;
     }
 
@@ -2089,7 +2099,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     // WP #255 - special waypoint - directly set desiredPosition
     // Only valid when armed and in poshold mode
     else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
-             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
+             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
              (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
         // Convert to local coordinates
         geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@@ -2297,7 +2307,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 #if defined(NAV_BLACKBOX)
     navFlags = 0;
     if (posControl.flags.estAltStatus == EST_TRUSTED)       navFlags |= (1 << 0);
-    if (posControl.flags.estAglStatus == EST_TRUSTED)   navFlags |= (1 << 1);
+    if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
 #if defined(NAV_GPS_GLITCH_DETECTION)
     if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
@@ -2368,12 +2378,12 @@ static bool canActivateAltHoldMode(void)
 
 static bool canActivatePosHoldMode(void)
 {
-    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
+    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }
 
-static bool posEstimationHasGlobalReference(void)
+static bool canActivateNavigationModes(void)
 {
-    return true;    // For now assume that we always have global coordinates available
+    return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }
 
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@@ -2389,8 +2399,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
-        bool canActivateAltHold = canActivateAltHoldMode();
-        bool canActivatePosHold = canActivatePosHoldMode();
+        bool canActivateAltHold    = canActivateAltHoldMode();
+        bool canActivatePosHold    = canActivatePosHoldMode();
+        bool canActivateNavigation = canActivateNavigationModes();
 
         // LAUNCH mode has priority over any other NAV mode
         if (STATE(FIXED_WING)) {
@@ -2417,7 +2428,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         // RTH/Failsafe_RTH can override MANUAL
-        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
+        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
             // If we request forced RTH - attempt to activate it no matter what
             // This might switch to emergency landing controller if GPS is unavailable
             canActivateWaypoint = false;    // Block WP mode if we switched to RTH for whatever reason
@@ -2431,7 +2442,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
-            if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
+            if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
                 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
         }
         else {
@@ -2670,6 +2681,7 @@ void navigationInit(void)
 
     posControl.flags.estAltStatus = EST_NONE;
     posControl.flags.estPosStatus = EST_NONE;
+    posControl.flags.estVelStatus = EST_NONE;
     posControl.flags.estHeadingStatus = EST_NONE;
     posControl.flags.estAglStatus = EST_NONE;
 

src/main/navigation/navigation.h

@@ -80,6 +80,7 @@ typedef struct positionEstimationConfig_s {
     uint8_t reset_home_type; // nav_reset_type_e
     uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
     uint8_t use_gps_velned;
+    uint8_t allow_dead_reckoning;
 
     uint16_t max_surface_altitude;
 
@@ -94,6 +95,9 @@ typedef struct positionEstimationConfig_s {
     float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
     float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
 
+    float w_xy_flow_p;
+    float w_xy_flow_v;
+
     float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
     float w_xy_res_v;
 
@@ -279,9 +283,20 @@ bool navIsCalibrationComplete(void);
 bool navigationTerrainFollowingEnabled(void);
 
 /* Access to estimated position and velocity */
+typedef struct {
+    uint8_t altStatus;
+    uint8_t posStatus;
+    uint8_t velStatus;
+    uint8_t aglStatus;
+    fpVector3_t pos;
+    fpVector3_t vel;
+    float agl;
+} navPositionAndVelocity_t;
+
 float getEstimatedActualVelocity(int axis);
 float getEstimatedActualPosition(int axis);
 int32_t getTotalTravelDistance(void);
+void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
 
 /* Waypoint list access functions */
 int getWaypointCount(void);