CR141 + CR142

Author: breadoven

docs/Settings.md

@@ -4022,6 +4022,16 @@ P gain of altitude PID controller (Multirotor)
 
 ---
 
+### nav_mc_toiletbowl_detection
+
+Sets sensitivity of toilet bowling detection for multirotors. On detection a heading correction is applied which should stop the toilet bowling. A setting of 2 works well for 5 inch multirotors. Increasing the setting will reduce sensitivity and delay detection. Set to 0 to disable. (Toilet bowling occurs most obviously during position hold when an inaccurate compass heading causes a rapidly increasing fast sweeping bowl shaped flight path).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 20 |
+
+---
+
 ### nav_mc_vel_xy_d
 
 D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target.

src/main/io/osd.c

@@ -5989,15 +5989,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
         /* WARNING: ensure number of messages returned does not exceed messages array size
          * Messages array set 1 larger than maximum expected message count of 6 */
-        const char *messages[7];
+        const char *messages[8];
         unsigned messageCount = 0;
 
         const char *failsafeInfoMessage = NULL;
         const char *invertedInfoMessage = NULL;
 
         if (ARMING_FLAG(ARMED)) {
             if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */
+                /* ADDS MAXIMUM OF 4 MESSAGES TO TOTAL NORMALLY, 6 MESSAGES DURING FAILSAFE */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -6063,7 +6063,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else {
                 /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
-                /* ADDS MAXIMUM OF 4 MESSAGES TO TOTAL */
+                /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */
 #ifdef USE_GEOZONE
                 char buf[12], buf1[12];
                 switch (geozone.messageState) {
@@ -6181,8 +6181,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     }
                 }
             }
+            // CR142
+            if (posControl.flags.posEstimateDegraded) {
+                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_POSEST_DEGRADED);
+            }
+            // CR142
             // CR141
-            if (STATE(MULTIROTOR) && isNavHoldPositionActive() && toiletBowlingHeadingCorrection) {
+            if (STATE(MULTIROTOR) && mcToiletBowlingHeadingCorrection) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_TOILET_BOWL);
             }
             // CR141

src/main/io/osd.h

@@ -123,7 +123,9 @@
 #define OSD_MSG_ANGLEHOLD_PITCH     "(ANGLEHOLD PITCH)"
 #define OSD_MSG_ANGLEHOLD_LEVEL     "(ANGLEHOLD LEVEL)"
 #define OSD_MSG_MOVE_STICKS         "MOVE STICKS TO ABORT"
-#define OSD_MSG_TOILET_BOWL         "TOILET BOWLING !"   // CR141
+#define OSD_MSG_TOILET_BOWL         "!TOILET BOWLING!"   // CR141
+#define OSD_MSG_POSEST_DEGRADED     "!POSITION ACCURACY POOR!"   // CR142
+
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"

src/main/navigation/navigation.c

@@ -253,7 +253,7 @@ static navWapointHeading_t wpHeadingControl;
 navigationPosControl_t posControl;
 navSystemStatus_t NAV_Status;
 static bool landingDetectorIsActive = false;
-int16_t toiletBowlingHeadingCorrection;    // Indicates toilet bowling detected multirotor // CR141
+int16_t mcToiletBowlingHeadingCorrection;    // Indicates toilet bowling detected multirotor // CR141
 
 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 
@@ -1291,6 +1291,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState
     resetAltitudeController(false);
     resetHeadingController();
     resetPositionController();
+    mcToiletBowlingHeadingCorrection = 0;  // CR141
 #ifdef USE_FW_AUTOLAND
     resetFwAutoland();
 #endif
@@ -2921,9 +2922,9 @@ void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroun
     if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
         newHeading = wrap_36000(newHeading + 9000);
     } else {
-        toiletBowlingHeadingCorrection = 0;
+        mcToiletBowlingHeadingCorrection = 0;
     }
-    // imuNavCompassSanity(toiletBowlingHeadingCorrection == 0);
+    // imuNavCompassSanity(mcToiletBowlingHeadingCorrection == 0);
     // CR141
     navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
 

src/main/navigation/navigation_multicopter.c

@@ -570,50 +570,54 @@ static float computeVelocityScale(
 // CR141
 static void checkForToiletBowling(void)
 {
-    bool isHoldingPosition = ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.cruise.multicopterSpeed < 50) || navGetCurrentStateFlags() & NAV_CTL_HOLD);
+    // bool isHoldingPosition = ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.cruise.multicopterSpeed < 50) || navGetCurrentStateFlags() & NAV_CTL_HOLD);
+    bool isHoldingPosition = FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || navGetCurrentStateFlags() & NAV_CTL_HOLD;
+    uint16_t distanceToHoldPoint = calculateDistanceToDestination(&posControl.desiredState.pos);
 
-    if (navConfig()->mc.toiletbowl_detection == 0 || !isHoldingPosition || posControl.flags.isAdjustingPosition) {
-        // toiletBowlingHeadingCorrection = 0;
+    if (posControl.actualState.velXY < 100 || distanceToHoldPoint < 100 || !isHoldingPosition) {  // || posControl.flags.isAdjustingPosition) {
         return;
     }
 
     static timeMs_t startTime = 0;
 
-    uint16_t courseToHoldPoint = calculateBearingToDestination(&posControl.desiredState.pos);
-    int16_t courseError = wrap_18000(courseToHoldPoint - 10 * gpsSol.groundCourse);
+    int16_t courseError = wrap_18000(calculateBearingToDestination(&posControl.desiredState.pos) - 10 * gpsSol.groundCourse);
     bool courseErrorCheck = ABS(courseError) > 3000 && ABS(courseError) < 15500;
 
-    uint16_t distanceToHoldPoint = calculateDistanceToDestination(&posControl.desiredState.pos);
     bool distanceSpeedCheck = posControl.actualState.velXY * distanceToHoldPoint > (navConfig()->mc.toiletbowl_detection * 10000);
 
-    DEBUG_SET(DEBUG_ALWAYS, 0, courseError);
-    DEBUG_SET(DEBUG_ALWAYS, 1, distanceToHoldPoint);
-    DEBUG_SET(DEBUG_ALWAYS, 2, posControl.actualState.velXY * distanceToHoldPoint);
-    DEBUG_SET(DEBUG_ALWAYS, 3, toiletBowlingHeadingCorrection);
-    DEBUG_SET(DEBUG_ALWAYS, 5, posControl.actualState.velXY);
-    DEBUG_SET(DEBUG_ALWAYS, 6, courseToHoldPoint);
-    DEBUG_SET(DEBUG_ALWAYS, 7, posControl.actualState.yaw);
-
-    if (toiletBowlingHeadingCorrection) {
-        uint16_t correctedHeading = wrap_36000(posControl.actualState.yaw - 0.67 * toiletBowlingHeadingCorrection);
-        posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(correctedHeading));
-        posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(correctedHeading));
-        DEBUG_SET(DEBUG_ALWAYS, 4, correctedHeading);
-    } else if (posControl.actualState.velXY > 100 && distanceToHoldPoint > 100 && courseErrorCheck  && distanceSpeedCheck) {
+    if (courseErrorCheck && distanceSpeedCheck) {
         if (startTime == 0) {
             startTime = millis();
         } else if (millis() - startTime > 1000) {
-            // Try to correct heading error
-            toiletBowlingHeadingCorrection = courseError;
+            mcToiletBowlingHeadingCorrection = 0.67 * courseError;   // Try to correct heading error
         }
     } else {
         startTime = 0;
     }
+
+    // DEBUG_SET(DEBUG_ALWAYS, 0, courseError);
+    // DEBUG_SET(DEBUG_ALWAYS, 2, posControl.actualState.velXY * distanceToHoldPoint);
 }
 // CR141
 static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit)  // , const float maxSpeed)   CR47
 {
-    checkForToiletBowling();    // CR141
+    // CR141
+    // DEBUG_SET(DEBUG_ALWAYS, 1, calculateDistanceToDestination(&posControl.desiredState.pos));
+    // DEBUG_SET(DEBUG_ALWAYS, 3, mcToiletBowlingHeadingCorrection);
+    // DEBUG_SET(DEBUG_ALWAYS, 5, posControl.actualState.velXY);
+    // DEBUG_SET(DEBUG_ALWAYS, 6, calculateBearingToDestination(&posControl.desiredState.pos));
+    // DEBUG_SET(DEBUG_ALWAYS, 7, posControl.actualState.yaw);
+    if (navConfig()->mc.toiletbowl_detection) {
+        if (mcToiletBowlingHeadingCorrection) {
+            uint16_t correctedHeading = wrap_36000(posControl.actualState.yaw - mcToiletBowlingHeadingCorrection);
+            posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(correctedHeading));
+            posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(correctedHeading));
+            // DEBUG_SET(DEBUG_ALWAYS, 4, correctedHeading);
+        } else {
+            checkForToiletBowling();
+        }
+    }
+    // CR141
 
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
     const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
@@ -905,7 +909,11 @@ bool isMulticopterLandingDetected(void)
     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
                           || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && throttleIsBelowMidHover)
                           || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
-
+    // CR137
+    if (FLIGHT_MODE(NAV_RTH_MODE)) {
+        startCondition = startCondition && posControl.actualState.abs.pos.z < 3000;
+    }
+    // CR137
     static timeMs_t landingDetectorStartedAt;
 
     if (!startCondition || posControl.flags.resetLandingDetector) {

src/main/navigation/navigation_pos_estimator.c

@@ -406,6 +406,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
         posEstimator.imu.accelNEU.y = accelReading.y + posEstimator.imu.accelBias.y;
         posEstimator.imu.accelNEU.z = accelReading.z + posEstimator.imu.accelBias.z;
         // CR140
+        // CR142
+        if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
+            posEstimator.imu.accelNEU.z += -navConfig()->mc.max_auto_climb_rate;
+        }
+        // CR142
 
         /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
         if (gyroConfig()->init_gyro_cal_enabled) {
@@ -530,7 +535,7 @@ static void estimationPredict(estimationContext_t * ctx)
         // }
         // CR131
         // DEBUG_SET(DEBUG_ALWAYS, 7, posEstimator.est.vel.z);
-        // DEBUG_SET(DEBUG_ALWAYS, 7, posEstimator.imu.accelNEU.z);
+        DEBUG_SET(DEBUG_ALWAYS, 7, posEstimator.imu.accelNEU.z);
     }
 
     /* Prediction step: XY-axis */
