src/main/navigation/navigation.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/filter.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "config/parameter_group.h"
  33 #include "config/parameter_group_ids.h"
  34
  35 #include "drivers/time.h"
  36
  37 #include "fc/fc_core.h"
  38 #include "fc/config.h"
  39 #include "fc/rc_controls.h"
  40 #include "fc/rc_modes.h"
  41 #include "fc/runtime_config.h"
  42 #ifdef USE_MULTI_MISSION
  43 #include "fc/cli.h"
  44 #endif
  45 #include "fc/settings.h"
  46
  47 #include "flight/imu.h"
  48 #include "flight/mixer.h"
  49 #include "flight/pid.h"
  50
  51 #include "io/beeper.h"
  52 #include "io/gps.h"
  53
  54 #include "navigation/navigation.h"
  55 #include "navigation/navigation_private.h"
  56
  57 #include "rx/rx.h"
  58
  59 #include "sensors/sensors.h"
  60 #include "sensors/acceleration.h"
  61 #include "sensors/boardalignment.h"
  62
  63
  64 // Multirotors:
  65 #define MR_RTH_CLIMB_OVERSHOOT_CM   100  // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
  66 #define MR_RTH_CLIMB_MARGIN_MIN_CM  100  // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
  67 #define MR_RTH_CLIMB_MARGIN_PERCENT 15   // on high RTH altitudes use even bigger margin - percent of the altitude set
  68 // Planes:
  69 #define FW_RTH_CLIMB_OVERSHOOT_CM   100
  70 #define FW_RTH_CLIMB_MARGIN_MIN_CM  100
  71 #define FW_RTH_CLIMB_MARGIN_PERCENT 15
  72
  73 /*-----------------------------------------------------------
  74  * Compatibility for home position
  75  *-----------------------------------------------------------*/
  76 gpsLocation_t GPS_home;
  77 uint32_t      GPS_distanceToHome;        // distance to home point in meters
  78 int16_t       GPS_directionToHome;       // direction to home point in degrees
  79
  80 fpVector3_t   original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
  81
  82 radar_pois_t radar_pois[RADAR_MAX_POIS];
  83 #if defined(USE_SAFE_HOME)
  84 int8_t safehome_index = -1;               // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
  85 uint32_t safehome_distance = 0;           // distance to the nearest safehome
  86 fpVector3_t nearestSafeHome;              // The nearestSafeHome found during arming
  87 bool safehome_applied = false;            // whether the safehome has been applied to home.
  88
  89 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
  90
  91 #endif
  92
  93 // waypoint 254, 255 are special waypoints
  94 STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
  95
  96 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
  97 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1);
  98 #endif
  99
 100 PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);
 101
 102 PG_RESET_TEMPLATE(navConfig_t, navConfig,
 103     .general = {
 104
 105         .flags = {
 106             .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT,
 107             .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
 108             .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
 109             .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
 110             .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT,                         // Climb first, turn after reaching safe altitude
 111             .rth_climb_first_stage_mode = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_MODE_DEFAULT,   // To determine how rth_climb_first_stage_altitude is used
 112             .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT,           // Ignore GPS loss on initial climb
 113             .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT,
 114             .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT,
 115             .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
 116             .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT,       // Override RTH Altitude and Climb First using Pitch and Roll stick
 117             .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
 118             .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
 119             .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT,       // Allow mode switch toggle to reset Mission Planner WPs
 120             .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT,           // WP mission restart action
 121             .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT,          // stops motor when Saoring mode enabled
 122         },
 123
 124         // General navigation parameters
 125         .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT,                            // 5 sec
 126         .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT,                                       // 2m diameter
 127         .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT,                         // centimeters - first waypoint should be closer than this
 128 #ifdef USE_MULTI_MISSION
 129         .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT,             // mission index selected from multi mission WP entry
 130 #endif
 131         .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT,                           // load waypoints automatically during boot
 132         .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT,                                           // speed in autonomous modes (3 m/s = 10.8 km/h)
 133         .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT,                                   // max allowed speed autonomous modes
 134         .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT,                             // 5 m/s
 135         .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
 136         .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
 137         .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT,                       // altitude in centimeters
 138         .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT,                       // altitude in meters
 139         .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT,                               // centimeters/s
 140         .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT,                               // centimeters/s
 141         .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT,                          // centimeters/s
 142         .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT,                               // centimeters, if closer than this land immediately
 143         .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT,                                       // altitude in centimeters
 144         .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT,                             // altitude in centimeters
 145         .rth_climb_first_stage_altitude = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_ALTITUDE_DEFAULT,   // altitude in centimetres, 0= off
 146         .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT,                         // centimeters - 500m should be safe for all aircraft
 147         .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT,              // max altitude in centimeters in terrain following mode
 148         .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT,                         // Max distance that a safehome is from the arming point
 149         .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
 150     },
 151
 152     // MC-specific
 153     .mc = {
 154         .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT,                          // degrees
 155         .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT,                // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
 156
 157 #ifdef USE_MR_BRAKING_MODE
 158         .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT,               // Braking can become active above 1m/s
 159         .braking_disengage_speed = SETTING_NAV_MC_BRAKING_DISENGAGE_SPEED_DEFAULT,               // Stop when speed goes below 0.75m/s
 160         .braking_timeout = SETTING_NAV_MC_BRAKING_TIMEOUT_DEFAULT,                               // Timeout barking after 2s
 161         .braking_boost_factor = SETTING_NAV_MC_BRAKING_BOOST_FACTOR_DEFAULT,                     // A 100% boost by default
 162         .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT,                   // Timout boost after 750ms
 163         .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT,   // Boost can happen only above 1.5m/s
 164         .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT,   // Disable boost at 1m/s
 165         .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT,                        // Max braking angle
 166 #endif
 167
 168         .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT,          // posDecelerationTime * 100
 169         .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT,                           // posResponseExpo * 100
 170         .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
 171     },
 172
 173     // Fixed wing
 174     .fw = {
 175         .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT,                    // degrees
 176         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                  // degrees
 177         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                    // degrees
 178         .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT,                    // cm/s
 179         .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
 180         .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
 181         .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
 182         .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT,                  // 75m
 183
 184         //Fixed wing landing
 185         .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT,              // 2 degrees dive by default
 186
 187         // Fixed wing launch
 188         .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT,       // 3 m/s
 189         .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT,             // cm/s/s (1.9*G)
 190         .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT,        // 40ms
 191         .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT,        // ms
 192         .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT,   // ms
 193         .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT,  // ms, time to gredually increase throttle from idle to launch
 194         .launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT,              // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
 195         .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT,              // ms, min time in launch mode
 196         .launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT,                // ms, timeout for launch procedure
 197         .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT,      // cm, altitude where to consider launch ended
 198         .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT,        // 18 degrees
 199         .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT,            // 45 deg
 200         .cruise_yaw_rate  = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT,             // 20dps
 201         .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
 202         .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
 203         .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
 204         .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
 205     }
 206 );
 207
 208 static navWapointHeading_t wpHeadingControl;
 209 navigationPosControl_t  posControl;
 210 navSystemStatus_t       NAV_Status;
 211 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 212
 213 // Blackbox states
 214 int16_t navCurrentState;
 215 int16_t navActualVelocity[3];
 216 int16_t navDesiredVelocity[3];
 217 int16_t navActualHeading;
 218 int16_t navDesiredHeading;
 219 int32_t navTargetPosition[3];
 220 int32_t navLatestActualPosition[3];
 221 int16_t navActualSurface;
 222 uint16_t navFlags;
 223 uint16_t navEPH;
 224 uint16_t navEPV;
 225 int16_t navAccNEU[3];
 226 //End of blackbox states
 227
 228 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
 229 static void updateDesiredRTHAltitude(void);
 230 static void resetAltitudeController(bool useTerrainFollowing);
 231 static void resetPositionController(void);
 232 static void setupAltitudeController(void);
 233 static void resetHeadingController(void);
 234 void resetGCSFlags(void);
 235
 236 static void setupJumpCounters(void);
 237 static void resetJumpCounter(void);
 238 static void clearJumpCounters(void);
 239
 240 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 241 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 242 void calculateInitialHoldPosition(fpVector3_t * pos);
 243 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
 244 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
 245 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
 246 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 247 static navigationFSMEvent_t nextForNonGeoStates(void);
 248 static bool isWaypointMissionValid(void);
 249 void missionPlannerSetWaypoint(void);
 250
 251 void initializeRTHSanityChecker(void);
 252 bool validateRTHSanityChecker(void);
 253 void updateHomePosition(void);
 254
 255 static bool rthAltControlStickOverrideCheck(unsigned axis);
 256
 257 /*************************************************************************************************/
 258 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
 259 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
 260 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
 261 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
 262 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
 263 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState);
 264 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState);
 265 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState);
 266 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState);
 267 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState);
 268 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState);
 269 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
 270 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
 271 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
 272 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
 273 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
 274 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
 275 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
 276 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
 277 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
 278 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
 279 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
 280 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
 281 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
 282 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
 283 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
 284 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
 285 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
 286 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
 287 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
 288 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
 289 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
 290 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
 291
 292 static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
 293     /** Idle state ******************************************************/
 294     [NAV_STATE_IDLE] = {
 295         .persistentId = NAV_PERSISTENT_ID_IDLE,
 296         .onEntry = navOnEnteringState_NAV_STATE_IDLE,
 297         .timeoutMs = 0,
 298         .stateFlags = 0,
 299         .mapToFlightModes = 0,
 300         .mwState = MW_NAV_STATE_NONE,
 301         .mwError = MW_NAV_ERROR_NONE,
 302         .onEvent    = {
 303             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 304             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 305             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 306             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 307             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 308             [NAV_FSM_EVENT_SWITCH_TO_LAUNCH]               = NAV_STATE_LAUNCH_INITIALIZE,
 309             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 310             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 311         }
 312     },
 313
 314     /** ALTHOLD mode ***************************************************/
 315     [NAV_STATE_ALTHOLD_INITIALIZE] = {
 316         .persistentId = NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE,
 317         .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
 318         .timeoutMs = 0,
 319         .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
 320         .mapToFlightModes = NAV_ALTHOLD_MODE,
 321         .mwState = MW_NAV_STATE_NONE,
 322         .mwError = MW_NAV_ERROR_NONE,
 323         .onEvent = {
 324             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_ALTHOLD_IN_PROGRESS,
 325             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 326             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 327         }
 328     },
 329
 330     [NAV_STATE_ALTHOLD_IN_PROGRESS] = {
 331         .persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS,
 332         .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
 333         .timeoutMs = 10,
 334         .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
 335         .mapToFlightModes = NAV_ALTHOLD_MODE,
 336         .mwState = MW_NAV_STATE_NONE,
 337         .mwError = MW_NAV_ERROR_NONE,
 338         .onEvent = {
 339             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_ALTHOLD_IN_PROGRESS,    // re-process the state
 340             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 341             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 342             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 343             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 344             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 345             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 346             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 347         }
 348     },
 349
 350     /** POSHOLD_3D mode ************************************************/
 351     [NAV_STATE_POSHOLD_3D_INITIALIZE] = {
 352         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
 353         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
 354         .timeoutMs = 0,
 355         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
 356         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
 357         .mwState = MW_NAV_STATE_HOLD_INFINIT,
 358         .mwError = MW_NAV_ERROR_NONE,
 359         .onEvent = {
 360             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_POSHOLD_3D_IN_PROGRESS,
 361             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 362             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 363         }
 364     },
 365
 366     [NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
 367         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
 368         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
 369         .timeoutMs = 10,
 370         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
 371         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
 372         .mwState = MW_NAV_STATE_HOLD_INFINIT,
 373         .mwError = MW_NAV_ERROR_NONE,
 374         .onEvent = {
 375             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_POSHOLD_3D_IN_PROGRESS,    // re-process the state
 376             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 377             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 378             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 379             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 380             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 381             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 382             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 383         }
 384     },
 385     /** CRUISE_HOLD mode ************************************************/
 386     [NAV_STATE_COURSE_HOLD_INITIALIZE] = {
 387         .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE,
 388         .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE,
 389         .timeoutMs = 0,
 390         .stateFlags = NAV_REQUIRE_ANGLE,
 391         .mapToFlightModes = NAV_COURSE_HOLD_MODE,
 392         .mwState = MW_NAV_STATE_NONE,
 393         .mwError = MW_NAV_ERROR_NONE,
 394         .onEvent = {
 395             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
 396             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 397             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 398         }
 399     },
 400
 401     [NAV_STATE_COURSE_HOLD_IN_PROGRESS] = {
 402         .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
 403         .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
 404         .timeoutMs = 10,
 405         .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
 406         .mapToFlightModes = NAV_COURSE_HOLD_MODE,
 407         .mwState = MW_NAV_STATE_NONE,
 408         .mwError = MW_NAV_ERROR_NONE,
 409         .onEvent = {
 410             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_COURSE_HOLD_IN_PROGRESS,    // re-process the state
 411             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 412             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 413             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 414             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 415             [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ]           = NAV_STATE_COURSE_HOLD_ADJUSTING,
 416             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 417             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 418             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 419         }
 420     },
 421
 422         [NAV_STATE_COURSE_HOLD_ADJUSTING] = {
 423         .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING,
 424         .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING,
 425         .timeoutMs = 10,
 426         .stateFlags =  NAV_REQUIRE_ANGLE | NAV_RC_POS,
 427         .mapToFlightModes = NAV_COURSE_HOLD_MODE,
 428         .mwState = MW_NAV_STATE_NONE,
 429         .mwError = MW_NAV_ERROR_NONE,
 430         .onEvent = {
 431             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
 432             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_COURSE_HOLD_ADJUSTING,
 433             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 434             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 435             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 436             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 437             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 438             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 439             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 440             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 441         }
 442     },
 443
 444     /** CRUISE_3D mode ************************************************/
 445     [NAV_STATE_CRUISE_INITIALIZE] = {
 446         .persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE,
 447         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE,
 448         .timeoutMs = 0,
 449         .stateFlags = NAV_REQUIRE_ANGLE,
 450         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
 451         .mwState = MW_NAV_STATE_NONE,
 452         .mwError = MW_NAV_ERROR_NONE,
 453         .onEvent = {
 454             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_CRUISE_IN_PROGRESS,
 455             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 456             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 457         }
 458     },
 459
 460     [NAV_STATE_CRUISE_IN_PROGRESS] = {
 461         .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
 462         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
 463         .timeoutMs = 10,
 464         .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
 465         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
 466         .mwState = MW_NAV_STATE_NONE,
 467         .mwError = MW_NAV_ERROR_NONE,
 468         .onEvent = {
 469             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_CRUISE_IN_PROGRESS,    // re-process the state
 470             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 471             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 472             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 473             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 474             [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ]           = NAV_STATE_CRUISE_ADJUSTING,
 475             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 476             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 477             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 478         }
 479     },
 480
 481     [NAV_STATE_CRUISE_ADJUSTING] = {
 482         .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING,
 483         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING,
 484         .timeoutMs = 10,
 485         .stateFlags =  NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
 486         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
 487         .mwState = MW_NAV_STATE_NONE,
 488         .mwError = MW_NAV_ERROR_NONE,
 489         .onEvent = {
 490             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_CRUISE_IN_PROGRESS,
 491             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_CRUISE_ADJUSTING,
 492             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 493             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 494             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 495             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 496             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 497             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 498             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 499             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 500         }
 501     },
 502
 503     /** RTH_3D mode ************************************************/
 504     [NAV_STATE_RTH_INITIALIZE] = {
 505         .persistentId = NAV_PERSISTENT_ID_RTH_INITIALIZE,
 506         .onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
 507         .timeoutMs = 10,
 508         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
 509         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 510         .mwState = MW_NAV_STATE_RTH_START,
 511         .mwError = MW_NAV_ERROR_NONE,
 512         .onEvent = {
 513             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_INITIALIZE,      // re-process the state
 514             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
 515             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 516             [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING]          = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
 517             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 518         }
 519     },
 520
 521     [NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = {
 522         .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
 523         .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
 524         .timeoutMs = 10,
 525         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
 526         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 527         .mwState = MW_NAV_STATE_RTH_CLIMB,
 528         .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
 529         .onEvent = {
 530             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,   // re-process the state
 531             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_HEAD_HOME,
 532             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 533             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 534             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 535             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 536             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 537             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 538         }
 539     },
 540
 541     [NAV_STATE_RTH_HEAD_HOME] = {
 542         .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
 543         .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
 544         .timeoutMs = 10,
 545         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
 546         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 547         .mwState = MW_NAV_STATE_RTH_ENROUTE,
 548         .mwError = MW_NAV_ERROR_NONE,
 549         .onEvent = {
 550             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
 551             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
 552             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 553             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 554             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 555             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 556             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 557             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 558         }
 559     },
 560
 561     [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
 562         .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
 563         .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
 564         .timeoutMs = 500,
 565         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
 566         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 567         .mwState = MW_NAV_STATE_LAND_SETTLE,
 568         .mwError = MW_NAV_ERROR_NONE,
 569         .onEvent = {
 570             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
 571             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_LANDING,
 572             [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
 573             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 574             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 575             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 576             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 577             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 578             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 579         }
 580     },
 581
 582     [NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
 583         .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
 584         .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
 585         .timeoutMs = 10,
 586         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
 587         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 588         .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
 589         .mwError = MW_NAV_ERROR_NONE,
 590         .onEvent = {
 591             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HOVER_ABOVE_HOME,
 592             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 593             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 594             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 595             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 596             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 597             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 598         }
 599     },
 600
 601     [NAV_STATE_RTH_LANDING] = {
 602         .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
 603         .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
 604         .timeoutMs = 10,
 605         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
 606         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 607         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
 608         .mwError = MW_NAV_ERROR_LANDING,
 609         .onEvent = {
 610             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_LANDING,         // re-process state
 611             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_FINISHING,
 612             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 613             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 614             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 615             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 616         }
 617     },
 618
 619     [NAV_STATE_RTH_FINISHING] = {
 620         .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
 621         .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
 622         .timeoutMs = 0,
 623         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
 624         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 625         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
 626         .mwError = MW_NAV_ERROR_LANDING,
 627         .onEvent = {
 628             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_FINISHED,
 629             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 630         }
 631     },
 632
 633     [NAV_STATE_RTH_FINISHED] = {
 634         .persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
 635         .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
 636         .timeoutMs = 10,
 637         .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
 638         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
 639         .mwState = MW_NAV_STATE_LANDED,
 640         .mwError = MW_NAV_ERROR_NONE,
 641         .onEvent = {
 642             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_FINISHED,         // re-process state
 643             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 644             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 645             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 646             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 647         }
 648     },
 649
 650     /** WAYPOINT mode ************************************************/
 651     [NAV_STATE_WAYPOINT_INITIALIZE] = {
 652         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE,
 653         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
 654         .timeoutMs = 0,
 655         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 656         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 657         .mwState = MW_NAV_STATE_PROCESS_NEXT,
 658         .mwError = MW_NAV_ERROR_NONE,
 659         .onEvent = {
 660             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_PRE_ACTION,
 661             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 662             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 663             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED]    = NAV_STATE_WAYPOINT_FINISHED,
 664         }
 665     },
 666
 667     [NAV_STATE_WAYPOINT_PRE_ACTION] = {
 668         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
 669         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
 670         .timeoutMs = 10,
 671         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 672         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 673         .mwState = MW_NAV_STATE_PROCESS_NEXT,
 674         .mwError = MW_NAV_ERROR_NONE,
 675         .onEvent = {
 676             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_PRE_ACTION,   // re-process the state (for JUMP)
 677             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_IN_PROGRESS,
 678             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
 679             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
 680             [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
 681             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
 682         }
 683     },
 684
 685     [NAV_STATE_WAYPOINT_IN_PROGRESS] = {
 686         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS,
 687         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
 688         .timeoutMs = 10,
 689         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 690         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 691         .mwState = MW_NAV_STATE_WP_ENROUTE,
 692         .mwError = MW_NAV_ERROR_NONE,
 693         .onEvent = {
 694             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_IN_PROGRESS,   // re-process the state
 695             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_REACHED,       // successfully reached waypoint
 696             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 697             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 698             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 699             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 700             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 701             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 702             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 703         }
 704     },
 705
 706     [NAV_STATE_WAYPOINT_REACHED] = {
 707         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED,
 708         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
 709         .timeoutMs = 10,
 710         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 711         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 712         .mwState = MW_NAV_STATE_PROCESS_NEXT,
 713         .mwError = MW_NAV_ERROR_NONE,
 714         .onEvent = {
 715             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_REACHED,   // re-process state
 716             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,
 717             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
 718             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
 719             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
 720             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
 721             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
 722             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
 723             [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
 724             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 725             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]       = NAV_STATE_COURSE_HOLD_INITIALIZE,
 726             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]            = NAV_STATE_CRUISE_INITIALIZE,
 727         }
 728     },
 729
 730     [NAV_STATE_WAYPOINT_HOLD_TIME] = {
 731         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
 732         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
 733         .timeoutMs = 10,
 734         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 735         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 736         .mwState = MW_NAV_STATE_HOLD_TIMED,
 737         .mwError = MW_NAV_ERROR_NONE,
 738         .onEvent = {
 739             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_HOLD_TIME,     // re-process the state
 740             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_NEXT,          // successfully reached waypoint
 741             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 742             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 743             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 744             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 745             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 746             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 747             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 748         }
 749     },
 750
 751     [NAV_STATE_WAYPOINT_RTH_LAND] = {
 752         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
 753         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
 754         .timeoutMs = 10,
 755         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 756         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 757         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
 758         .mwError = MW_NAV_ERROR_LANDING,
 759         .onEvent = {
 760             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_RTH_LAND,   // re-process state
 761             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_FINISHED,
 762             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 763             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 764             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 765             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 766             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 767             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 768             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 769         }
 770     },
 771
 772     [NAV_STATE_WAYPOINT_NEXT] = {
 773         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT,
 774         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT,
 775         .timeoutMs = 0,
 776         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
 777         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 778         .mwState = MW_NAV_STATE_PROCESS_NEXT,
 779         .mwError = MW_NAV_ERROR_NONE,
 780         .onEvent = {
 781             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_PRE_ACTION,
 782             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED]    = NAV_STATE_WAYPOINT_FINISHED,
 783         }
 784     },
 785
 786     [NAV_STATE_WAYPOINT_FINISHED] = {
 787         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
 788         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
 789         .timeoutMs = 10,
 790         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
 791         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
 792         .mwState = MW_NAV_STATE_WP_ENROUTE,
 793         .mwError = MW_NAV_ERROR_FINISH,
 794         .onEvent = {
 795             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_FINISHED,   // re-process state
 796             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 797             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 798             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
 799             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 800             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 801             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
 802             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
 803         }
 804     },
 805
 806     /** EMERGENCY LANDING ************************************************/
 807     [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
 808         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
 809         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 810         .timeoutMs = 0,
 811         .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
 812         .mapToFlightModes = 0,
 813         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
 814         .mwError = MW_NAV_ERROR_LANDING,
 815         .onEvent = {
 816             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
 817             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 818             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 819             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 820             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 821             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 822         }
 823     },
 824
 825     [NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
 826         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
 827         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
 828         .timeoutMs = 10,
 829         .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
 830         .mapToFlightModes = 0,
 831         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
 832         .mwError = MW_NAV_ERROR_LANDING,
 833         .onEvent = {
 834             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,    // re-process the state
 835             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_EMERGENCY_LANDING_FINISHED,
 836             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 837             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
 838             [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
 839             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
 840         }
 841     },
 842
 843     [NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
 844         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
 845         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
 846         .timeoutMs = 10,
 847         .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
 848         .mapToFlightModes = 0,
 849         .mwState = MW_NAV_STATE_LANDED,
 850         .mwError = MW_NAV_ERROR_LANDING,
 851         .onEvent = {
 852             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_EMERGENCY_LANDING_FINISHED,
 853             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 854         }
 855     },
 856
 857     [NAV_STATE_LAUNCH_INITIALIZE] = {
 858         .persistentId = NAV_PERSISTENT_ID_LAUNCH_INITIALIZE,
 859         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
 860         .timeoutMs = 0,
 861         .stateFlags = NAV_REQUIRE_ANGLE,
 862         .mapToFlightModes = NAV_LAUNCH_MODE,
 863         .mwState = MW_NAV_STATE_NONE,
 864         .mwError = MW_NAV_ERROR_NONE,
 865         .onEvent = {
 866             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_LAUNCH_WAIT,
 867             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 868             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 869         }
 870     },
 871
 872     [NAV_STATE_LAUNCH_WAIT] = {
 873         .persistentId = NAV_PERSISTENT_ID_LAUNCH_WAIT,
 874         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
 875         .timeoutMs = 10,
 876         .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
 877         .mapToFlightModes = NAV_LAUNCH_MODE,
 878         .mwState = MW_NAV_STATE_NONE,
 879         .mwError = MW_NAV_ERROR_NONE,
 880         .onEvent = {
 881             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_LAUNCH_WAIT,    // re-process the state
 882             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_LAUNCH_IN_PROGRESS,
 883             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 884             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 885         }
 886     },
 887
 888     [NAV_STATE_LAUNCH_IN_PROGRESS] = {
 889         .persistentId = NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS,
 890         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
 891         .timeoutMs = 10,
 892         .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
 893         .mapToFlightModes = NAV_LAUNCH_MODE,
 894         .mwState = MW_NAV_STATE_NONE,
 895         .mwError = MW_NAV_ERROR_NONE,
 896         .onEvent = {
 897             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_LAUNCH_IN_PROGRESS,    // re-process the state
 898             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_IDLE,
 899             [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
 900             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
 901         }
 902     },
 903 };
 904
 905 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
 906 {
 907     return navFSM[state].stateFlags;
 908 }
 909
 910 static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
 911 {
 912     return navFSM[state].mapToFlightModes;
 913 }
 914
 915 navigationFSMStateFlags_t navGetCurrentStateFlags(void)
 916 {
 917     return navGetStateFlags(posControl.navState);
 918 }
 919
 920 static bool navTerrainFollowingRequested(void)
 921 {
 922     // Terrain following not supported on FIXED WING aircraft yet
 923     return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
 924 }
 925
 926 /*************************************************************************************************/
 927 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
 928 {
 929     UNUSED(previousState);
 930
 931     resetAltitudeController(false);
 932     resetHeadingController();
 933     resetPositionController();
 934
 935     return NAV_FSM_EVENT_NONE;
 936 }
 937
 938 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
 939 {
 940     const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
 941     const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
 942
 943     resetGCSFlags();
 944
 945     // If surface tracking mode changed value - reset altitude controller
 946     if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
 947         resetAltitudeController(navTerrainFollowingRequested());
 948     }
 949
 950     if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
 951         setupAltitudeController();
 952         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);  // This will reset surface offset
 953     }
 954
 955     return NAV_FSM_EVENT_SUCCESS;
 956 }
 957
 958 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState)
 959 {
 960     UNUSED(previousState);
 961
 962     // If GCS was disabled - reset altitude setpoint
 963     if (posControl.flags.isGCSAssistedNavigationReset) {
 964         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
 965         resetGCSFlags();
 966     }
 967
 968     return NAV_FSM_EVENT_NONE;
 969 }
 970
 971 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
 972 {
 973     const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
 974     const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
 975
 976     resetGCSFlags();
 977
 978     if ((prevFlags & NAV_CTL_POS) == 0) {
 979         resetPositionController();
 980     }
 981
 982     if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
 983         resetAltitudeController(navTerrainFollowingRequested());
 984         setupAltitudeController();
 985     }
 986
 987     if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
 988         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);  // This will reset surface offset
 989     }
 990
 991     if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
 992         fpVector3_t targetHoldPos;
 993         calculateInitialHoldPosition(&targetHoldPos);
 994         setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
 995     }
 996
 997     return NAV_FSM_EVENT_SUCCESS;
 998 }
 999
