diff --git a/docs/Settings.md b/docs/Settings.md index ad03f282e3f..abf296f7781 100644 --- a/docs/Settings.md +++ b/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. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3e353e9dfcd..71275e6365d 100644 --- a/src/main/fc/settings.yaml +++ b/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" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..450ad653f1e 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..7977654dbe9 100644 --- a/src/main/io/osd.h +++ b/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" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e7f0774925e..29d97d54452 100644 --- a/src/main/navigation/navigation.c +++ b/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; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ddc7e23e76d..52f8cdd3066 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; @@ -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 { diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7da742829c5..7a154282483 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/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) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f6cf9329e98..01f0d6a2c66 100644 --- a/src/main/navigation/navigation_private.h +++ b/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);