diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0bdb9e3375f..d53c9ffc038 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6359,6 +6359,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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index e55aeb22461..0f77de99fdc 100644 --- a/src/main/io/osd.h +++ b/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" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 03d4a3da36b..ed4fdf3bf38 100644 --- a/src/main/navigation/navigation.c +++ b/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)); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index eb1621e9f8d..44e59898201 100644 --- a/src/main/navigation/navigation.h +++ b/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; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 5d4b7a5b425..602574dbea2 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/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) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index a1f07e470c0..cf444e263b3 100644 --- a/src/main/navigation/navigation_private.h +++ b/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);