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

Change Control to Pitch Stick

Browse source 
Override trigger changed from AlfHold mode to pitch stick input.
This commit is contained in:
breadoven 2020-11-17 21:15:37 +00:00
parent 181a1ce9e5
commit 8f2c2620ef
4 changed files with 29 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1092,8 +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);
posControl.flags.canOverrideRTHAlt = false; //prevent unwanted override if AltHold selected at RTH initialize
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
@ -1157,24 +1155,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
}
}
static void overrideRTHAtitudePreset(void)
{
if (!navConfig()->general.flags.rth_alt_control_override) {
return;
}
if (!IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
posControl.flags.canOverrideRTHAlt = true;
}
else {
if (posControl.flags.canOverrideRTHAlt && !posControl.flags.forcedRTHActivated) {
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
posControl.flags.canOverrideRTHAlt = false;
}
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -2531,6 +2511,33 @@ void updateHomePosition(void)
}
}
/* -----------------------------------------------------------
* Override preset RTH altitude to current altitude
*-----------------------------------------------------------*/
static void overrideRTHAtitudePreset(void)
{
if (!navConfig()->general.flags.rth_alt_control_override) {
return;
}
static timeMs_t PitchStickHoldStartTime;
if (rxGetChannelValue(PITCH) > 1900) {
if (!PitchStickHoldStartTime) {
PitchStickHoldStartTime = millis();
} else {
timeMs_t currentTime = millis();
if (currentTime - PitchStickHoldStartTime > 1000 && !posControl.flags.forcedRTHActivated) {
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
}
}
} else {
PitchStickHoldStartTime = 0;
}
DEBUG_SET(DEBUG_CRUISE, 1, PitchStickHoldStartTime);
}
/*-----------------------------------------------------------
* Update flight statistics
*-----------------------------------------------------------*/