@@ -707,9 +712,8 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
     return false;
 }
 
-static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
+static void estimationCalculateGroundCourse(void)
 {
-    UNUSED(currentTimeUs);
     if ((STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
@@ -719,7 +723,34 @@ static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
         posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
     }
 }
+// CR142
+static void checkEstimateSanity(timeMs_t currentTimeMs)
+{
+    static timeMs_t startTime[3];
+    static bool useGpsRecovery = false;
+    uint8_t axis;
+
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        bool estimateSanityDegraded = ABS(posEstimator.est.vel.v[axis] - posEstimator.gps.vel.v[axis]) > 200 ||
+                                      ABS(posEstimator.est.pos.v[axis] - posEstimator.gps.pos.v[axis]) > 1000;
+
+        if (estimateSanityDegraded || useGpsRecovery) {
+            if (startTime[axis] == 0) {
+                startTime[axis] = currentTimeMs;
+            } else if (currentTimeMs - startTime[axis] > 3000) {
+                posEstimator.imu.accelBias.v[axis] = 0;
+                posEstimator.est.vel.v[axis] = posEstimator.gps.vel.v[axis];
+                posEstimator.est.pos.v[axis] = posEstimator.gps.pos.v[axis];
+                useGpsRecovery = currentTimeMs - startTime[axis] < 13000;
+            }
+        } else {
+            startTime[axis] = 0;
+        }
 
+        posControl.flags.posEstimateDegraded = useGpsRecovery;
+    }
+}
+// CR142
 /**
  * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
  *  Function is called at main loop rate
@@ -780,7 +811,11 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     // Apply corrections
     vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
-
+    // CR142
+    if (ctx.newFlags & EST_XY_VALID && ctx.newFlags & EST_GPS_XY_VALID && ctx.newFlags & EST_Z_VALID && ctx.newFlags & EST_GPS_Z_VALID) {
+        checkEstimateSanity(US2MS(currentTimeUs));
+    }
+    // CR142
     /* Correct accelerometer bias */
     const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
     if (w_acc_bias > 0.0f) {
@@ -796,7 +831,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     }
     // CR140
     /* Update ground course */
-    estimationCalculateGroundCourse(currentTimeUs);
+    estimationCalculateGroundCourse();
 
     /* Update uncertainty */
     posEstimator.est.eph = ctx.newEPH;
@@ -805,13 +840,13 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     // Keep flags for further usage
     posEstimator.flags = ctx.newFlags;
 
-    // DEBUG_SET(DEBUG_ALWAYS, 0, posEstimator.imu.accelBias.z * 1000);
-    // DEBUG_SET(DEBUG_ALWAYS, 1, ctx.accBiasCorr.z);
-    // DEBUG_SET(DEBUG_ALWAYS, 2, ctx.estPosCorr.z * 1000);
-    // DEBUG_SET(DEBUG_ALWAYS, 3, ctx.estVelCorr.z * 1000);
-    // DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.imu.accWeightFactor * 100);
-    // DEBUG_SET(DEBUG_ALWAYS, 5, posEstimator.baro.baroAltRate);
-    // DEBUG_SET(DEBUG_ALWAYS, 6, posEstimator.baro.alt);
+    DEBUG_SET(DEBUG_ALWAYS, 0, posEstimator.imu.accelBias.z * 1000);
+    DEBUG_SET(DEBUG_ALWAYS, 1, ctx.accBiasCorr.z);
+    DEBUG_SET(DEBUG_ALWAYS, 2, ctx.estPosCorr.z * 1000);
+    DEBUG_SET(DEBUG_ALWAYS, 3, ctx.estVelCorr.z * 1000);
+    DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.imu.accWeightFactor * 100);
+    DEBUG_SET(DEBUG_ALWAYS, 5, posEstimator.baro.baroAltRate);
+    DEBUG_SET(DEBUG_ALWAYS, 6, posEstimator.baro.alt);
 }
 
 /**

src/main/navigation/navigation_private.h

@@ -90,6 +90,8 @@ typedef struct navigationFlags_s {
     navigationEstimateStatus_e estAglStatus;
     navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane
     bool gpsCfEstimatedAltitudeMismatch;            // Indicates a mismatch between GPS altitude and estimated altitude
+    
+    bool posEstimateDegraded;                       // CR142
 
     climbRateToAltitudeControllerMode_e rocToAltMode;
 
@@ -527,7 +529,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
 
 extern navigationPosControl_t posControl;
 extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
-extern int16_t toiletBowlingHeadingCorrection;    // Indicates toilet bowling detected multirotor // CR141
+extern int16_t mcToiletBowlingHeadingCorrection;    // Indicates toilet bowling detected multirotor // CR141
 
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);