diff --git a/docs/Settings.md b/docs/Settings.md index af81a3c860c..7750369a90f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5936,6 +5936,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled. | FAKE | | | MSP | | | DLVR-L10D | | +| MS5525 | | --- diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8e7d5bef5fc..76cf6515468 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..a004b0420ca 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 d640e2b25c3..82d65761632 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 398781fa596..3bd62a72c00 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);