1000 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState)
1001 {
1002     UNUSED(previousState);
1003
1004     // If GCS was disabled - reset 2D pos setpoint
1005     if (posControl.flags.isGCSAssistedNavigationReset) {
1006         fpVector3_t targetHoldPos;
1007         calculateInitialHoldPosition(&targetHoldPos);
1008         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
1009         setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1010         resetGCSFlags();
1011     }
1012
1013     return NAV_FSM_EVENT_NONE;
1014 }
1015 /////////////////
1016
1017 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
1018 {
1019     const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1020
1021     if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
1022
1023     DEBUG_SET(DEBUG_CRUISE, 0, 1);
1024     if (checkForPositionSensorTimeout()) {
1025         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1026     }  // Switch to IDLE if we do not have an healty position. Try the next iteration.
1027
1028     if (!(prevFlags & NAV_CTL_POS)) {
1029         resetPositionController();
1030     }
1031
1032     posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
1033     posControl.cruise.previousYaw = posControl.cruise.yaw;
1034     posControl.cruise.lastYawAdjustmentTime = 0;
1035
1036     return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
1037 }
1038
1039 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
1040 {
1041     const timeMs_t currentTimeMs = millis();
1042
1043     if (checkForPositionSensorTimeout()) {
1044         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1045     } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
1046
1047     DEBUG_SET(DEBUG_CRUISE, 0, 2);
1048     DEBUG_SET(DEBUG_CRUISE, 2, 0);
1049
1050     if (posControl.flags.isAdjustingPosition) {
1051         return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
1052     }
1053
1054     // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
1055     if (posControl.flags.isAdjustingHeading) {
1056         timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
1057         if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
1058         float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
1059         float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
1060         posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
1061         DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
1062         posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
1063     }
1064
1065     if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
1066         posControl.cruise.previousYaw = posControl.cruise.yaw;
1067
1068     uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
1069
1070     if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
1071             || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
1072             || posControl.flags.isAdjustingHeading) {
1073         calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
1074         DEBUG_SET(DEBUG_CRUISE, 2, 1);
1075     } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
1076         calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
1077         DEBUG_SET(DEBUG_CRUISE, 2, 2);
1078     }
1079
1080     setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
1081
1082     return NAV_FSM_EVENT_NONE;
1083 }
1084
1085 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState)
1086 {
1087     UNUSED(previousState);
1088     DEBUG_SET(DEBUG_CRUISE, 0, 3);
1089
1090     // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
1091     if (posControl.flags.isAdjustingPosition) {
1092         posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
1093         posControl.cruise.lastYawAdjustmentTime = millis();
1094         return NAV_FSM_EVENT_NONE;  // reprocess the state
1095     }
1096
1097     resetPositionController();
1098
1099     return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
1100 }
1101
1102 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
1103 {
1104     if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
1105
1106     navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
1107
1108     return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);
1109 }
1110
1111 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState)
1112 {
1113     navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1114
1115     return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState);
1116 }
1117
1118 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState)
1119 {
1120     navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1121
1122     return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState);
1123 }
1124
1125 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
1126 {
1127     navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1128
1129     if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
1130         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
1131         // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
1132         // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
1133         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1134     }
1135
1136     if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
1137         // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
1138         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1139     }
1140
1141     // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
1142     if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
1143         // Reset altitude and position controllers if necessary
1144         if ((prevFlags & NAV_CTL_POS) == 0) {
1145             resetPositionController();
1146         }
1147
1148         // Reset altitude controller if it was not enabled or if we are in terrain follow mode
1149         if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
1150             // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
1151             resetAltitudeController(false);
1152             setupAltitudeController();
1153         }
1154
1155         // If close to home - reset home position and land
1156         if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
1157             setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
1158             setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1159
1160             return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;   // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1161         }
1162         else {
1163             fpVector3_t targetHoldPos;
1164
1165             if (STATE(FIXED_WING_LEGACY)) {
1166                 // Airplane - climbout before heading home
1167                 if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
1168                     // Spiral climb centered at xy of RTH activation
1169                     calculateInitialHoldPosition(&targetHoldPos);
1170                 } else {
1171                     calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f);  // 1km away Linear climb
1172                 }
1173             } else {
1174                 // Multicopter, hover and climb
1175                 calculateInitialHoldPosition(&targetHoldPos);
1176
1177                 // Initialize RTH sanity check to prevent fly-aways on RTH
1178                 // For airplanes this is delayed until climb-out is finished
1179                 initializeRTHSanityChecker();
1180             }
1181
1182             setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1183
1184             return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
1185         }
1186     }
1187     /* Position sensor failure timeout - land. Land immediately if failsafe RTH and timeout disabled (set to 0) */
1188     else if (checkForPositionSensorTimeout() || (!navConfig()->general.pos_failure_timeout && posControl.flags.forcedRTHActivated)) {
1189         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1190     }
1191     /* No valid POS sensor but still within valid timeout - wait */
1192     return NAV_FSM_EVENT_NONE;
1193 }
1194
1195 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
1196 {
1197     UNUSED(previousState);
1198
1199     if (!STATE(ALTITUDE_CONTROL)) {
1200         //If altitude control is not a thing, switch to RTH in progress instead
1201         return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
1202     }
1203
1204     rthAltControlStickOverrideCheck(PITCH);
1205
1206     /* Position sensor failure timeout and not configured to ignore GPS loss - land */
1207     if ((posControl.flags.estHeadingStatus == EST_NONE) ||
1208         (checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) {
1209         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1210     }
1211
1212     const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
1213     const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
1214
1215     // If we reached desired initial RTH altitude or we don't want to climb first
1216     if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
1217
1218         // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
1219         if (STATE(FIXED_WING_LEGACY)) {
1220             initializeRTHSanityChecker();
1221         }
1222
1223         // Save initial home distance for future use
1224         posControl.rthState.rthInitialDistance = posControl.homeDistance;
1225         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1226
1227         if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
1228             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1229         }
1230         else {
1231             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1232         }
1233
1234         return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_RTH_HEAD_HOME
1235
1236     } else {
1237
1238         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1239
1240         /* For multi-rotors execute sanity check during initial ascent as well */
1241         if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
1242             return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1243         }
1244
1245         // Climb to safe altitude and turn to correct direction
1246         if (STATE(FIXED_WING_LEGACY)) {
1247             if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
1248                 float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
1249                 updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
1250             } else {
1251                 tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
1252                 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1253             }
1254         } else {
1255             // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
1256             // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
1257             tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
1258
1259             if (navConfig()->general.flags.rth_tail_first) {
1260                 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1261             } else {
1262                 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1263             }
1264         }
1265
1266         return NAV_FSM_EVENT_NONE;
1267     }
1268 }
1269
1270 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
1271 {
1272     UNUSED(previousState);
1273
1274     rthAltControlStickOverrideCheck(PITCH);
1275
1276     /* If position sensors unavailable - land immediately */
1277     if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) {
1278         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1279     }
1280
1281     // If we have position sensor - continue home
1282     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
1283         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
1284
1285         if (isWaypointPositionReached(tmpHomePos, true)) {
1286             // Successfully reached position target - update XYZ-position
1287             setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1288             return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1289         } else {
1290             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
1291             return NAV_FSM_EVENT_NONE;
1292         }
1293     }
1294     /* Position sensor failure timeout - land */
1295     else if (checkForPositionSensorTimeout()) {
1296         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1297     }
1298     /* No valid POS sensor but still within valid timeout - wait */
1299     return NAV_FSM_EVENT_NONE;
1300 }
1301
1302 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
1303 {
1304     UNUSED(previousState);
1305
1306     //On ROVER and BOAT we immediately switch to the next event
1307     if (!STATE(ALTITUDE_CONTROL)) {
1308         return NAV_FSM_EVENT_SUCCESS;
1309     }
1310
1311     /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1312     if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1313         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1314     }
1315
1316     // If position ok OR within valid timeout - continue
1317     // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
1318     if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
1319         resetLandingDetector();
1320         updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
1321         return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
1322     } else {
1323         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
1324         setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1325         return NAV_FSM_EVENT_NONE;
1326     }
1327 }
1328
1329 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
1330 {
1331     UNUSED(previousState);
1332
1333     /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1334     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1335         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1336     }
1337
1338     fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
1339
1340     if (navConfig()->general.rth_home_altitude) {
1341         float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
1342
1343         if (timeToReachHomeAltitude < 1) {
1344             // we almost reached the target home altitude so set the desired altitude to the desired home altitude
1345             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1346         } else {
1347             float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
1348             updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
1349         }
1350     }
1351     else {
1352         setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1353     }
1354
1355     return NAV_FSM_EVENT_NONE;
1356 }
1357
1358 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
1359 {
1360     UNUSED(previousState);
1361
1362     //On ROVER and BOAT we immediately switch to the next event
1363     if (!STATE(ALTITUDE_CONTROL)) {
1364         return NAV_FSM_EVENT_SUCCESS;
1365     }
1366
1367     if (!ARMING_FLAG(ARMED) || isLandingDetected()) {
1368         return NAV_FSM_EVENT_SUCCESS;
1369     }
1370
1371     /* If position sensors unavailable - land immediately (wait for timeout on GPS)
1372      * Continue to check for RTH sanity during landing */
1373     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1374         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1375     }
1376
1377     float descentVelLimited = 0;
1378
1379     // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
1380     if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
1381         // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
1382         // Do not allow descent velocity slower than -30cm/s so the landing detector works.
1383         descentVelLimited = navConfig()->general.land_minalt_vspd;
1384     } else {
1385         // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
1386         float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
1387                                                 navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
1388                                                 navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1389
1390         descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1391     }
1392
1393     updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
1394
1395     return NAV_FSM_EVENT_NONE;
1396 }
1397
1398 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState)
1399 {
1400     UNUSED(previousState);
1401
1402     //On ROVER and BOAT disarm immediately
1403     if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
1404         disarm(DISARM_NAVIGATION);
1405     }
1406
1407     return NAV_FSM_EVENT_SUCCESS;
1408 }
1409
1410 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState)
1411 {
1412     // Stay in this state
1413     UNUSED(previousState);
1414
1415     if (STATE(ALTITUDE_CONTROL)) {
1416         updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL);  // FIXME
1417     }
1418
1419     // Prevent I-terms growing when already landed
1420     pidResetErrorAccumulators();
1421     return NAV_FSM_EVENT_NONE;
1422 }
1423
1424 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState)
1425 {
1426     UNUSED(previousState);
1427
1428     if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
1429         return NAV_FSM_EVENT_ERROR;
1430     }
1431     else {
1432         // Prepare controllers
1433         resetPositionController();
1434
1435         // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
1436         resetAltitudeController(false);
1437         setupAltitudeController();
1438 /*
1439   Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
1440   Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
1441 */
1442         if (posControl.activeWaypointIndex == 0 || posControl.wpMissionRestart) {
1443             setupJumpCounters();
1444             posControl.activeWaypointIndex = 0;
1445             wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1446         }
1447
1448         if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
1449             posControl.wpMissionRestart = posControl.activeWaypointIndex ? !posControl.wpMissionRestart : false;
1450         } else {
1451             posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
1452         }
1453
1454         return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1455     }
1456 }
1457
1458 static navigationFSMEvent_t nextForNonGeoStates(void)
1459 {
1460         /* simple helper for non-geographical states that just set other data */
1461     const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
1462
1463     if (isLastWaypoint) {
1464             // non-geo state is the last waypoint, switch to finish.
1465         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1466     } else {
1467             // Finished non-geo,  move to next WP
1468         posControl.activeWaypointIndex++;
1469         return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1470     }
1471 }
1472
1473 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
1474 {
1475     /* A helper function to do waypoint-specific action */
1476     UNUSED(previousState);
1477
1478     switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1479         case NAV_WP_ACTION_HOLD_TIME:
1480         case NAV_WP_ACTION_WAYPOINT:
1481         case NAV_WP_ACTION_LAND:
1482             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
1483             posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
1484             posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
1485             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1486
1487                 // We use p3 as the volatile jump counter (p2 is the static value)
1488         case NAV_WP_ACTION_JUMP:
1489             if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
1490                 if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
1491                     resetJumpCounter();
1492                     return nextForNonGeoStates();
1493                 }
1494                 else
1495                 {
1496                     posControl.waypointList[posControl.activeWaypointIndex].p3--;
1497                 }
1498             }
1499             posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
1500             return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1501
1502         case NAV_WP_ACTION_SET_POI:
1503             if (STATE(MULTIROTOR)) {
1504                 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
1505                 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
1506                                            &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
1507             }
1508             return nextForNonGeoStates();
1509
1510         case NAV_WP_ACTION_SET_HEAD:
1511             if (STATE(MULTIROTOR)) {
1512                 if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
1513                     posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
1514                     wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1515                 } else {
1516                     wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
1517                     wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
1518                 }
1519             }
1520             return nextForNonGeoStates();
1521
1522         case NAV_WP_ACTION_RTH:
1523             posControl.wpMissionRestart = true;
1524             return NAV_FSM_EVENT_SWITCH_TO_RTH;
1525     };
1526
1527     UNREACHABLE();
1528 }
1529
1530 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
1531 {
1532     UNUSED(previousState);
1533
1534     // If no position sensor available - land immediately
1535     if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
1536         switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1537             case NAV_WP_ACTION_HOLD_TIME:
1538             case NAV_WP_ACTION_WAYPOINT:
1539             case NAV_WP_ACTION_LAND:
1540                 if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
1541                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
1542                 }
1543                 else {
1544                     fpVector3_t tmpWaypoint;
1545                     tmpWaypoint.x = posControl.activeWaypoint.pos.x;
1546                     tmpWaypoint.y = posControl.activeWaypoint.pos.y;
1547                     tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
1548                         posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
1549                         posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
1550                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1551                     if(STATE(MULTIROTOR)) {
1552                         switch (wpHeadingControl.mode) {
1553                             case NAV_WP_HEAD_MODE_NONE:
1554                                 break;
1555                             case NAV_WP_HEAD_MODE_FIXED:
1556                                 setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
1557                                 break;
1558                             case NAV_WP_HEAD_MODE_POI:
1559                                 setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
1560                                 break;
1561                         }
1562                     }
1563                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
1564                 }
1565                 break;
1566
1567             case NAV_WP_ACTION_JUMP:
1568             case NAV_WP_ACTION_SET_HEAD:
1569             case NAV_WP_ACTION_SET_POI:
1570             case NAV_WP_ACTION_RTH:
1571                 UNREACHABLE();
1572         }
1573     }
1574     /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1575     else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
1576         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1577     }
1578
1579     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
1580 }
1581
1582 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
1583 {
1584     UNUSED(previousState);
1585
1586     switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1587         case NAV_WP_ACTION_WAYPOINT:
1588             return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
1589
1590         case NAV_WP_ACTION_JUMP:
1591         case NAV_WP_ACTION_SET_HEAD:
1592         case NAV_WP_ACTION_SET_POI:
1593         case NAV_WP_ACTION_RTH:
1594             UNREACHABLE();
1595
1596         case NAV_WP_ACTION_LAND:
1597             return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
1598
1599         case NAV_WP_ACTION_HOLD_TIME:
1600             // Save the current time for the time the waypoint was reached
1601             posControl.wpReachedTime = millis();
1602             return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1603     }
1604
1605     UNREACHABLE();
1606 }
1607
1608 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
1609 {
1610     UNUSED(previousState);
1611
1612     /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1613     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
1614         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1615     }
1616
1617     timeMs_t currentTime = millis();
1618
1619     if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
1620         (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
1621         return NAV_FSM_EVENT_SUCCESS;
1622     }
1623
1624
1625     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
1626 }
1627
1628 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
1629 {
1630     UNUSED(previousState);
1631
1632     const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
1633     if (landEvent == NAV_FSM_EVENT_SUCCESS) {
1634         // Landing controller returned success - invoke RTH finishing state and finish the waypoint
1635         navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
1636         return NAV_FSM_EVENT_SUCCESS;
1637     }
1638     else {
1639         return NAV_FSM_EVENT_NONE;
1640     }
1641 }
1642
1643 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
1644 {
1645     UNUSED(previousState);
1646
1647     if (isLastMissionWaypoint()) {      // Last waypoint reached
1648         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1649     }
1650     else {
1651         // Waypoint reached, do something and move on to next waypoint
1652         posControl.activeWaypointIndex++;
1653         return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1654     }
1655 }
1656
1657 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState)
1658 {
1659     UNUSED(previousState);
1660
1661     clearJumpCounters();
1662     posControl.wpMissionRestart = true;
1663
1664     /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1665     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
1666         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1667     }
1668
1669     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
1670 }
1671
1672 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
1673 {
1674     // TODO:
1675     UNUSED(previousState);
1676
1677     // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
1678     // Make sure terrain following is not enabled
1679     resetAltitudeController(false);
1680
1681     return NAV_FSM_EVENT_SUCCESS;
1682 }
1683
1684 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
1685 {
1686     // TODO:
1687     UNUSED(previousState);
1688     return NAV_FSM_EVENT_NONE;
1689 }
1690
1691 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
1692 {
1693     // TODO:
1694     UNUSED(previousState);
1695
1696     // Prevent I-terms growing when already landed
1697     pidResetErrorAccumulators();
1698
1699     return NAV_FSM_EVENT_SUCCESS;
1700 }
1701
1702 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
1703 {
1704     const timeUs_t currentTimeUs = micros();
1705     UNUSED(previousState);
1706
1707     resetFixedWingLaunchController(currentTimeUs);
1708
1709     return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_LAUNCH_WAIT
1710 }
1711
1712 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState)
1713 {
1714     const timeUs_t currentTimeUs = micros();
1715     UNUSED(previousState);
1716
1717     if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
1718         enableFixedWingLaunchController(currentTimeUs);
1719         return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_LAUNCH_IN_PROGRESS
1720     }
1721
1722     //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
1723     if (feature(FEATURE_FW_LAUNCH)) {
1724         throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
1725         if ((throttleStatus == THROTTLE_LOW) && (isRollPitchStickDeflected())) {
1726             abortFixedWingLaunch();
1727             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1728         }
1729     }
1730
1731     return NAV_FSM_EVENT_NONE;
1732 }
1733
1734 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState)
1735 {
1736     UNUSED(previousState);
1737
1738     if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) {
1739         return NAV_FSM_EVENT_SUCCESS;
1740     }
1741
1742     return NAV_FSM_EVENT_NONE;
1743 }
1744
1745 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
1746 {
1747     navigationFSMState_t previousState;
1748
1749     previousState = posControl.navState;
1750     if (posControl.navState != newState) {
1751         posControl.navState = newState;
1752         posControl.navPersistentId = navFSM[newState].persistentId;
1753     }
1754     return previousState;
1755 }
1756
1757 static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
1758 {
1759     const timeMs_t currentMillis = millis();
1760     navigationFSMState_t previousState;
1761     static timeMs_t lastStateProcessTime = 0;
1762
1763     /* Inject new event */
1764     if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
1765         /* Update state */
1766         previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
1767
1768         /* Call new state's entry function */
1769         while (navFSM[posControl.navState].onEntry) {
1770             navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
1771
1772             if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
1773                 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
1774             }
1775             else {
1776                 break;
1777             }
1778         }
1779
1780         lastStateProcessTime  = currentMillis;
1781     }
1782
1783     /* If timeout event defined and timeout reached - switch state */
1784     if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
1785             ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
1786         /* Update state */
1787         previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
1788
1789         /* Call new state's entry function */
1790         while (navFSM[posControl.navState].onEntry) {
1791             navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
1792
1793             if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
1794                 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
1795             }
1796             else {
1797                 break;
1798             }
1799         }
1800
1801         lastStateProcessTime  = currentMillis;
1802     }
1803
1804     /* Update public system state information */
1805     NAV_Status.mode = MW_GPS_MODE_NONE;
1806
1807     if (ARMING_FLAG(ARMED)) {
1808         navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
1809
1810         if (navStateFlags & NAV_AUTO_RTH) {
1811             NAV_Status.mode = MW_GPS_MODE_RTH;
1812         }
1813         else if (navStateFlags & NAV_AUTO_WP) {
1814             NAV_Status.mode = MW_GPS_MODE_NAV;
1815         }
1816         else if (navStateFlags & NAV_CTL_EMERG) {
1817             NAV_Status.mode = MW_GPS_MODE_EMERG;
1818         }
1819         else if (navStateFlags & NAV_CTL_POS) {
1820             NAV_Status.mode = MW_GPS_MODE_HOLD;
1821         }
1822     }
1823
1824     NAV_Status.state = navFSM[posControl.navState].mwState;
1825     NAV_Status.error = navFSM[posControl.navState].mwError;
1826
1827     NAV_Status.flags = 0;
1828     if (posControl.flags.isAdjustingPosition)   NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
1829     if (posControl.flags.isAdjustingAltitude)   NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
1830
1831     NAV_Status.activeWpNumber = posControl.activeWaypointIndex + 1;
1832     NAV_Status.activeWpAction = 0;
1833     if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
1834         NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
1835     }
1836 }
1837
1838 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
1839 {
1840     posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos;
1841
1842     switch (mode) {
1843         case RTH_HOME_ENROUTE_INITIAL:
1844             posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
1845             break;
1846
1847         case RTH_HOME_ENROUTE_PROPORTIONAL:
1848             {
1849                 float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
1850                 if (rthTotalDistanceToTravel >= 100) {
1851                     float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
1852                     posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
1853                 }
1854                 else {
1855                     posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1856                 }
1857             }
1858             break;
1859
1860         case RTH_HOME_ENROUTE_FINAL:
1861             posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1862             break;
1863
1864         case RTH_HOME_FINAL_HOVER:
1865             if (navConfig()->general.rth_home_altitude) {
1866                 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
1867             }
1868             else {
1869                 // If home altitude not defined - fall back to final ENROUTE altitude
1870                 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1871             }
1872             break;
1873
1874         case RTH_HOME_FINAL_LAND:
1875             // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
1876             if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
1877                 posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100;   // 100 -> m to cm
1878                 if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
1879                     posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt;  // correct to relative if absolute SL altitude datum used
1880                 }
1881             }
1882             break;
1883     }
1884
1885     return &posControl.rthState.homeTmpWaypoint;
1886 }
1887
1888 /*-----------------------------------------------------------
1889  * Detects if thrust vector is facing downwards
1890  *-----------------------------------------------------------*/
1891 bool isThrustFacingDownwards(void)
1892 {
1893     // Tilt angle <= 80 deg; cos(80) = 0.17364817766693034885171662676931
1894     return (calculateCosTiltAngle() >= 0.173648178f);
1895 }
1896
1897 /*-----------------------------------------------------------
1898  * Checks if position sensor (GPS) is failing for a specified timeout (if enabled)
1899  *-----------------------------------------------------------*/
1900 bool checkForPositionSensorTimeout(void)
1901 {
1902     if (navConfig()->general.pos_failure_timeout) {
1903         if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
1904             return true;
1905         }
1906         else {
1907             return false;
1908         }
1909     }
1910     else {
1911         // Timeout not defined, never fail
1912         return false;
1913     }
1914 }
1915
1916 /*-----------------------------------------------------------
1917  * Processes an update to XY-position and velocity
1918  *-----------------------------------------------------------*/
1919 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
1920 {
1921     posControl.actualState.abs.pos.x = newX;
1922     posControl.actualState.abs.pos.y = newY;
1923     posControl.actualState.abs.vel.x = newVelX;
1924     posControl.actualState.abs.vel.y = newVelY;
1925
1926     posControl.actualState.agl.pos.x = newX;
1927     posControl.actualState.agl.pos.y = newY;
1928     posControl.actualState.agl.vel.x = newVelX;
1929     posControl.actualState.agl.vel.y = newVelY;
1930
1931     posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
1932
1933     // CASE 1: POS & VEL valid
1934     if (estPosValid && estVelValid) {
1935         posControl.flags.estPosStatus = EST_TRUSTED;
1936         posControl.flags.estVelStatus = EST_TRUSTED;
1937         posControl.flags.horizontalPositionDataNew = true;
1938         posControl.lastValidPositionTimeMs = millis();
1939     }
1940     // CASE 1: POS invalid, VEL valid
1941     else if (!estPosValid && estVelValid) {
1942         posControl.flags.estPosStatus = EST_USABLE;     // Pos usable, but not trusted
1943         posControl.flags.estVelStatus = EST_TRUSTED;
1944         posControl.flags.horizontalPositionDataNew = true;
1945         posControl.lastValidPositionTimeMs = millis();
1946     }
1947     // CASE 3: can't use pos/vel data
1948     else {
1949         posControl.flags.estPosStatus = EST_NONE;
1950         posControl.flags.estVelStatus = EST_NONE;
1951         posControl.flags.horizontalPositionDataNew = false;
1952     }
1953
1954     //Update blackbox data
1955     navLatestActualPosition[X] = newX;
1956     navLatestActualPosition[Y] = newY;
1957     navActualVelocity[X] = constrain(newVelX, -32678, 32767);
1958     navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
1959 }
1960
1961 /*-----------------------------------------------------------
1962  * Processes an update to Z-position and velocity
1963  *-----------------------------------------------------------*/
1964 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
1965 {
1966     posControl.actualState.abs.pos.z = newAltitude;
1967     posControl.actualState.abs.vel.z = newVelocity;
1968
1969     posControl.actualState.agl.pos.z = surfaceDistance;
1970     posControl.actualState.agl.vel.z = surfaceVelocity;
1971
1972     // Update altitude that would be used when executing RTH
1973     if (estimateValid) {
1974         updateDesiredRTHAltitude();
1975
1976         // If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
1977         if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
1978             // If we are in terrain-following modes - signal that we should update the surface tracking setpoint
1979             //      NONE/USABLE means that we were flying blind, now we should lock to surface
1980             //updateSurfaceTrackingSetpoint();
1981         }
1982
1983         posControl.flags.estAglStatus = surfaceStatus;  // Could be TRUSTED or USABLE
1984         posControl.flags.estAltStatus = EST_TRUSTED;
1985         posControl.flags.verticalPositionDataNew = true;
1986         posControl.lastValidAltitudeTimeMs = millis();
1987     }
1988     else {
1989         posControl.flags.estAltStatus = EST_NONE;
1990         posControl.flags.estAglStatus = EST_NONE;
1991         posControl.flags.verticalPositionDataNew = false;
1992     }
1993
1994     if (ARMING_FLAG(ARMED)) {
1995         if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
1996             if (posControl.actualState.surfaceMin > 0) {
1997                 posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
1998             }
1999             else {
2000                 posControl.actualState.surfaceMin = surfaceDistance;
2001             }
2002         }
2003     }
2004     else {
2005         posControl.actualState.surfaceMin = -1;
2006     }
2007
2008     //Update blackbox data
2009     navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
2010     navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
2011 }
2012
2013 /*-----------------------------------------------------------
2014  * Processes an update to estimated heading
2015  *-----------------------------------------------------------*/
2016 void updateActualHeading(bool headingValid, int32_t newHeading)
2017 {
2018     /* Update heading. Check if we're acquiring a valid heading for the
2019      * first time and update home heading accordingly.
2020      */
2021     navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
2022     if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
2023         (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
2024         (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
2025
2026         // Home was stored using the fake heading (assuming boot as 0deg). Calculate
2027         // the offset from the fake to the actual yaw and apply the same rotation
2028         // to the home point.
2029         int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
2030
2031         posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
2032         if (posControl.rthState.homePosition.yaw < 0) {
2033             posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
2034         }
2035         if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
2036             posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
2037         }
2038         posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2039     }
2040     posControl.actualState.yaw = newHeading;
2041     posControl.flags.estHeadingStatus = newEstHeading;
2042
2043     /* Precompute sin/cos of yaw angle */
2044     posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2045     posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2046 }
2047
2048 /*-----------------------------------------------------------
2049  * Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
2050  *-----------------------------------------------------------*/
2051 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
2052 {
2053     return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
2054 }
2055
2056 /*-----------------------------------------------------------
2057  * Calculates distance and bearing to destination point
2058  *-----------------------------------------------------------*/
2059 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
2060 {
2061     return fast_fsqrtf(sq(deltaX) + sq(deltaY));
2062 }
2063
2064 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
2065 {
2066     return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
2067 }
2068
2069 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
2070 {
2071     const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2072     const float deltaX = destinationPos->x - posvel->pos.x;
2073     const float deltaY = destinationPos->y - posvel->pos.y;
2074
2075     return calculateDistanceFromDelta(deltaX, deltaY);
2076 }
2077
2078 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
2079 {
2080     const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2081     const float deltaX = destinationPos->x - posvel->pos.x;
2082     const float deltaY = destinationPos->y - posvel->pos.y;
2083
2084     return calculateBearingFromDelta(deltaX, deltaY);
2085 }
2086
2087 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
2088 {
2089     if (posControl.flags.estPosStatus == EST_NONE ||
2090         posControl.flags.estHeadingStatus == EST_NONE) {
2091
2092         return false;
2093     }
2094
2095     const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2096     const float deltaX = destinationPos->x - posvel->pos.x;
2097     const float deltaY = destinationPos->y - posvel->pos.y;
2098
2099     result->distance = calculateDistanceFromDelta(deltaX, deltaY);
2100     result->bearing = calculateBearingFromDelta(deltaX, deltaY);
2101     return true;
2102 }
2103
2104 /*-----------------------------------------------------------
2105  * Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing
2106  *-----------------------------------------------------------*/
2107 bool isWaypointMissed(const navWaypointPosition_t * waypoint)
2108 {
2109     int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
2110     bearingError = wrap_18000(bearingError);
2111
2112     return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
2113 }
2114
2115 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
2116 {
2117     // We consider waypoint reached if within specified radius
2118     posControl.wpDistance = calculateDistanceToDestination(pos);
2119
2120     if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
2121         // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
2122         return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
2123                 || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f));  // 10% margin of desired circular loiter radius
2124     }
2125     else {
2126         return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
2127     }
2128 }
2129
2130 bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome)
2131 {
2132     return isWaypointPositionReached(&waypoint->pos, isWaypointHome);
2133 }
2134
2135 static void updateHomePositionCompatibility(void)
2136 {
2137     geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
2138     GPS_distanceToHome = posControl.homeDistance * 0.01f;
2139     GPS_directionToHome = posControl.homeDirection * 0.01f;
2140 }
2141
2142 // Backdoor for RTH estimator
2143 float getFinalRTHAltitude(void)
2144 {
2145     return posControl.rthState.rthFinalAltitude;
2146 }
2147
2148 /*-----------------------------------------------------------
2149  * Update the RTH Altitudes
2150  *-----------------------------------------------------------*/
2151 static void updateDesiredRTHAltitude(void)
2152 {
2153     if (ARMING_FLAG(ARMED)) {
2154         if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
2155           || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
2156             switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
2157                 case NAV_RTH_CLIMB_STAGE_AT_LEAST:
2158                     posControl.rthState.rthClimbStageAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2159                     break;
2160                 case NAV_RTH_CLIMB_STAGE_EXTRA:
2161                     posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2162                     break;
2163             }
2164
2165             switch (navConfig()->general.flags.rth_alt_control_mode) {
2166                 case NAV_RTH_NO_ALT:
2167                     posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2168                     posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2169                     break;
2170
2171                 case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
2172                     posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
2173                     posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2174                     break;
2175
2176                 case NAV_RTH_MAX_ALT:
2177                     posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z);
2178                     posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2179                     break;
2180
2181                 case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
2182                     posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2183                     posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2184                     break;
2185
2186                 case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
2187                     posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2188                     posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2189                     break;
2190
2191                 case NAV_RTH_CONST_ALT:     // Climb/descend to predefined altitude above home
2192                 default:
2193                     posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2194                     posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2195             }
2196         }
2197     } else {
2198         posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
2199         posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2200         posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
2201     }
2202 }
2203
2204 /*-----------------------------------------------------------
2205  * RTH sanity test logic
2206  *-----------------------------------------------------------*/
2207 void initializeRTHSanityChecker(void)
2208 {
2209     const timeMs_t currentTimeMs = millis();
2210
2211     posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2212     posControl.rthSanityChecker.rthSanityOK = true;
2213     posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2214 }
2215
2216 bool validateRTHSanityChecker(void)
2217 {
2218     const timeMs_t currentTimeMs = millis();
2219
2220     // Ability to disable sanity checker
2221     if (navConfig()->general.rth_abort_threshold == 0) {
2222         return true;
2223     }
2224
2225     // Check at 10Hz rate
2226     if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
2227         const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2228         posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2229
2230         if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) {
2231             posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
2232         } else {
2233             // If while doing RTH we got even farther away from home - RTH is doing something crazy
2234             posControl.rthSanityChecker.rthSanityOK = (currentDistanceToHome - posControl.rthSanityChecker.minimalDistanceToHome) < navConfig()->general.rth_abort_threshold;
2235         }
2236     }
2237
2238     return posControl.rthSanityChecker.rthSanityOK;
2239 }
2240
2241 /*-----------------------------------------------------------
2242  * Reset home position to current position
2243  *-----------------------------------------------------------*/
2244 void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
2245 {
2246     // XY-position
2247     if ((useMask & NAV_POS_UPDATE_XY) != 0) {
2248         posControl.rthState.homePosition.pos.x = pos->x;
2249         posControl.rthState.homePosition.pos.y = pos->y;
2250         if (homeFlags & NAV_HOME_VALID_XY) {
2251             posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
2252         } else {
2253             posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
2254         }
2255     }
2256
2257     // Z-position
2258     if ((useMask & NAV_POS_UPDATE_Z) != 0) {
2259         posControl.rthState.homePosition.pos.z = pos->z;
2260         if (homeFlags & NAV_HOME_VALID_Z) {
2261             posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
2262         } else {
2263             posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
2264         }
2265     }
2266
2267     // Heading
2268     if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
2269         // Heading
2270         posControl.rthState.homePosition.yaw = yaw;
2271         if (homeFlags & NAV_HOME_VALID_HEADING) {
2272             posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2273         } else {
2274             posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING;
2275         }
2276     }
2277
2278     posControl.homeDistance = 0;
2279     posControl.homeDirection = 0;
2280
2281     // Update target RTH altitude as a waypoint above home
2282     updateDesiredRTHAltitude();
2283
2284     updateHomePositionCompatibility();
2285     ENABLE_STATE(GPS_FIX_HOME);
2286 }
2287
2288 static navigationHomeFlags_t navigationActualStateHomeValidity(void)
2289 {
2290     navigationHomeFlags_t flags = 0;
2291
2292     if (posControl.flags.estPosStatus >= EST_USABLE) {
2293         flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
2294     }
2295
2296     if (posControl.flags.estHeadingStatus >= EST_USABLE) {
2297         flags |= NAV_HOME_VALID_HEADING;
2298     }
2299
2300     return flags;
2301 }
2302
2303 #if defined(USE_SAFE_HOME)
2304
2305 void checkSafeHomeState(bool shouldBeEnabled)
2306 {
2307         if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
2308                 shouldBeEnabled = false;
2309         } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
2310                 // if safehomes are only used with failsafe and we're trying to enable safehome
2311                 // then enable the safehome only with failsafe
2312                 shouldBeEnabled = posControl.flags.forcedRTHActivated;
2313         }
2314     // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
2315         if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
2316                 return;
2317         }
2318     if (shouldBeEnabled) {
2319                 // set home to safehome
2320         setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2321                 safehome_applied = true;
2322         } else {
2323                 // set home to original arming point
2324         setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2325                 safehome_applied = false;
2326         }
2327         // if we've changed the home position, update the distance and direction
2328     updateHomePosition();
2329 }
2330
2331 /***********************************************************
2332  *  See if there are any safehomes near where we are arming.
2333  *  If so, save the nearest one in case we need it later for RTH.
2334  **********************************************************/
2335 bool findNearestSafeHome(void)
2336 {
2337     safehome_index = -1;
2338     uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
2339     uint32_t distance_to_current;
2340     fpVector3_t currentSafeHome;
2341     gpsLocation_t shLLH;
2342     shLLH.alt = 0;
2343     for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
2344                 if (!safeHomeConfig(i)->enabled)
2345                     continue;
2346
2347         shLLH.lat = safeHomeConfig(i)->lat;
2348         shLLH.lon = safeHomeConfig(i)->lon;
2349         geoConvertGeodeticToLocal(&currentSafeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
2350         distance_to_current = calculateDistanceToDestination(&currentSafeHome);
2351         if (distance_to_current < nearest_safehome_distance) {
2352              // this safehome is the nearest so far - keep track of it.
2353              safehome_index = i;
2354              nearest_safehome_distance = distance_to_current;
2355              nearestSafeHome.x = currentSafeHome.x;
2356              nearestSafeHome.y = currentSafeHome.y;
2357              nearestSafeHome.z = currentSafeHome.z;
2358         }
2359     }
2360     if (safehome_index >= 0) {
2361                 safehome_distance = nearest_safehome_distance;
2362     } else {
2363         safehome_distance = 0;
2364     }
2365     return safehome_distance > 0;
2366 }
2367 #endif
2368
2369 /*-----------------------------------------------------------
2370  * Update home position, calculate distance and bearing to home
2371  *-----------------------------------------------------------*/
2372 void updateHomePosition(void)
2373 {
2374     // Disarmed and have a valid position, constantly update home
2375     if (!ARMING_FLAG(ARMED)) {
2376         if (posControl.flags.estPosStatus >= EST_USABLE) {
2377             const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
2378             bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
2379             switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
2380                 case NAV_RESET_NEVER:
2381                     break;
2382                 case NAV_RESET_ON_FIRST_ARM:
2383                     setHome |= !ARMING_FLAG(WAS_EVER_ARMED);
2384                     break;
2385                 case NAV_RESET_ON_EACH_ARM:
2386                     setHome = true;
2387                     break;
2388             }
2389             if (setHome) {
2390 #if defined(USE_SAFE_HOME)
2391                 findNearestSafeHome();
2392 #endif
2393                 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2394                 // save the current location in case it is replaced by a safehome or HOME_RESET
2395                 original_rth_home.x = posControl.rthState.homePosition.pos.x;
2396                 original_rth_home.y = posControl.rthState.homePosition.pos.y;
2397                 original_rth_home.z = posControl.rthState.homePosition.pos.z;
2398             }
2399         }
2400     }
2401     else {
2402         static bool isHomeResetAllowed = false;
2403
2404         // If pilot so desires he may reset home position to current position
2405         if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
2406             if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
2407                 const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
2408                 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
2409                 isHomeResetAllowed = false;
2410             }
2411         }
2412         else {
2413             isHomeResetAllowed = true;
2414         }
2415
2416         // Update distance and direction to home if armed (home is not updated when armed)
2417         if (STATE(GPS_FIX_HOME)) {
2418             fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
2419             posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
2420             posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
2421             updateHomePositionCompatibility();
2422         }
2423     }
2424 }
2425
2426 /* -----------------------------------------------------------
2427  * Override RTH preset altitude and Climb First option
2428  * using Pitch/Roll stick held for > 1 seconds
2429  * Climb First override limited to Fixed Wing only
2430  *-----------------------------------------------------------*/
2431 static bool rthAltControlStickOverrideCheck(unsigned axis)
2432 {
2433     if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) {
2434         return false;
2435     }
2436     static timeMs_t rthOverrideStickHoldStartTime[2];
2437
2438     if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
2439         timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis];
2440
2441         if (!rthOverrideStickHoldStartTime[axis]) {
2442             rthOverrideStickHoldStartTime[axis] = millis();
2443         } else if (ABS(1500 - holdTime) < 500) {    // 1s delay to activate, activation duration limited to 1 sec
2444             if (axis == PITCH) {           // PITCH down to override preset altitude reset to current altitude
2445                 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2446                 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2447                 return true;
2448             } else if (axis == ROLL) {     // ROLL right to override climb first
2449                 return true;
2450             }
2451         }
2452     } else {
2453         rthOverrideStickHoldStartTime[axis] = 0;
2454     }
2455
2456     return false;
2457 }
2458
2459 /* ---------------------------------------------------
2460  * If climb stage is being used, see if it is time to
2461  * transiton in to turn.
2462  * Limited to fixed wing only.
2463  * --------------------------------------------------- */
2464  bool rthClimbStageActiveAndComplete() {
2465     if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
2466         if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
2467             return true;
2468         }
2469     }
2470
2471     return false;
2472  }
2473
2474 /*-----------------------------------------------------------
2475  * Update flight statistics
2476  *-----------------------------------------------------------*/
2477 static void updateNavigationFlightStatistics(void)
2478 {
2479     static timeMs_t previousTimeMs = 0;
2480     const timeMs_t currentTimeMs = millis();
2481     const timeDelta_t timeDeltaMs = currentTimeMs - previousTimeMs;
2482     previousTimeMs = currentTimeMs;
2483
2484     if (ARMING_FLAG(ARMED)) {
2485         posControl.totalTripDistance += posControl.actualState.velXY * MS2S(timeDeltaMs);
2486     }
2487 }
2488
2489 uint32_t getTotalTravelDistance(void)
2490 {
2491     return lrintf(posControl.totalTripDistance);
2492 }
2493
2494 /*-----------------------------------------------------------
2495  * Calculate platform-specific hold position (account for deceleration)
2496  *-----------------------------------------------------------*/
2497 void calculateInitialHoldPosition(fpVector3_t * pos)
2498 {
2499     if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2500         calculateFixedWingInitialHoldPosition(pos);
2501     }
2502     else {
2503         calculateMulticopterInitialHoldPosition(pos);
2504     }
2505 }
2506
2507 /*-----------------------------------------------------------
2508  * Set active XYZ-target and desired heading
2509  *-----------------------------------------------------------*/
2510 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
2511 {
2512     // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
2513     if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
2514         posControl.desiredState.pos.x = pos->x;
2515         posControl.desiredState.pos.y = pos->y;
2516     }
2517
2518     // Z-position
2519     if ((useMask & NAV_POS_UPDATE_Z) != 0) {
2520         updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);   // Reset RoC/RoD -> altitude controller
2521         posControl.desiredState.pos.z = pos->z;
2522     }
2523
2524     // Heading
2525     if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
2526         // Heading
2527         posControl.desiredState.yaw = yaw;
2528     }
2529     else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) {
2530         posControl.desiredState.yaw = calculateBearingToDestination(pos);
2531     }
2532     else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
2533         posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
2534     }
2535 }
2536
2537 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
2538 {
2539     farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
2540     farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
2541     farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
2542 }
2543
2544 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance)
2545 {
2546     origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
2547     origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
2548     origin->z = origin->z;
2549 }
2550
2551 /*-----------------------------------------------------------
2552  * NAV land detector
2553  *-----------------------------------------------------------*/
2554 void resetLandingDetector(void)
2555 {
2556     if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2557         resetFixedWingLandingDetector();
2558     }
2559     else {
2560         resetMulticopterLandingDetector();
2561     }
2562 }
2563
2564 bool isLandingDetected(void)
2565 {
2566     bool landingDetected;
2567
2568     if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2569         landingDetected = isFixedWingLandingDetected();
2570     }
2571     else {
2572         landingDetected = isMulticopterLandingDetected();
2573     }
2574
2575     return landingDetected;
2576 }
2577
2578 /*-----------------------------------------------------------
2579  * Z-position controller
2580  *-----------------------------------------------------------*/
2581 void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
2582 {
2583     static timeUs_t lastUpdateTimeUs;
2584     timeUs_t currentTimeUs = micros();
2585
2586     // Terrain following uses different altitude measurement
2587     const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
2588
2589     if (mode == ROC_TO_ALT_RESET) {
2590         lastUpdateTimeUs = currentTimeUs;
2591         posControl.desiredState.pos.z = altitudeToUse;
2592     }
2593     else {
2594
2595         /*
2596          * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
2597          * In other words, when altitude is reached, allow it only to shrink
2598          */
2599         if (navConfig()->general.max_altitude > 0 &&
2600             altitudeToUse >= navConfig()->general.max_altitude &&
2601             desiredClimbRate > 0
2602         ) {
2603             desiredClimbRate = 0;
2604         }
2605
2606         if (STATE(FIXED_WING_LEGACY)) {
2607             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
2608             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
2609
2610             if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
2611                 posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
2612                 posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500);    // FIXME: calculate sanity limits in a smarter way
2613             }
2614         }
2615         else {
2616             // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
2617             posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
2618         }
2619
2620         lastUpdateTimeUs = currentTimeUs;
2621     }
2622 }
2623
2624 static void resetAltitudeController(bool useTerrainFollowing)
2625 {
2626     // Set terrain following flag
2627     posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
2628
2629     if (STATE(FIXED_WING_LEGACY)) {
2630         resetFixedWingAltitudeController();
2631     }
2632     else {
2633         resetMulticopterAltitudeController();
2634     }
2635 }
2636
2637 static void setupAltitudeController(void)
2638 {
2639     if (STATE(FIXED_WING_LEGACY)) {
2640         setupFixedWingAltitudeController();
2641     }
2642     else {
2643         setupMulticopterAltitudeController();
2644     }
2645 }
2646
2647 static bool adjustAltitudeFromRCInput(void)
2648 {
2649     if (STATE(FIXED_WING_LEGACY)) {
2650         return adjustFixedWingAltitudeFromRCInput();
2651     }
2652     else {
2653         return adjustMulticopterAltitudeFromRCInput();
2654     }
2655 }
2656
2657 /*-----------------------------------------------------------
2658  * Jump Counter support functions
2659  *-----------------------------------------------------------*/
2660 static void setupJumpCounters(void)
2661 {
2662     for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
2663         if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
2664             posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
2665         }
2666     }
2667 }
2668
2669 static void resetJumpCounter(void)
2670 {
2671         // reset the volatile counter from the set / static value
2672     posControl.waypointList[posControl.activeWaypointIndex].p3 =
2673         posControl.waypointList[posControl.activeWaypointIndex].p2;
2674 }
2675
2676 static void clearJumpCounters(void)
2677 {
2678     for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
2679         if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
2680             posControl.waypointList[wp].p3 = 0;
2681         }
2682     }
2683 }
2684
2685
2686
2687 /*-----------------------------------------------------------
2688  * Heading controller (pass-through to MAG mode)
2689  *-----------------------------------------------------------*/
2690 static void resetHeadingController(void)
2691 {
2692     if (STATE(FIXED_WING_LEGACY)) {
2693         resetFixedWingHeadingController();
2694     }
2695     else {
2696         resetMulticopterHeadingController();
2697     }
2698 }
2699
2700 static bool adjustHeadingFromRCInput(void)
2701 {
2702     if (STATE(FIXED_WING_LEGACY)) {
2703         return adjustFixedWingHeadingFromRCInput();
2704     }
2705     else {
2706         return adjustMulticopterHeadingFromRCInput();
2707     }
2708 }
2709
2710 /*-----------------------------------------------------------
2711  * XY Position controller
2712  *-----------------------------------------------------------*/
2713 static void resetPositionController(void)
2714 {
2715     if (STATE(FIXED_WING_LEGACY)) {
2716         resetFixedWingPositionController();
2717     }
2718     else {
2719         resetMulticopterPositionController();
2720         resetMulticopterBrakingMode();
2721     }
2722 }
2723
2724 static bool adjustPositionFromRCInput(void)
2725 {
2726     bool retValue;
2727
2728     if (STATE(FIXED_WING_LEGACY)) {
2729         retValue = adjustFixedWingPositionFromRCInput();
2730     }
2731     else {
2732
2733         const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
2734         const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
2735
2736         retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
2737     }
2738
2739     return retValue;
2740 }
2741
2742 /*-----------------------------------------------------------
2743  * WP controller
2744  *-----------------------------------------------------------*/
2745 void resetGCSFlags(void)
2746 {
2747     posControl.flags.isGCSAssistedNavigationReset = false;
2748     posControl.flags.isGCSAssistedNavigationEnabled = false;
2749 }
2750
2751 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
2752 {
2753     /* Default waypoint to send */
2754     wpData->action = NAV_WP_ACTION_RTH;
2755     wpData->lat = 0;
2756     wpData->lon = 0;
2757     wpData->alt = 0;
2758     wpData->p1 = 0;
2759     wpData->p2 = 0;
2760     wpData->p3 = 0;
2761     wpData->flag = NAV_WP_FLAG_LAST;
2762
2763     // WP #0 - special waypoint - HOME
2764     if (wpNumber == 0) {
2765         if (STATE(GPS_FIX_HOME)) {
2766             wpData->lat = GPS_home.lat;
2767             wpData->lon = GPS_home.lon;
2768             wpData->alt = GPS_home.alt;
2769         }
2770     }
2771     // WP #255 - special waypoint - directly get actualPosition
2772     else if (wpNumber == 255) {
2773         gpsLocation_t wpLLH;
2774
2775         geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
2776
2777         wpData->lat = wpLLH.lat;
2778         wpData->lon = wpLLH.lon;
2779         wpData->alt = wpLLH.alt;
2780     }
2781     // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode
2782     else if (wpNumber == 254) {
2783         navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
2784
2785         if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
2786             gpsLocation_t wpLLH;
2787
2788             geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
2789
2790             wpData->lat = wpLLH.lat;
2791             wpData->lon = wpLLH.lon;
2792             wpData->alt = wpLLH.alt;
2793         }
2794     }
2795     // WP #1 - #60 - common waypoints - pre-programmed mission
2796     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
2797         if (wpNumber <= posControl.waypointCount) {
2798             *wpData = posControl.waypointList[wpNumber - 1];
2799             if(wpData->action == NAV_WP_ACTION_JUMP) {
2800                 wpData->p1 += 1; // make WP # (vice index)
2801             }
2802
2803         }
2804     }
2805 }
2806
2807 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
2808 {
2809     gpsLocation_t wpLLH;
2810     navWaypointPosition_t wpPos;
2811
2812     // Pre-fill structure to convert to local coordinates
2813     wpLLH.lat = wpData->lat;
2814     wpLLH.lon = wpData->lon;
2815     wpLLH.alt = wpData->alt;
2816
2817     // WP #0 - special waypoint - HOME
2818     if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
2819         // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
2820         geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
2821         setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
2822     }
2823     // WP #255 - special waypoint - directly set desiredPosition
2824     // Only valid when armed and in poshold mode
2825     else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
2826              ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
2827              (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
2828         // Convert to local coordinates
2829         geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
2830
2831         navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
2832
2833         // If we received global altitude == 0, use current altitude
2834         if (wpData->alt != 0) {
2835             waypointUpdateFlags |= NAV_POS_UPDATE_Z;
2836         }
2837
2838         if (wpData->p1 > 0 && wpData->p1 < 360) {
2839             waypointUpdateFlags |= NAV_POS_UPDATE_HEADING;
2840         }
2841
2842         setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
2843     }
2844     // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
2845     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
2846         if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
2847             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
2848             static int8_t nonGeoWaypointCount = 0;
2849
2850             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
2851                 if (wpNumber == 1) {
2852                     resetWaypointList();
2853                 }
2854                 posControl.waypointList[wpNumber - 1] = *wpData;
2855                 if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
2856                     nonGeoWaypointCount += 1;
2857                     if(wpData->action == NAV_WP_ACTION_JUMP) {
2858                         posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
2859                     }
2860                 }
2861
2862                 posControl.waypointCount = wpNumber;
2863                 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
2864                 posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
2865                 if (posControl.waypointListValid) {
2866                     nonGeoWaypointCount = 0;
2867                 }
2868             }
2869         }
2870     }
2871 }
2872
2873 void resetWaypointList(void)
2874 {
2875     /* Can only reset waypoint list if not armed */
2876     if (!ARMING_FLAG(ARMED)) {
2877         posControl.waypointCount = 0;
2878         posControl.waypointListValid = false;
2879         posControl.geoWaypointCount = 0;
2880 #ifdef USE_MULTI_MISSION
2881         posControl.loadedMultiMissionIndex = 0;
2882         posControl.loadedMultiMissionStartWP = 0;
2883         posControl.loadedMultiMissionWPCount = 0;
2884 #endif
2885     }
2886 }
2887
2888 bool isWaypointListValid(void)
2889 {
2890     return posControl.waypointListValid;
2891 }
2892
2893 int getWaypointCount(void)
2894 {
2895     return posControl.waypointCount;
2896 }
2897 #ifdef USE_MULTI_MISSION
2898 void selectMultiMissionIndex(int8_t increment)
2899 {
2900     if (posControl.multiMissionCount > 1) {     // stick selection only active when multi mission loaded
2901         navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 0, posControl.multiMissionCount);
2902     }
2903 }
2904
2905 void setMultiMissionOnArm(void)
2906 {
2907     if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) {
2908         posControl.waypointCount = posControl.loadedMultiMissionWPCount;
2909
2910         for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
2911             posControl.waypointList[i] = posControl.waypointList[i + posControl.loadedMultiMissionStartWP];
2912             if (i == posControl.waypointCount - 1) {
2913                 break;
2914             }
2915         }
2916
2917         posControl.loadedMultiMissionStartWP = 0;
2918         posControl.loadedMultiMissionWPCount = 0;
2919     }
2920 }
2921
2922 bool checkMissionCount(int8_t waypoint)
2923 {
2924     if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
2925         posControl.multiMissionCount += 1;  // count up no missions in multi mission WP file
2926         if (waypoint != NAV_MAX_WAYPOINTS - 1) {
2927             return (nonVolatileWaypointList(waypoint + 1)->flag == NAV_WP_FLAG_LAST &&
2928                     nonVolatileWaypointList(waypoint + 1)->action ==NAV_WP_ACTION_RTH);
2929             // end of multi mission file if successive NAV_WP_FLAG_LAST and default action (RTH)
2930         }
2931     }
2932     return false;
2933 }
2934 #endif  // multi mission
2935 #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
2936 bool loadNonVolatileWaypointList(bool clearIfLoaded)
2937 {
2938 #ifdef USE_MULTI_MISSION
2939     /* multi_mission_index 0 only used for non NVM missions - don't load.
2940      * Don't load if mission planner WP count > 0 */
2941     if (ARMING_FLAG(ARMED) || !navConfig()->general.waypoint_multi_mission_index || posControl.wpPlannerActiveWPIndex) {
2942 #else
2943     if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
2944 #endif
2945         return false;
2946     }
2947
2948     // if forced and waypoints are already loaded, just unload them.
2949     if (clearIfLoaded && posControl.waypointCount > 0) {
2950         resetWaypointList();
2951         return false;
2952     }
2953 #ifdef USE_MULTI_MISSION
2954     /* Reset multi mission index to 1 if exceeds number of available missions */
2955     if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
2956         navConfigMutable()->general.waypoint_multi_mission_index = 1;
2957     }
2958     posControl.multiMissionCount = 0;
2959     posControl.loadedMultiMissionWPCount = 0;
2960     int8_t loadedMultiMissionGeoWPCount;
2961 #endif
2962     for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
2963         setWaypoint(i + 1, nonVolatileWaypointList(i));
2964 #ifdef USE_MULTI_MISSION
2965         /* store details of selected mission */
2966         if ((posControl.multiMissionCount + 1 == navConfig()->general.waypoint_multi_mission_index)) {
2967             // mission start WP
2968             if (posControl.loadedMultiMissionWPCount == 0) {
2969                 posControl.loadedMultiMissionWPCount = 1;   // start marker only, value here unimportant (but not 0)
2970                 posControl.loadedMultiMissionStartWP = i;
2971                 loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
2972             }
2973             // mission end WP
2974             if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
2975                 posControl.loadedMultiMissionWPCount = i - posControl.loadedMultiMissionStartWP + 1;
2976                 loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1;
2977             }
2978         }
2979
2980         /* count up number of missions */
2981         if (checkMissionCount(i)) {
2982             break;
2983         }
2984     }
2985
2986     posControl.geoWaypointCount = loadedMultiMissionGeoWPCount;
2987     posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0;
2988
2989     /* Mission sanity check failed - reset the list
2990      * Also reset if no selected mission loaded (shouldn't happen) */
2991     if (!posControl.waypointListValid || !posControl.loadedMultiMissionWPCount) {
2992 #else
2993         // check this is the last waypoint
2994         if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
2995             break;
2996         }
2997     }
2998     // Mission sanity check failed - reset the list
2999     if (!posControl.waypointListValid) {
3000 #endif
3001         resetWaypointList();
3002     }
3003
3004     return posControl.waypointListValid;
3005 }
3006
3007 bool saveNonVolatileWaypointList(void)
3008 {
3009     if (ARMING_FLAG(ARMED) || !posControl.waypointListValid)
3010         return false;
3011
3012     for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3013         getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
3014     }
3015 #ifdef USE_MULTI_MISSION
3016     navConfigMutable()->general.waypoint_multi_mission_index = 1;    // reset selected mission to 1 when new entries saved
3017 #endif
3018     saveConfigAndNotify();
3019
3020     return true;
3021 }
3022 #endif
3023
3024 #if defined(USE_SAFE_HOME)
3025
3026 void resetSafeHomes(void)
3027 {
3028     memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
3029 }
3030 #endif
3031
3032 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
3033 {
3034     gpsLocation_t wpLLH;
3035
3036     /* Default to home position if lat & lon = 0 or HOME flag set
3037      * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
3038     if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
3039         wpLLH.lat = GPS_home.lat;
3040         wpLLH.lon = GPS_home.lon;
3041     } else {
3042         wpLLH.lat = waypoint->lat;
3043         wpLLH.lon = waypoint->lon;
3044     }
3045     wpLLH.alt = waypoint->alt;
3046
3047     geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
3048 }
3049
3050 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
3051 {
3052     posControl.activeWaypoint.pos = *pos;
3053
3054     // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
3055     posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
3056
3057     // Set desired position to next waypoint (XYZ-controller)
3058     setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
3059 }
3060
3061 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
3062 {
3063     return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
3064 }
3065
3066 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
3067 {
3068     fpVector3_t localPos;
3069     mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
3070     calculateAndSetActiveWaypointToLocalPosition(&localPos);
3071 }
3072
3073 /* Checks if active waypoint is last in mission */
3074 bool isLastMissionWaypoint(void)
3075 {
3076     return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
3077             (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
3078 }
3079
3080 /* Checks if Nav hold position is active */
3081 bool isNavHoldPositionActive(void)
3082 {
3083     if (FLIGHT_MODE(NAV_WP_MODE)) {     // WP mode last WP hold and Timed hold positions
3084         return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
3085     }
3086     // RTH spiral climb and Home positions and POSHOLD position
3087     return FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_POSHOLD_MODE);
3088 }
3089
3090 float getActiveWaypointSpeed(void)
3091 {
3092     if (posControl.flags.isAdjustingPosition) {
3093         // In manual control mode use different cap for speed
3094         return navConfig()->general.max_manual_speed;
3095     }
3096     else {
3097         uint16_t waypointSpeed = navConfig()->general.auto_speed;
3098
3099         if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
3100             if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
3101                 float wpSpecificSpeed = 0.0f;
3102                 if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
3103                     wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
3104                 else
3105                     wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
3106
3107                 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
3108                     waypointSpeed = wpSpecificSpeed;
3109                 } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
3110                     waypointSpeed = navConfig()->general.max_auto_speed;
3111                 }
3112             }
3113         }
3114
3115         return waypointSpeed;
3116     }
3117 }
3118
3119 /*-----------------------------------------------------------
3120  * Process adjustments to alt, pos and yaw controllers
3121  *-----------------------------------------------------------*/
3122 static void processNavigationRCAdjustments(void)
3123 {
3124     /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
3125     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3126     if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
3127         posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
3128     }
3129     else {
3130         posControl.flags.isAdjustingAltitude = false;
3131     }
3132
3133     if (navStateFlags & NAV_RC_POS) {
3134         if (!FLIGHT_MODE(FAILSAFE_MODE)) {
3135             posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
3136         }
3137         else {
3138             if (!STATE(FIXED_WING_LEGACY)) {
3139                 resetMulticopterBrakingMode();
3140             }
3141         }
3142     }
3143     else {
3144         posControl.flags.isAdjustingPosition = false;
3145     }
3146
3147     if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
3148         posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
3149     }
3150     else {
3151         posControl.flags.isAdjustingHeading = false;
3152     }
3153 }
3154
3155 /*-----------------------------------------------------------
3156  * A main function to call position controllers at loop rate
3157  *-----------------------------------------------------------*/
3158 void applyWaypointNavigationAndAltitudeHold(void)
3159 {
3160     const timeUs_t currentTimeUs = micros();
3161
3162     //Updata blackbox data
3163     navFlags = 0;
3164     if (posControl.flags.estAltStatus == EST_TRUSTED)       navFlags |= (1 << 0);
3165     if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
3166     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
3167     if (posControl.flags.isTerrainFollowEnabled)            navFlags |= (1 << 3);
3168 #if defined(NAV_GPS_GLITCH_DETECTION)
3169     if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
3170 #endif
3171     if (posControl.flags.estHeadingStatus == EST_TRUSTED)   navFlags |= (1 << 5);
3172
3173     // Reset all navigation requests - NAV controllers will set them if necessary
3174     DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
3175
3176     // No navigation when disarmed
3177     if (!ARMING_FLAG(ARMED)) {
3178         // If we are disarmed, abort forced RTH or Emergency Landing
3179         posControl.flags.forcedRTHActivated = false;
3180         posControl.flags.forcedEmergLandingActivated = false;
3181         //  ensure WP missions always restart from first waypoint after disarm
3182         posControl.activeWaypointIndex = 0;
3183         return;
3184     }
3185
3186     /* Reset flags */
3187     posControl.flags.horizontalPositionDataConsumed = false;
3188     posControl.flags.verticalPositionDataConsumed = false;
3189
3190     /* Process controllers */
3191     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3192     if (STATE(ROVER) || STATE(BOAT)) {
3193         applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
3194     } else if (STATE(FIXED_WING_LEGACY)) {
3195         applyFixedWingNavigationController(navStateFlags, currentTimeUs);
3196     }
3197     else {
3198         applyMulticopterNavigationController(navStateFlags, currentTimeUs);
3199     }
3200
3201     /* Consume position data */
3202     if (posControl.flags.horizontalPositionDataConsumed)
3203         posControl.flags.horizontalPositionDataNew = false;
3204
3205     if (posControl.flags.verticalPositionDataConsumed)
3206         posControl.flags.verticalPositionDataNew = false;
3207
3208     //Update blackbox data
3209     if (posControl.flags.isAdjustingPosition)       navFlags |= (1 << 6);
3210     if (posControl.flags.isAdjustingAltitude)       navFlags |= (1 << 7);
3211     if (posControl.flags.isAdjustingHeading)        navFlags |= (1 << 8);
3212
3213     navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
3214     navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
3215     navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
3216 }
3217
3218 /*-----------------------------------------------------------
3219  * Set CF's FLIGHT_MODE from current NAV_MODE
3220  *-----------------------------------------------------------*/
3221 void switchNavigationFlightModes(void)
3222 {
3223     const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
3224     const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
3225     DISABLE_FLIGHT_MODE(disabledFlightModes);
3226     ENABLE_FLIGHT_MODE(enabledNavFlightModes);
3227 }
3228
3229 /*-----------------------------------------------------------
3230  * desired NAV_MODE from combination of FLIGHT_MODE flags
3231  *-----------------------------------------------------------*/
3232 static bool canActivateAltHoldMode(void)
3233 {
3234     return (posControl.flags.estAltStatus >= EST_USABLE);
3235 }
3236
3237 static bool canActivatePosHoldMode(void)
3238 {
3239     return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
3240 }
3241
3242 static bool canActivateNavigationModes(void)
3243 {
3244     return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
3245 }
3246
3247 static bool isWaypointMissionValid(void)
3248 {
3249     return posControl.waypointListValid && (posControl.waypointCount > 0);
3250 }
3251
3252 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
3253 {
3254     static bool canActivateWaypoint = false;
3255     static bool canActivateLaunchMode = false;
3256
3257     //We can switch modes only when ARMED
3258     if (ARMING_FLAG(ARMED)) {
3259         // Ask failsafe system if we can use navigation system
3260         if (failsafeBypassNavigation()) {
3261             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3262         }
3263
3264         // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
3265         const bool canActivateAltHold    = canActivateAltHoldMode();
3266         const bool canActivatePosHold    = canActivatePosHoldMode();
3267         const bool canActivateNavigation = canActivateNavigationModes();
3268         const bool isExecutingRTH        = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
3269         checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
3270
3271         /* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */
3272         if (posControl.flags.forcedEmergLandingActivated) {
3273             return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
3274         }
3275
3276         /* Keep Emergency landing mode active once triggered. Is cancelled when landing in progress if position sensors working again.
3277          * If failsafe not active landing also cancelled if WP or RTH deselected or if Manual or Althold modes selected.
3278          * Remains active if landing finished regardless of sensor status or flight mode selection
3279          * RTH Sanity check emergency landing remains active so long as RTH remains selected */
3280         if (navigationIsExecutingAnEmergencyLanding()) {
3281             bool autonomousNavIsPossible = canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME);
3282             bool emergLandingCancel = (!autonomousNavIsPossible && !(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH))) ||
3283                                       (autonomousNavIsPossible && !IS_RC_MODE_ACTIVE(BOXNAVRTH));
3284
3285             if ((!posControl.rthSanityChecker.rthSanityOK || !autonomousNavIsPossible) && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) {
3286                 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
3287             }
3288         }
3289         posControl.rthSanityChecker.rthSanityOK = true;
3290
3291         // Keep canActivateWaypoint flag at FALSE if there is no mission loaded
3292         // Also block WP mission if we are executing RTH
3293         if (!isWaypointMissionValid() || isExecutingRTH) {
3294             canActivateWaypoint = false;
3295         }
3296
3297         /* Airplane specific modes */
3298         if (STATE(AIRPLANE)) {
3299             // LAUNCH mode has priority over any other NAV mode
3300             if (isNavLaunchEnabled()) {     // FIXME: Only available for fixed wing aircrafts now
3301                 if (canActivateLaunchMode) {
3302                     canActivateLaunchMode = false;
3303                     return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
3304                 }
3305                 else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
3306                     // Make sure we don't bail out to IDLE
3307                     return NAV_FSM_EVENT_NONE;
3308                 }
3309             }
3310             else {
3311                 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
3312                 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
3313                     throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
3314                     if (throttleStatus != THROTTLE_LOW)
3315                         return NAV_FSM_EVENT_NONE;
3316                     else
3317                         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3318                 }
3319             }
3320
3321             /* Soaring mode, disables altitude control in Position hold and Course hold modes.
3322              * Pitch allowed to freefloat within defined Angle mode deadband */
3323             if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
3324                 if (!FLIGHT_MODE(SOARING_MODE)) {
3325                     ENABLE_FLIGHT_MODE(SOARING_MODE);
3326                 }
3327             } else {
3328                 DISABLE_FLIGHT_MODE(SOARING_MODE);
3329             }
3330         }
3331
3332         // Failsafe_RTH (can override MANUAL)
3333         if (posControl.flags.forcedRTHActivated) {
3334             // If we request forced RTH - attempt to activate it no matter what
3335             // This might switch to emergency landing controller if GPS is unavailable
3336             return NAV_FSM_EVENT_SWITCH_TO_RTH;
3337         }
3338
3339         /* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
3340          * Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
3341          * Also prevent WP falling back to RTH if WP mission planner is active */
3342         const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive;
3343         if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) {
3344             // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
3345             // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold
3346             // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
3347             // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
3348             if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
3349                 return NAV_FSM_EVENT_SWITCH_TO_RTH;
3350             }
3351         }
3352
3353         // MANUAL mode has priority over WP/PH/AH
3354         if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
3355             canActivateWaypoint = false;    // Block WP mode if we are in PASSTHROUGH mode
3356             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3357         }
3358
3359         // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
3360         // Block activation if using WP Mission Planner
3361         if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
3362             if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
3363                 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
3364         }
3365         else {
3366             // Arm the state variable if the WP BOX mode is not enabled
3367             canActivateWaypoint = true;
3368         }
3369
3370         if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
3371             if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
3372                 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
3373         }
3374
3375         // CRUISE has priority over COURSE_HOLD and AH
3376         if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
3377             if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
3378                 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
3379         }
3380
3381         // PH has priority over COURSE_HOLD
3382         // CRUISE has priority on AH
3383         if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
3384
3385             if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
3386                 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
3387
3388             if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
3389                 return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
3390
3391         }
3392
3393         if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
3394             if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
3395                 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
3396         }
3397     }
3398     else {
3399         canActivateWaypoint = false;
3400
3401         // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight)
3402         canActivateLaunchMode = isNavLaunchEnabled();
3403     }
3404
3405     return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3406 }
3407
3408 /*-----------------------------------------------------------
3409  * An indicator that throttle tilt compensation is forced
3410  *-----------------------------------------------------------*/
3411 bool navigationRequiresThrottleTiltCompensation(void)
3412 {
3413     return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
3414 }
3415
3416 /*-----------------------------------------------------------
3417  * An indicator that ANGLE mode must be forced per NAV requirement
3418  *-----------------------------------------------------------*/
3419 bool navigationRequiresAngleMode(void)
3420 {
3421     const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
3422     return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
3423 }
3424
3425 /*-----------------------------------------------------------
3426  * An indicator that TURN ASSISTANCE is required for navigation
3427  *-----------------------------------------------------------*/
3428 bool navigationRequiresTurnAssistance(void)
3429 {
3430     const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
3431     if (STATE(FIXED_WING_LEGACY)) {
3432         // For airplanes turn assistant is always required when controlling position
3433         return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
3434     }
3435     else {
3436         return false;
3437     }
3438 }
3439
3440 /**
3441  * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
3442  */
3443 int8_t navigationGetHeadingControlState(void)
3444 {
3445     // For airplanes report as manual heading control
3446     if (STATE(FIXED_WING_LEGACY)) {
3447         return NAV_HEADING_CONTROL_MANUAL;
3448     }
3449
3450     // For multirotors it depends on navigation system mode
3451     if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
3452         if (posControl.flags.isAdjustingHeading) {
3453             return NAV_HEADING_CONTROL_MANUAL;
3454         }
3455         else {
3456             return NAV_HEADING_CONTROL_AUTO;
3457         }
3458     }
3459     else {
3460         return NAV_HEADING_CONTROL_NONE;
3461     }
3462 }
3463
3464 bool navigationTerrainFollowingEnabled(void)
3465 {
3466     return posControl.flags.isTerrainFollowEnabled;
3467 }
3468
3469 uint32_t distanceToFirstWP(void)
3470 {
3471     fpVector3_t startingWaypointPos;
3472 #ifdef USE_MULTI_MISSION
3473     mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE);
3474 #else
3475     mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
3476 #endif
3477     return calculateDistanceToDestination(&startingWaypointPos);
3478 }
3479
3480 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
3481 {
3482     const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
3483     const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
3484
3485     if (usedBypass) {
3486         *usedBypass = false;
3487     }
3488
3489     if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_OFF) {
3490         return NAV_ARMING_BLOCKER_NONE;
3491     }
3492
3493     // Apply extra arming safety only if pilot has any of GPS modes configured
3494     if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
3495         if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
3496             (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
3497             if (usedBypass) {
3498                 *usedBypass = true;
3499             }
3500             return NAV_ARMING_BLOCKER_NONE;
3501         }
3502         return NAV_ARMING_BLOCKER_MISSING_GPS_FIX;
3503     }
3504
3505     // Don't allow arming if any of NAV modes is active
3506     if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
3507         return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
3508     }
3509
3510     // Don't allow arming if first waypoint is farther than configured safe distance
3511     if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
3512         if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
3513             return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
3514         }
3515     }
3516
3517         /*
3518          * Don't allow arming if any of JUMP waypoint has invalid settings
3519          * First WP can't be JUMP
3520          * Can't jump to immediately adjacent WPs (pointless)
3521          * Can't jump beyond WP list
3522          * Only jump to geo-referenced WP types
3523          */
3524 #ifdef USE_MULTI_MISSION
3525          // Only perform check when mission loaded at start of posControl.waypointList
3526     if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
3527 #else
3528     if (posControl.waypointCount) {
3529 #endif
3530         for (uint8_t wp = 0; wp < posControl.waypointCount; wp++){
3531             if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
3532                 if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >=  posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
3533                     return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
3534                 }
3535                     /* check for target geo-ref sanity */
3536                 uint16_t target = posControl.waypointList[wp].p1;
3537                 if(!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) {
3538                     return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
3539                 }
3540             }
3541         }
3542     }
3543
3544     return NAV_ARMING_BLOCKER_NONE;
3545 }
3546
3547 bool navigationPositionEstimateIsHealthy(void)
3548 {
3549     return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
3550 }
3551
3552 /**
3553  * Indicate ready/not ready status
3554  */
3555 static void updateReadyStatus(void)
3556 {
3557     static bool posReadyBeepDone = false;
3558
3559     /* Beep out READY_BEEP once when position lock is firstly acquired and HOME set */
3560     if (navigationPositionEstimateIsHealthy() && !posReadyBeepDone) {
3561         beeper(BEEPER_READY_BEEP);
3562         posReadyBeepDone = true;
3563     }
3564 }
3565
3566 void updateFlightBehaviorModifiers(void)
3567 {
3568     if (posControl.flags.isGCSAssistedNavigationEnabled && !IS_RC_MODE_ACTIVE(BOXGCSNAV)) {
3569         posControl.flags.isGCSAssistedNavigationReset = true;
3570     }
3571
3572     posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
3573 }
3574
3575 /* On the fly WP mission planner mode allows WP missions to be setup during navigation.
3576  * Uses the WP mode switch to save WP at current location (WP mode disabled when active)
3577  * Mission can be flown after mission planner mode switched off and saved after disarm. */
3578
3579 void updateWpMissionPlanner(void)
3580 {
3581     static timeMs_t resetTimerStart = 0;
3582     if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
3583         const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
3584
3585         posControl.flags.wpMissionPlannerActive = true;
3586         if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
3587             posControl.waypointCount = posControl.wpPlannerActiveWPIndex = 0;
3588             posControl.waypointListValid = false;
3589             posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
3590         }
3591         if (positionTrusted && posControl.wpMissionPlannerStatus != WP_PLAN_FULL) {
3592             missionPlannerSetWaypoint();
3593         } else {
3594             posControl.wpMissionPlannerStatus = posControl.wpMissionPlannerStatus == WP_PLAN_FULL ? WP_PLAN_FULL : WP_PLAN_WAIT;
3595         }
3596     } else if (posControl.flags.wpMissionPlannerActive) {
3597         posControl.flags.wpMissionPlannerActive = false;
3598         posControl.activeWaypointIndex = 0;
3599         resetTimerStart = millis();
3600     }
3601 }
3602
3603 void missionPlannerSetWaypoint(void)
3604 {
3605     static bool boxWPModeIsReset = true;
3606
3607     boxWPModeIsReset = !boxWPModeIsReset ? !IS_RC_MODE_ACTIVE(BOXNAVWP) : boxWPModeIsReset; // only able to save new WP when WP mode reset
3608     posControl.wpMissionPlannerStatus = boxWPModeIsReset ? boxWPModeIsReset : posControl.wpMissionPlannerStatus;  // hold save status until WP mode reset
3609
3610     if (!boxWPModeIsReset || !IS_RC_MODE_ACTIVE(BOXNAVWP)) {
3611         return;
3612     }
3613
3614     if (!posControl.wpPlannerActiveWPIndex) {   // reset existing mission data before adding first WP
3615         resetWaypointList();
3616     }
3617
3618     gpsLocation_t wpLLH;
3619     geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
3620
3621     posControl.waypointList[posControl.wpPlannerActiveWPIndex].action = 1;
3622     posControl.waypointList[posControl.wpPlannerActiveWPIndex].lat = wpLLH.lat;
3623     posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
3624     posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
3625     posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
3626     posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1;                      // use absolute altitude datum
3627     posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
3628     posControl.waypointListValid = true;
3629
3630     if (posControl.wpPlannerActiveWPIndex) {
3631         posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
3632     }
3633
3634     posControl.wpPlannerActiveWPIndex += 1;
3635     posControl.waypointCount = posControl.geoWaypointCount = posControl.wpPlannerActiveWPIndex;
3636     posControl.wpMissionPlannerStatus = posControl.waypointCount == NAV_MAX_WAYPOINTS ? WP_PLAN_FULL : WP_PLAN_OK;
3637     boxWPModeIsReset = false;
3638 }
3639
3640 /**
3641  * Process NAV mode transition and WP/RTH state machine
3642  *  Update rate: RX (data driven or 50Hz)
3643  */
3644 void updateWaypointsAndNavigationMode(void)
3645 {
3646     /* Initiate home position update */
3647     updateHomePosition();
3648
3649     /* Update flight statistics */
3650     updateNavigationFlightStatistics();
3651
3652     /* Update NAV ready status */
3653     updateReadyStatus();
3654
3655     // Update flight behaviour modifiers
3656     updateFlightBehaviorModifiers();
3657
3658     // Process switch to a different navigation mode (if needed)
3659     navProcessFSMEvents(selectNavEventFromBoxModeInput());
3660
3661     // Process pilot's RC input to adjust behaviour
3662     processNavigationRCAdjustments();
3663
3664     // Map navMode back to enabled flight modes
3665     switchNavigationFlightModes();
3666
3667     // Update WP mission planner
3668     updateWpMissionPlanner();
3669
3670     //Update Blackbox data
3671     navCurrentState = (int16_t)posControl.navPersistentId;
3672 }
3673
3674 /*-----------------------------------------------------------
3675  * NAV main control functions
3676  *-----------------------------------------------------------*/
3677 void navigationUsePIDs(void)
3678 {
3679     /** Multicopter PIDs */
3680     // Brake time parameter
3681     posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f;
3682
3683     // Position controller expo (taret vel expo for MC)
3684     posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f);
3685
3686     // Initialize position hold P-controller
3687     for (int axis = 0; axis < 2; axis++) {
3688         navPidInit(
3689             &posControl.pids.pos[axis],
3690             (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
3691             0.0f,
3692             0.0f,
3693             0.0f,
3694             NAV_DTERM_CUT_HZ
3695         );
3696
3697         navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
3698                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
3699                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
3700                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
3701                                                pidProfile()->navVelXyDTermLpfHz
3702         );
3703     }
3704
3705     /*
3706      * Set coefficients used in MC VEL_XY
3707      */
3708     multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f;
3709     multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f;
3710     multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f;
3711
3712 #ifdef USE_MR_BRAKING_MODE
3713     multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;
3714 #endif
3715
3716     // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
3717     navPidInit(
3718         &posControl.pids.pos[Z],
3719         (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
3720         0.0f,
3721         0.0f,
3722         0.0f,
3723         NAV_DTERM_CUT_HZ
3724     );
3725
3726     navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
3727                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
3728                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
3729                                         0.0f,
3730                                         NAV_DTERM_CUT_HZ
3731     );
3732
3733     // Initialize surface tracking PID
3734     navPidInit(&posControl.pids.surface, 2.0f,
3735                                          0.0f,
3736                                          0.0f,
3737                                          0.0f,
3738                                          NAV_DTERM_CUT_HZ
3739     );
3740
3741     /** Airplane PIDs */
3742     // Initialize fixed wing PID controllers
3743     navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
3744                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
3745                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
3746                                         0.0f,
3747                                         NAV_DTERM_CUT_HZ
3748     );
3749
3750     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
3751                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
3752                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
3753                                         0.0f,
3754                                         NAV_DTERM_CUT_HZ
3755     );
3756
3757     navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
3758                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
3759                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
3760                                         0.0f,
3761                                         2.0f
3762     );
3763 }
3764
3765 void navigationInit(void)
3766 {
3767     /* Initial state */
3768     posControl.navState = NAV_STATE_IDLE;
3769
3770     posControl.flags.horizontalPositionDataNew = false;
3771     posControl.flags.verticalPositionDataNew = false;
3772
3773     posControl.flags.estAltStatus = EST_NONE;
3774     posControl.flags.estPosStatus = EST_NONE;
3775     posControl.flags.estVelStatus = EST_NONE;
3776     posControl.flags.estHeadingStatus = EST_NONE;
3777     posControl.flags.estAglStatus = EST_NONE;
3778
3779     posControl.flags.forcedRTHActivated = false;
3780     posControl.flags.forcedEmergLandingActivated = false;
3781     posControl.waypointCount = 0;
3782     posControl.activeWaypointIndex = 0;
3783     posControl.waypointListValid = false;
3784     posControl.wpPlannerActiveWPIndex = 0;
3785     posControl.flags.wpMissionPlannerActive = false;
3786 #ifdef USE_MULTI_MISSION
3787     posControl.multiMissionCount = 0;
3788     posControl.loadedMultiMissionStartWP = 0;
3789 #endif
3790     /* Set initial surface invalid */
3791     posControl.actualState.surfaceMin = -1.0f;
3792
3793     /* Reset statistics */
3794     posControl.totalTripDistance = 0.0f;
3795
3796     /* Use system config */
3797     navigationUsePIDs();
3798
3799     if (
3800         mixerConfig()->platformType == PLATFORM_BOAT ||
3801         mixerConfig()->platformType == PLATFORM_ROVER ||
3802         navConfig()->fw.useFwNavYawControl
3803     ) {
3804         ENABLE_STATE(FW_HEADING_USE_YAW);
3805     } else {
3806         DISABLE_STATE(FW_HEADING_USE_YAW);
3807     }
3808 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
3809     /* configure WP missions at boot */
3810 #ifdef USE_MULTI_MISSION
3811     for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {    // check number missions in NVM
3812         if (checkMissionCount(i)) {
3813             break;
3814         }
3815     }
3816     /* set index to 1 if saved mission index > available missions */
3817     if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
3818         navConfigMutable()->general.waypoint_multi_mission_index = 1;
3819     }
3820 #endif
3821     /* load mission on boot */
3822     if (navConfig()->general.waypoint_load_on_boot) {
3823         loadNonVolatileWaypointList(false);
3824     }
3825 #endif
3826 }
3827
3828 /*-----------------------------------------------------------
3829  * Access to estimated position/velocity data
3830  *-----------------------------------------------------------*/
3831 float getEstimatedActualVelocity(int axis)
3832 {
3833     return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
3834 }
3835
3836 float getEstimatedActualPosition(int axis)
3837 {
3838     return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
3839 }
3840
3841 /*-----------------------------------------------------------
3842  * Ability to execute RTH on external event
3843  *-----------------------------------------------------------*/
3844 void activateForcedRTH(void)
3845 {
3846     abortFixedWingLaunch();
3847     posControl.flags.forcedRTHActivated = true;
3848     checkSafeHomeState(true);
3849     navProcessFSMEvents(selectNavEventFromBoxModeInput());
3850 }
3851
3852 void abortForcedRTH(void)
3853 {
3854     // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
3855     // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
3856     posControl.flags.forcedRTHActivated = false;
3857     checkSafeHomeState(false);
3858     navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
3859 }
3860
3861 rthState_e getStateOfForcedRTH(void)
3862 {
3863     /* If forced RTH activated and in AUTO_RTH or EMERG state */
3864     if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
3865         if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
3866             return RTH_HAS_LANDED;
3867         }
3868         else {
3869             return RTH_IN_PROGRESS;
3870         }
3871     }
3872     else {
3873         return RTH_IDLE;
3874     }
3875 }
3876
3877 /*-----------------------------------------------------------
3878  * Ability to execute Emergency Landing on external event
3879  *-----------------------------------------------------------*/
3880 void activateForcedEmergLanding(void)
3881 {
3882     abortFixedWingLaunch();
3883     posControl.flags.forcedEmergLandingActivated = true;
3884     navProcessFSMEvents(selectNavEventFromBoxModeInput());
3885 }
3886
3887 void abortForcedEmergLanding(void)
3888 {
3889     // Disable emergency landing and make sure we back out of navigation mode to IDLE
3890     // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update
3891     posControl.flags.forcedEmergLandingActivated = false;
3892     navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
3893 }
3894
3895 emergLandState_e getStateOfForcedEmergLanding(void)
3896 {
3897     /* If forced emergency landing activated and in EMERG state */
3898     if (posControl.flags.forcedEmergLandingActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_EMERG)) {
3899         if (posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
3900             return EMERG_LAND_HAS_LANDED;
3901         } else {
3902             return EMERG_LAND_IN_PROGRESS;
3903         }
3904     } else {
3905         return EMERG_LAND_IDLE;
3906     }
3907 }
3908
3909 bool isWaypointMissionRTHActive(void)
3910 {
3911     return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
3912 }
3913
3914 bool navigationIsExecutingAnEmergencyLanding(void)
3915 {
3916     return navGetCurrentStateFlags() & NAV_CTL_EMERG;
3917 }
3918
3919 bool navigationInAutomaticThrottleMode(void)
3920 {
3921     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3922     return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
3923 }
3924
3925 bool navigationIsControllingThrottle(void)
3926 {
3927     // Note that this makes a detour into mixer code to evaluate actual motor status
3928     return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE);
3929 }
3930
3931 bool navigationIsControllingAltitude(void) {
3932     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3933     return (stateFlags & NAV_CTL_ALT);
3934 }
3935
3936 bool navigationIsFlyingAutonomousMode(void)
3937 {
3938     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3939     return (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
3940 }
3941
3942 bool navigationRTHAllowsLanding(void)
3943 {
3944     // WP mission RTH landing setting
3945     if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
3946         return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
3947     }
3948
3949     // normal RTH landing setting
3950     navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
3951     return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
3952         (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
3953 }
3954
3955 bool isNavLaunchEnabled(void)
3956 {
3957     return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
3958 }
3959
3960 int32_t navigationGetHomeHeading(void)
3961 {
3962     return posControl.rthState.homePosition.yaw;
3963 }
3964
3965 // returns m/s
3966 float calculateAverageSpeed() {
3967     float flightTime = getFlightTime();
3968     if (flightTime == 0.0f) return 0;
3969     return (float)getTotalTravelDistance() / (flightTime * 100);
3970 }
3971
3972 const navigationPIDControllers_t* getNavigationPIDControllers(void) {
3973     return &posControl.pids;
3974 }
3975
3976 bool isAdjustingPosition(void) {
3977     return posControl.flags.isAdjustingPosition;
3978 }
3979
3980 bool isAdjustingHeading(void) {
3981     return posControl.flags.isAdjustingHeading;
3982 }
3983
3984 int32_t getCruiseHeadingAdjustment(void) {
3985     return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
3986 }