Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Improved Code

Browse source 
Cleaned up code removing Climb First flag. Tested in flight, works as expected.
This commit is contained in:
breadoven 2020-11-29 13:06:29 +00:00
parent b3005fbc3c
commit fcedba8618
3 changed files with 18 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/navigation/navigation.c
View file

@ -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;
}
/*-----------------------------------------------------------

2
src/main/navigation/navigation.h
View file

@ -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;

1
src/main/navigation/navigation_private.h
View file

@ -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 {