|
|
@ -94,7 +94,7 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO |
|
|
|
STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); |
|
|
|
|
|
|
|
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) |
|
|
|
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); |
|
|
|
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1); |
|
|
|
#endif |
|
|
|
|
|
|
|
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14); |
|
|
@ -1212,7 +1212,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n |
|
|
|
|
|
|
|
// 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 == OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) { |
|
|
|
|
|
|
|
|
|
|
|
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance |
|
|
|
if (STATE(FIXED_WING_LEGACY)) { |
|
|
|
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); |
|
|
@ -2161,9 +2161,9 @@ static void updateDesiredRTHAltitude(void) |
|
|
|
break; |
|
|
|
case NAV_RTH_CLIMB_STAGE_EXTRA: |
|
|
|
posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude; |
|
|
|
break; |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
switch (navConfig()->general.flags.rth_alt_control_mode) { |
|
|
|
case NAV_RTH_NO_ALT: |
|
|
|
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; |
|
|
@ -2462,7 +2462,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) |
|
|
|
} |
|
|
|
|
|
|
|
/* --------------------------------------------------- |
|
|
|
* If climb stage is being used, see if it is time to |
|
|
|
* If climb stage is being used, see if it is time to |
|
|
|
* transiton in to turn. |
|
|
|
* Limited to fixed wing only. |
|
|
|
* --------------------------------------------------- */ |
|
|
|