Multirotor toilet bowling detection

docs/Settings.md

@@ -4052,6 +4052,16 @@ P gain of altitude PID controller (Multirotor)
 
 ---
 
+### nav_mc_toiletbowl_detection
+
+Sets the sensitivity of toilet bowling detection for multirotors and whether a heading correction is applied on detection which should stop the toilet bowling occuring. Sensitivity can be set from 1 to 9. A setting of 2 works well for 5 inch multirotors. Increasing the setting will reduce sensitivity and delay detection. If a heading correction is required on detection the sensitivity setting should be multiplied by 10, e.g. set to 20 for a sensitivity of 2 with heading correction on detection. If no heading correction is required, e.g. setting of 2, then toilet bowling detection will be indicated only by an OSD system message. Set to 0 to disable detection entirely.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 90 |
+
+---
+
 ### 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/fc/settings.yaml

@@ -2571,6 +2571,12 @@ groups:
         field: mc.inverted_crash_detection
         min: 0
         max: 15
+      - name: nav_mc_toiletbowl_detection
+        description: "Sets the sensitivity of toilet bowling detection for multirotors and whether a heading correction is applied on detection which should stop the toilet bowling occuring. Sensitivity can be set from 1 to 9. A setting of 2 works well for 5 inch multirotors. Increasing the setting will reduce sensitivity and delay detection. If a heading correction is required on detection the sensitivity setting should be multiplied by 10, e.g. set to 20 for a sensitivity of 2 with heading correction on detection. If no heading correction is required, e.g. setting of 2, then toilet bowling detection will be indicated only by an OSD system message. Set to 0 to disable detection entirely."
+        default_value: 0
+        field: mc.toiletbowl_detection
+        min: 0
+        max: 90
       - name: nav_mc_althold_throttle
         description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
         default_value: "STICK"

src/main/io/osd.c

@@ -6213,66 +6213,66 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             } else if (STATE(LANDING_DETECTED)) {
                 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 3 MESSAGES TO TOTAL */
 #ifdef USE_GEOZONE
-            char buf[12], buf1[12];
-            switch (geozone.messageState) {
-                case GEOZONE_MESSAGE_STATE_NFZ:
-                    messages[messageCount++] = OSD_MSG_NFZ;
-                    break;
-                case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
+                char buf[12], buf1[12];
+                switch (geozone.messageState) {
+                    case GEOZONE_MESSAGE_STATE_NFZ:
+                        messages[messageCount++] = OSD_MSG_NFZ;
+                        break;
+                    case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
+                        osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
+                        tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
+                        messages[messageCount++] = messageBuf;
+                        break;
+                    case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
+                        messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
+                        break;
+                    case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
                     osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
-                    tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
-                    messages[messageCount++] = messageBuf;
-                    break;
-                case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
-                    messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
-                    break;
-                case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
-                osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
-                    if (geozone.zoneInfo == INT32_MAX) {
-                        tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
-                    } else {
-                        osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
-                    }
-                    tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
-                    messages[messageCount++] = messageBuf;
-                    break;
-                case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
-                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
-                    if (!posControl.sendTo.lockSticks) {
-                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
-                    }
-                    break;
-                case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
-                    messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
-                    if (!posControl.sendTo.lockSticks) {
-                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
-                    }
-                    break;
-                case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
-                    messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
-                    if (!posControl.sendTo.lockSticks) {
-                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
-                    }
-                    break;
-                case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
-                    messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
-                    if (!posControl.sendTo.lockSticks) {
-                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
-                    }
-                    break;
-                case GEOZONE_MESSAGE_STATE_POS_HOLD:
-                    messages[messageCount++] = OSD_MSG_AVOIDING_FB;
-                    if (!geozone.sticksLocked) {
-                        messages[messageCount++] = OSD_MSG_MOVE_STICKS;
-                    }
-                    break;
-                case GEOZONE_MESSAGE_STATE_NONE:
-                    break;
-            }
+                        if (geozone.zoneInfo == INT32_MAX) {
+                            tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
+                        } else {
+                            osdFormatAltitudeSymbol(buf1, geozone.zoneInfo);
+                        }
+                        tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1);
+                        messages[messageCount++] = messageBuf;
+                        break;
+                    case GEOZONE_MESSAGE_STATE_AVOIDING_FB:
+                        messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                        if (!posControl.sendTo.lockSticks) {
+                            messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                        }
+                        break;
+                    case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE:
+                        messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE;
+                        if (!posControl.sendTo.lockSticks) {
+                            messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                        }
+                        break;
+                    case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH:
+                        messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH;
+                        if (!posControl.sendTo.lockSticks) {
+                            messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                        }
+                        break;
+                    case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ:
+                        messages[messageCount++] = OSD_MSG_FLYOUT_NFZ;
+                        if (!posControl.sendTo.lockSticks) {
+                            messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                        }
+                        break;
+                    case GEOZONE_MESSAGE_STATE_POS_HOLD:
+                        messages[messageCount++] = OSD_MSG_AVOIDING_FB;
+                        if (!geozone.sticksLocked) {
+                            messages[messageCount++] = OSD_MSG_MOVE_STICKS;
+                        }
+                        break;
+                    case GEOZONE_MESSAGE_STATE_NONE:
+                        break;
+                }
 #endif
