toilet bowling detection

Author: breadoven

docs/Settings.md

@@ -3992,6 +3992,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/fc/settings.yaml

@@ -2516,6 +2516,12 @@ groups:
         field: mc.inverted_crash_detection
         min: 0
         max: 15
+      - name: nav_mc_toiletbowl_detection
+        description: "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_value: 0
+        field: mc.toiletbowl_detection
+        min: 0
+        max: 20
       - 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

@@ -6156,6 +6156,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     }
                 }
             }
+            if (STATE(MULTIROTOR) && isNavHoldPositionActive() && toiletBowlingHeadingCorrection) {
+                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 !"
 
 #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 toiletBowlingHeadingCorrection;    // 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

@@ -483,6 +483,33 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
     return 1.0f - posControl.posResponseExpo * (1.0f - (velScale * velScale));  // x^3 expo factor
 }
 
+static void isToiletBowlingDetected(void)
+{
+    static timeMs_t startTime = 0;
+
+    uint16_t courseToHoldPoint = calculateBearingToDestination(&posControl.desiredState.pos);
+    int16_t courseError = wrap_18000(courseToHoldPoint - 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);
+
+    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));
+    } else if (posControl.actualState.velXY > 100 && distanceToHoldPoint > 100 && courseErrorCheck  && distanceSpeedCheck) {
+        if (startTime == 0) {
+            startTime = millis();
+        } else if (millis() - startTime > 1000) {
+            // Try to correct heading error
+            toiletBowlingHeadingCorrection = courseError;
+        }
+    } else {
+        startTime = 0;
+    }
+}
+
 static void updatePositionVelocityController_MC(const float maxSpeed)
 {
     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
@@ -498,6 +525,12 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
         }
     }
 
+    if (navConfig()->mc.toiletbowl_detection && isNavHoldPositionActive()) {
+        isToiletBowlingDetected();
+    } else {
+        toiletBowlingHeadingCorrection = 0;
+    }
+
     const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 

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 toiletBowlingHeadingCorrection;
 
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);