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

Changes to fix EXTRA mode

Browse source 
Initial testing went pretty well. Using the RTH climb stage worked perfectly with the AT_LEAST method. However the EXTRA method didn't work. After reviewing the code, I can see why, and have implemented a fix. This should now work as expected. I'll test ASAP.
This commit is contained in:
Darren Lines 2021-07-24 19:55:24 +01:00
parent a1c05beeed
commit 9ed0b1d29b
3 changed files with 10 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2158,6 +2158,8 @@ static void updateDesiredRTHAltitude(void)
if (ARMING_FLAG(ARMED)) {
if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
|| ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
@ -2191,6 +2193,7 @@ static void updateDesiredRTHAltitude(void)
}
}
} else {
posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
}
@ -2468,7 +2471,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
}
break;
case NAV_RTH_CLIMB_STAGE_EXTRA:
if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthInitialAltitude + navConfig()->general.rth_climb_first_stage_altitude)) {
if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthStartAltitude + navConfig()->general.rth_climb_first_stage_altitude)) {
return true;
}
break;