-                /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
-                /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
                 if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandingBeCancelled()) {
@@ -6309,7 +6309,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
-
                 } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {
@@ -6332,6 +6331,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     }
                 }
             }
+            if (STATE(MULTIROTOR) && mcToiletBowlingHeadingCorrection) {
+                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_TOILET_BOWL);
+            }
         } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
             unsigned invalidIndex;
 

src/main/io/osd.h

@@ -122,6 +122,7 @@
 #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 DETECTED!"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"

src/main/navigation/navigation.c

@@ -201,6 +201,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
         .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT,                        // STICK
         .inverted_crash_detection = SETTING_NAV_MC_INVERTED_CRASH_DETECTION_DEFAULT,             // 0 - disarm time delay for inverted crash detection
+        .toiletbowl_detection = SETTING_NAV_MC_TOILETBOWL_DETECTION_DEFAULT,                     // 0 - sensitivity factor for toilet bowling detection
     },
 
     // Fixed wing
@@ -252,6 +253,7 @@ static navWapointHeading_t wpHeadingControl;
 navigationPosControl_t posControl;
 navSystemStatus_t NAV_Status;
 static bool landingDetectorIsActive;
+int16_t mcToiletBowlingHeadingCorrection;    // Indicates toilet bowling detected multirotor
 
 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 

src/main/navigation/navigation.h

@@ -197,7 +197,7 @@ typedef struct geozone_s {
     int32_t distanceHorToNearestZone;
     int32_t distanceVertToNearestZone;
     int32_t zoneInfo;
-    int32_t currentzoneMaxAltitude; 
+    int32_t currentzoneMaxAltitude;
     int32_t currentzoneMinAltitude;
     bool nearestHorZoneHasAction;
     bool sticksLocked;
@@ -464,6 +464,7 @@ typedef struct navConfig_s {
         bool slowDownForTurning;                // Slow down during WP missions when changing heading on next waypoint
         uint8_t althold_throttle_type;          // throttle zero datum type for alt hold
         uint8_t inverted_crash_detection;       // Enables inverted crash detection, setting defines disarm time delay (0 = disabled)
+        uint8_t toiletbowl_detection;           // Enables toilet bowling detection and heading correction (0 = disabled)
     } mc;
 
     struct {

src/main/navigation/navigation_multicopter.c

@@ -554,8 +554,56 @@ static float computeVelocityScale(
     return constrainf(scale, 0, attenuationFactor);
 }
 
+static void checkForToiletBowling(void)
+{
+    bool isHoldingPosition = ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.cruise.multicopterSpeed < 50) || navGetCurrentStateFlags() & NAV_CTL_HOLD);
+    uint16_t distanceToHoldPoint = calculateDistanceToDestination(&posControl.desiredState.pos);
+
+    if (posControl.actualState.velXY < 100 || distanceToHoldPoint < 100 || !isHoldingPosition || (posControl.flags.isAdjustingPosition && navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI)) {
+
+        return;
+    }
+
+    /* Compare required course back to hold point against actual GPS CoG
+     * Toilet bowling likely if heading error > 30 dges. Also ignore errors > 155 degs -> could be caused by gusting wind */
+    int16_t courseError = wrap_18000(calculateBearingToDestination(&posControl.desiredState.pos) - 10 * gpsSol.groundCourse);
+    bool courseErrorCheck = ABS(courseError) > 3000 && ABS(courseError) < 15500;
+
+    uint8_t sensitivity = navConfig()->mc.toiletbowl_detection;
+    if (sensitivity > 9) {
+        sensitivity /= 10;
+    }
+
+    /* vel x distanceToHoldPoint provides good indication of toilet bowling with value rising rapidly when hold control is lost */
+    bool distanceSpeedCheck = posControl.actualState.velXY * distanceToHoldPoint > (sensitivity * 10000);
+
+    static timeMs_t startTime = 0;
+    if (courseErrorCheck && distanceSpeedCheck) {
+        if (startTime == 0) {
+            startTime = millis();
+        } else if (millis() - startTime > 1000) {
+            /* Set heading correction if check conditions exist > 1 sec. 2/3 of actual course error seems to work best */
+            mcToiletBowlingHeadingCorrection = 0.67 * courseError;
+        }
+    } else {
+        startTime = 0;
+    }
+}
+
 static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
 {
+    if (navConfig()->mc.toiletbowl_detection) {
+        if (mcToiletBowlingHeadingCorrection) {
+            if (navConfig()->mc.toiletbowl_detection > 9) {
+                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));
+            }
+        } else {
+            checkForToiletBowling();
+        }
+    }
+
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
     const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
 
@@ -993,6 +1041,7 @@ void calculateMulticopterInitialHoldPosition(fpVector3_t * pos)
 void resetMulticopterHeadingController(void)
 {
     updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
+    mcToiletBowlingHeadingCorrection = 0;
 }
 
 static void applyMulticopterHeadingController(void)

src/main/navigation/navigation_private.h

@@ -525,6 +525,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
 
 extern navigationPosControl_t posControl;
 extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
+extern int16_t mcToiletBowlingHeadingCorrection;
 
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);