Multirotor toilet bowling detection

docs/Settings.md

@@ -5936,6 +5936,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 | FAKE |  |
 | MSP |  |
 | DLVR-L10D |  |
+| MS5525 |  |
 
 ---
 

src/main/io/osd.c

@@ -2301,7 +2301,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_GPS_EXTRA_STATS:
         {
             gpsSetOsdMonRfWidgetEnabled(true);
-            
+
             // Collect satellite stats grouped by GNSS ID: 0,2,3,6
             uint8_t stats[4][2] = { {0,0}, {0,0}, {0,0}, {0,0} }; // [group][0]=total, [group][1]=good
             for (int i = 0; i < UBLOX_MAX_SIGNALS; ++i) {
@@ -4591,7 +4591,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
     osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
-    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);    
+    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_EXTRA_STATS] = OSD_POS(3, 11);
 
     osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
@@ -6463,6 +6463,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     }
                 }
             }
+            if (STATE(MULTIROTOR) && mcToiletBowlingDetected) {
+                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

@@ -124,6 +124,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

@@ -3874,10 +3874,10 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
 
 int isGCSValid(void)
 {
-    return (ARMING_FLAG(ARMED) && 
-            (posControl.flags.estPosStatus >= EST_TRUSTED) && 
-            posControl.gpsOrigin.valid && 
-            posControl.flags.isGCSAssistedNavigationEnabled && 
+    return (ARMING_FLAG(ARMED) &&
+            (posControl.flags.estPosStatus >= EST_TRUSTED) &&
+            posControl.gpsOrigin.valid &&
+            posControl.flags.isGCSAssistedNavigationEnabled &&
             (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS));
 }
 

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;

src/main/navigation/navigation_multicopter.c

@@ -298,6 +298,7 @@ bool adjustMulticopterHeadingFromRCInput(void)
  * XY-position controller for multicopter aircraft
  *-----------------------------------------------------------*/
 static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f;
+bool mcToiletBowlingDetected;
 
 void resetMulticopterBrakingMode(void)
 {
@@ -563,8 +564,41 @@ 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 (!isHoldingPosition || posControl.actualState.velXY < 100 || distanceToHoldPoint < 100 || (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;
+
+    /* vel x distanceToHoldPoint provides good indication of toilet bowling with value rising rapidly when hold control is lost
+     * 30000 should provide a reliable detection threshold without excessive delay or false triggers */
+    bool distanceSpeedCheck = posControl.actualState.velXY * distanceToHoldPoint > (30000);
+
+    static timeMs_t startTime = 0;
+    if (courseErrorCheck && distanceSpeedCheck) {
+        if (startTime == 0) {
+            startTime = millis();
+        } else {
+            // Set detection flag if check conditions exist > 1 sec
+            mcToiletBowlingDetected = millis() - startTime > 1000;
+        }
+    } else {
+        startTime = 0;
+    }
+}
+
 static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
 {
+    if (!mcToiletBowlingDetected) checkForToiletBowling();
+
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
     const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
 
@@ -1010,6 +1044,7 @@ void calculateMulticopterInitialHoldPosition(fpVector3_t * pos)
 void resetMulticopterHeadingController(void)
 {
     updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
+    mcToiletBowlingDetected = false;
 }
 
 static void applyMulticopterHeadingController(void)

src/main/navigation/navigation_private.h

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