From f7e10fe44e504ebdd247f45a4c8343bf87b16eeb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 1 Aug 2022 16:16:39 +0100 Subject: [PATCH] altitude tolerance fix --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 9 +++++---- src/main/io/osd.c | 6 +++--- src/main/navigation/navigation.c | 14 ++++++-------- src/main/navigation/navigation.h | 2 +- 5 files changed, 17 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8966d74d828..369dd645dea 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3884,11 +3884,11 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT ### nav_wp_enforce_altitude -Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. +Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. 0 = disabled, otherwise setting defines altitude capture tolerance [cm], e.g. 100 means required altitude is achieved when within 100cm of waypoint altitude setting. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 0 | 0 | 2000 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ae5d7d04ead..d5ef9afdc44 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2421,10 +2421,11 @@ groups: min: 10 max: 10000 - name: nav_wp_enforce_altitude - description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on." - default_value: OFF - field: general.flags.waypoint_enforce_altitude - type: bool + description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. 0 = disabled, otherwise setting defines altitude capture tolerance [cm], e.g. 100 means required altitude is achieved when within 100cm of waypoint altitude setting." + default_value: 0 + field: general.waypoint_enforce_altitude + min: 0 + max: 2000 - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." default_value: 10000 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a5b09925d0e..2a84045c34f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1098,7 +1098,7 @@ uint16_t osdGetRemainingGlideTime(void) { value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); glideTimeUpdatedMs = curTimeMs; - + if (value < 0) { value = osdGetAltitude() / abs((int)value); } else { @@ -2385,7 +2385,7 @@ static bool osdDrawSingleElement(uint8_t item) } break; } - case OSD_GLIDE_TIME_REMAINING: + case OSD_GLIDE_TIME_REMAINING: { uint16_t glideTime = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; @@ -4409,7 +4409,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { - if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); } else { // WP hold time countdown in seconds diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 56912d4b234..bafde139527 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,8 +61,6 @@ #include "sensors/boardalignment.h" #include "sensors/battery.h" -#define WP_ALTITUDE_MARGIN_CM 100 // WP enforce altitude tolerance, used when WP altitude setting enforced when WP reached - // Multirotors: #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) @@ -100,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -122,7 +120,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled - .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved .rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT // RTH trackback useage mode }, @@ -153,6 +150,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT, .rth_trackback_distance = SETTING_NAV_RTH_TRACKBACK_DISTANCE_DEFAULT, // Max distance allowed for RTH trackback + .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved }, // MC-specific @@ -1670,13 +1668,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga { UNUSED(previousState); - if (navConfig()->general.flags.waypoint_enforce_altitude) { + if (navConfig()->general.waypoint_enforce_altitude) { posControl.wpAltitudeReached = isWaypointAltitudeReached(); } switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; } else { return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT @@ -1709,7 +1707,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting if (STATE(AIRPLANE)) { int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; @@ -2243,7 +2241,7 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp bool isWaypointAltitudeReached(void) { - return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < WP_ALTITUDE_MARGIN_CM; + return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < navConfig()->general.waypoint_enforce_altitude; } static void updateHomePositionCompatibility(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 21eda4a37c6..28612daf22f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -227,7 +227,6 @@ typedef struct navConfig_s { uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0) uint8_t waypoint_mission_restart; // Waypoint mission restart action - uint8_t waypoint_enforce_altitude; // Forces waypoint altitude to be achieved uint8_t rth_trackback_mode; // Useage mode setting for RTH trackback } flags; @@ -257,6 +256,7 @@ typedef struct navConfig_s { uint16_t safehome_max_distance; // Max distance that a safehome is from the arming point uint16_t max_altitude; // Max altitude when in AltHold mode (not Surface Following) uint16_t rth_trackback_distance; // RTH trackback maximum distance [m] + uint16_t waypoint_enforce_altitude; // Forces waypoint altitude to be achieved } general; struct {