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

Setting Change

Browse source 
Added Spiral setting to nav_rth_climb_first setting.
This commit is contained in:
breadoven 2021-01-19 13:35:16 +00:00
parent bc6438a6f6
commit 55113eea65
5 changed files with 15 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -106,7 +106,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_tail_first = 0,
.disarm_on_landing = 0,
.rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS,
.rth_fw_spiral_climb = 0, // Loiter climb for FW RTH
.nav_overrides_motor_stop = NOMS_ALL_NAV,
},
@ -1152,7 +1151,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
if (STATE(FIXED_WING_LEGACY)) {
// Airplane - climbout before heading home
if (navConfig()->general.flags.rth_fw_spiral_climb) {
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
// Spiral climb centered at xy of RTH activation
calculateInitialHoldPosition(&targetHoldPos);
} else {
@ -1201,7 +1200,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)) {
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING_LEGACY)) {
@ -1232,7 +1231,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
if (navConfig()->general.flags.rth_fw_spiral_climb) {
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {