Refactor position estimation code

Remove position estimate history code Remove rangefinder median filtering Change VEL_XY PID to allow more authority
iNavFlight · Dec 22, 2017 · a60e9f6 · a60e9f6
1 parent f3422ce
commit a60e9f6
Show file tree

Hide file tree

Showing 11 changed files with 128 additions and 182 deletions.
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
@@ -175,6 +175,7 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
     }
+
     if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
@@ -201,8 +201,6 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 #ifdef USE_OPTICAL_FLOW
 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
-
     if (!sensors(SENSOR_OPFLOW))
         return;
 

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -206,6 +206,9 @@ groups:
     members:
       - name: rangefinder_hardware
         table: rangefinder_hardware
+      - name: rangefinder_median_filter
+        field: use_median_filtering
+        type: bool
 
   - name: PG_OPFLOW_CONFIG
     type: opticalFlowConfig_t
@@ -260,7 +263,7 @@ groups:
     members:
       - name: baro_hardware
         table: baro_hardware
-      - name: baro_use_median_filter
+      - name: baro_median_filter
         field: use_median_filtering
         type: bool
 
@@ -976,10 +979,6 @@ groups:
       - name: inav_use_gps_velned
         field: use_gps_velned
         type: bool
-      - name: inav_gps_delay
-        field: gps_delay_ms
-        min: 0
-        max: 500
       - name: inav_reset_altitude
         field: reset_altitude_type
         table: reset_altitude

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -126,7 +126,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .D = 10,   // posResponseExpo * 100
                 },
                 [PID_VEL_XY] = {
-                    .P = 180,  // NAV_VEL_XY_P * 100
+                    .P = 40,   // NAV_VEL_XY_P * 20
                     .I = 15,   // NAV_VEL_XY_I * 100
                     .D = 100,  // NAV_VEL_XY_D * 100
                 },

diff --git a/src/main/io/opflow_cxof.c b/src/main/io/opflow_cxof.c
@@ -48,13 +48,13 @@ static uint8_t  buffer[10];
 static int      bufferPtr;
 
 typedef struct __attribute__((packed)) {
-    uint8_t     header;
-    uint8_t     res0;
+    uint8_t     header;     // 0xFE
+    uint8_t     res0;       // Seems to be 0x04 all the time
     int16_t     motionX;
     int16_t     motionY;
-    int8_t      motionT;
-    uint8_t     squal;
-    uint8_t     footer;
+    int8_t      motionT;    // ???
+    uint8_t     squal;      // Not sure about this
+    uint8_t     footer;     // 0xAA
 } cxofPacket_t;
 
 static bool cxofOpflowDetect(void)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -171,6 +171,7 @@ static void setupAltitudeController(void);
 void resetNavigation(void);
 void resetGCSFlags(void);
 
+static bool posEstimationHasGlobalReference(void);
 static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos);
 void calculateInitialHoldPosition(t_fp_vector * pos);
@@ -790,8 +791,9 @@ 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)) {
+    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
         // 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;
     }
 
@@ -1477,8 +1479,6 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
         posControl.flags.horizontalPositionDataNew = 0;
     }
 
-    posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE;
-
 #if defined(NAV_BLACKBOX)
     navLatestActualPosition[X] = newX;
     navLatestActualPosition[Y] = newY;
@@ -1534,18 +1534,19 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
     }
 
 #if defined(NAV_BLACKBOX)
-    navLatestActualPosition[Z] = constrain(newAltitude, -32678, 32767);
-    navActualVelocity[Z] = constrain(newVelocity, -32678, 32767);
+    navLatestActualPosition[Z] = constrain(posControl.actualState.pos.V.Z, -32678, 32767);
+    navActualVelocity[Z] = constrain(posControl.actualState.vel.V.Z, -32678, 32767);
 #endif
 }
 
 /*-----------------------------------------------------------
  * Processes an update to estimated heading
  *-----------------------------------------------------------*/
-void updateActualHeading(int32_t newHeading)
+void updateActualHeading(bool headingValid, int32_t newHeading)
 {
     /* Update heading */
     posControl.actualState.yaw = newHeading;
+    posControl.flags.estHeadingStatus = headingValid ? EST_TRUSTED : EST_NONE;
 
     /* Precompute sin/cos of yaw angle */
     posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
@@ -2319,6 +2320,11 @@ static bool canActivatePosHoldMode(void)
     return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }
 
+static bool posEstimationHasGlobalReference(void)
+{
+    return true;    // For now assume that we always have global coordinates available
+}
+
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
 {
     static bool canActivateWaypoint = false;
@@ -2370,7 +2376,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
-            if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
+            if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
                 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
         }
         else {
@@ -2571,7 +2577,7 @@ void navigationUsePIDs(void)
     for (int axis = 0; axis < 2; axis++) {
         navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f);
 
-        navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 100.0f,
+        navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f);
     }

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -76,7 +76,6 @@ typedef struct positionEstimationConfig_s {
     uint8_t reset_altitude_type;
     uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
     uint8_t use_gps_velned;
-    uint16_t gps_delay_ms;
 
     uint16_t max_surface_altitude;