From fcedba8618b201c6047c4fe960db4c98d09d0de7 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 29 Nov 2020 13:06:29 +0000 Subject: [PATCH] Improved Code Cleaned up code removing Climb First flag. Tested in flight, works as expected. --- src/main/navigation/navigation.c | 35 ++++++++++++------------ src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_private.h | 1 - 3 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e1bbfdbfd8..cb5c58ddbf 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -229,7 +229,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); -static void rthAltControlStickOverrideCheck(unsigned axis); +static bool rthAltControlStickOverrideCheck(unsigned axis); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -1092,9 +1092,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - - // reset flag to override RTH climb first - posControl.flags.rthClimbFirstOverride = false; if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1161,9 +1158,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); - + rthAltControlStickOverrideCheck(PITCH); - rthAltControlStickOverrideCheck(ROLL); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1180,7 +1176,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || posControl.flags.rthClimbFirstOverride) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || rthAltControlStickOverrideCheck(ROLL)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1238,7 +1234,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) { UNUSED(previousState); - + rthAltControlStickOverrideCheck(PITCH); if ((posControl.flags.estHeadingStatus == EST_NONE)) { @@ -2517,33 +2513,36 @@ void updateHomePosition(void) /* ----------------------------------------------------------- * Override RTH preset altitude and Climb First option - * Set using Pitch/Roll stick held for > 2 seconds + * using Pitch/Roll stick held for > 2 seconds *-----------------------------------------------------------*/ -static void rthAltControlStickOverrideCheck(unsigned axis) +static bool rthAltControlStickOverrideCheck(unsigned axis) { if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated) { - return; + return false; } static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { - + timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; - + if (!rthOverrideStickHoldStartTime[axis]) { rthOverrideStickHoldStartTime[axis] = millis(); - } else if (ABS(2500 - holdTime) < 500) { - if (axis == PITCH) { // pitch down to override preset altitude reset to current altitude + } else if (ABS(2500 - holdTime) < 500) { + if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - } else { // roll right to override climb first - posControl.flags.rthClimbFirstOverride = true; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + return true; + } else if (axis == ROLL) { // ROLL right to override climb first + return true; } } } else { rthOverrideStickHoldStartTime[axis] = 0; } + + return false; } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 7b9c30606e..ac07036442 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -181,7 +181,7 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH - uint8_t rth_alt_control_override; // Set RTH preset climb altitude to Current altutude during climb using full down pitch stick + uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor } flags; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 5d3818d055..fcae1b7ded 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -87,7 +87,6 @@ typedef struct navigationFlags_s { bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; - bool rthClimbFirstOverride; // RTH Climb First override using Roll stick } navigationFlags_t; typedef enum {