?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Update navigation.c
[inav.git] / src / main / navigation / navigation.c
blobadd001fa8a73b2e737f68a5b25d672342702e6b3
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/multifunction.h"
40 #include "fc/rc_controls.h"
41 #include "fc/rc_modes.h"
42 #include "fc/runtime_config.h"
43 #ifdef USE_MULTI_MISSION
44 #include "fc/rc_adjustments.h"
45 #include "fc/cli.h"
46 #endif
47 #include "fc/settings.h"
48
49 #include "flight/imu.h"
50 #include "flight/mixer_profile.h"
51 #include "flight/pid.h"
52 #include "flight/wind_estimator.h"
53
54 #include "io/beeper.h"
55 #include "io/gps.h"
56
57 #include "navigation/navigation.h"
58 #include "navigation/navigation_private.h"
59
60 #include "rx/rx.h"
61
62 #include "sensors/sensors.h"
63 #include "sensors/acceleration.h"
64 #include "sensors/boardalignment.h"
65 #include "sensors/battery.h"
66 #include "sensors/gyro.h"
67
68 #include "programming/global_variables.h"
69 #include "sensors/rangefinder.h"
70
71 // Multirotors:
72 #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)
73 #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
74 #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set
75 #define MR_RTH_LAND_MARGIN_CM 2000 // pause landing if this amount of cm *before* remaining to the home point (2D distance)
76
77 // Planes:
78 #define FW_RTH_CLIMB_OVERSHOOT_CM 100
79 #define FW_RTH_CLIMB_MARGIN_MIN_CM 100
80 #define FW_RTH_CLIMB_MARGIN_PERCENT 15
81 #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec)
82 #define FW_LAND_LOITER_ALT_TOLERANCE 150
83
84 /*-----------------------------------------------------------
85 * Compatibility for home position
86 *-----------------------------------------------------------*/
87 gpsLocation_t GPS_home;
88 uint32_t GPS_distanceToHome; // distance to home point in meters
89 int16_t GPS_directionToHome; // direction to home point in degrees
90
91 radar_pois_t radar_pois[RADAR_MAX_POIS];
92
93 #ifdef USE_FW_AUTOLAND
94 PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
95
96 PG_REGISTER_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig, PG_FW_AUTOLAND_APPROACH_CONFIG, 0);
97
98 PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
99 .approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
100 .finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT,
101 .flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT,
102 .glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT,
103 .flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
104 .maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
105 .glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
106 );
107 #endif
108
109 #if defined(USE_SAFE_HOME)
110 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
111 #endif
112
113 // waypoint 254, 255 are special waypoints
114 STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
115
116 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
117 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
118 #endif
119
120 PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
121
122 PG_RESET_TEMPLATE(navConfig_t, navConfig,
123 .general = {
124
125 .flags = {
126 .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
127 .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
128 .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
129 .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT, // Climb first, turn after reaching safe altitude
130 .rth_climb_first_stage_mode = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_MODE_DEFAULT, // To determine how rth_climb_first_stage_altitude is used
131 .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT, // Ignore GPS loss on initial climb
132 .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT,
133 .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT,
134 .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
135 .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
136 .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
137 .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
138 .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
139 .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
140 .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
141 .rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
142 .rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
143 .landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump
144 },
145
146 // General navigation parameters
147 .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
148 .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
149 .waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this
150 #ifdef USE_MULTI_MISSION
151 .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
152 #endif
153 .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
154 .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
155 .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s)
156 .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
157 .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
158 .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
159 .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
160 .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
161 .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
162 .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
163 .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s
164 .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s
165 .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately
166 .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters
167 .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT, // altitude in centimeters
168 .rth_climb_first_stage_altitude = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_ALTITUDE_DEFAULT, // altitude in centimetres, 0= off
169 .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT, // centimeters - 500m should be safe for all aircraft
170 .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT, // max altitude in centimeters in terrain following mode
171 .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point
172 .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
173 .rth_trackback_distance = SETTING_NAV_RTH_TRACKBACK_DISTANCE_DEFAULT, // Max distance allowed for RTH trackback
174 .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
175 .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
176 .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
177 .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
178 .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
179 .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
180 },
181
182 // MC-specific
183 .mc = {
184 .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
185
186 #ifdef USE_MR_BRAKING_MODE
187 .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
188 .braking_disengage_speed = SETTING_NAV_MC_BRAKING_DISENGAGE_SPEED_DEFAULT, // Stop when speed goes below 0.75m/s
189 .braking_timeout = SETTING_NAV_MC_BRAKING_TIMEOUT_DEFAULT, // Timeout barking after 2s
190 .braking_boost_factor = SETTING_NAV_MC_BRAKING_BOOST_FACTOR_DEFAULT, // A 100% boost by default
191 .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms
192 .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s
193 .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s
194 .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle
195 #endif
196
197 .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
198 .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
199 .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
200 .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT, // STICK
201 },
202
203 // Fixed wing
204 .fw = {
205 .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
206 .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
207 .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
208 .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
209 .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
210 .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
211 .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
212 .minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
213 .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
214 .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
215
216 //Fixed wing landing
217 .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
218
219 // Fixed wing launch
220 .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
221 .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
222 .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
223 .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
224 .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
225 .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
226 .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
227 .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
228 .launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
229 .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
230 .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
231 .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
232 .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
233 .launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
234
235 .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
236 .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
237 .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
238 .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
239 .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
240 .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
241 .wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
242 }
243 );
244
245 /* NAV variables */
246 static navWapointHeading_t wpHeadingControl;
247 navigationPosControl_t posControl;
248 navSystemStatus_t NAV_Status;
249 static bool landingDetectorIsActive;
250
251 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
252
253 // Blackbox states
254 int16_t navCurrentState;
255 int16_t navActualVelocity[3];
256 int16_t navDesiredVelocity[3];
257 int32_t navTargetPosition[3];
258 int32_t navLatestActualPosition[3];
259 int16_t navActualHeading;
260 uint16_t navDesiredHeading;
261 int16_t navActualSurface;
262 uint16_t navFlags;
263 uint16_t navEPH;
264 uint16_t navEPV;
265 int16_t navAccNEU[3];
266 //End of blackbox states
267
268 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
269 static void updateDesiredRTHAltitude(void);
270 static void resetAltitudeController(bool useTerrainFollowing);
271 static void resetPositionController(void);
272 static void setupAltitudeController(void);
273 static void resetHeadingController(void);
274
275 #ifdef USE_FW_AUTOLAND
276 static void resetFwAutoland(void);
277 #endif
278
279 void resetGCSFlags(void);
280
281 static void setupJumpCounters(void);
282 static void resetJumpCounter(void);
283 static void clearJumpCounters(void);
284
285 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
286 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
287 void calculateInitialHoldPosition(fpVector3_t * pos);
288 void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance);
289 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
290 static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
291 bool isWaypointAltitudeReached(void);
292 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
293 static navigationFSMEvent_t nextForNonGeoStates(void);
294 static bool isWaypointMissionValid(void);
295 void missionPlannerSetWaypoint(void);
296
297 void initializeRTHSanityChecker(void);
298 bool validateRTHSanityChecker(void);
299 void updateHomePosition(void);
300 bool abortLaunchAllowed(void);
301
302 static bool rthAltControlStickOverrideCheck(unsigned axis);
303
304 static void updateRthTrackback(bool forceSaveTrackPoint);
305 static fpVector3_t * rthGetTrackbackPos(void);
306
307 #ifdef USE_FW_AUTOLAND
308 static float getLandAltitude(void);
309 static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
310 static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
311 static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
312 #endif
313
314 /*************************************************************************************************/
315 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
316 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
317 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
318 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
319 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
320 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState);
321 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState);
322 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState);
323 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState);
324 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState);
325 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState);
326 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
327 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
328 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState);
329 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
330 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
331 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
332 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
333 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
334 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
335 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
336 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
337 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
338 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
339 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
340 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
341 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
342 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
343 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
344 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
345 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
346 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
347 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
348 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
349 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState);
350 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState);
351 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState);
352 #ifdef USE_FW_AUTOLAND
353 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState);
354 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState);
355 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
356 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
357 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
358 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
359 #endif
360
361 static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
362 /** Idle state ******************************************************/
363 [NAV_STATE_IDLE] = {
364 .persistentId = NAV_PERSISTENT_ID_IDLE,
365 .onEntry = navOnEnteringState_NAV_STATE_IDLE,
366 .timeoutMs = 0,
367 .stateFlags = 0,
368 .mapToFlightModes = 0,
369 .mwState = MW_NAV_STATE_NONE,
370 .mwError = MW_NAV_ERROR_NONE,
371 .onEvent = {
372 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
373 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
374 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
375 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
376 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
377 [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
378 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
379 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
380 [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
381 }
382 },
383
384 /** ALTHOLD mode ***************************************************/
385 [NAV_STATE_ALTHOLD_INITIALIZE] = {
386 .persistentId = NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE,
387 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
388 .timeoutMs = 0,
389 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
390 .mapToFlightModes = NAV_ALTHOLD_MODE,
391 .mwState = MW_NAV_STATE_NONE,
392 .mwError = MW_NAV_ERROR_NONE,
393 .onEvent = {
394 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS,
395 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
396 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
397 }
398 },
399
400 [NAV_STATE_ALTHOLD_IN_PROGRESS] = {
401 .persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS,
402 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
403 .timeoutMs = 10,
404 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
405 .mapToFlightModes = NAV_ALTHOLD_MODE,
406 .mwState = MW_NAV_STATE_NONE,
407 .mwError = MW_NAV_ERROR_NONE,
408 .onEvent = {
409 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
410 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
411 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
412 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
413 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
414 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
415 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
416 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
417 }
418 },
419
420 /** POSHOLD_3D mode ************************************************/
421 [NAV_STATE_POSHOLD_3D_INITIALIZE] = {
422 .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
423 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
424 .timeoutMs = 0,
425 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
426 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
427 .mwState = MW_NAV_STATE_HOLD_INFINIT,
428 .mwError = MW_NAV_ERROR_NONE,
429 .onEvent = {
430 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS,
431 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
432 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
433 }
434 },
435
436 [NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
437 .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
438 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
439 .timeoutMs = 10,
440 .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,
441 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
442 .mwState = MW_NAV_STATE_HOLD_INFINIT,
443 .mwError = MW_NAV_ERROR_NONE,
444 .onEvent = {
445 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
446 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
447 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
448 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
449 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
450 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
451 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
452 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
453 }
454 },
455 /** CRUISE_HOLD mode ************************************************/
456 [NAV_STATE_COURSE_HOLD_INITIALIZE] = {
457 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE,
458 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE,
459 .timeoutMs = 0,
460 .stateFlags = NAV_REQUIRE_ANGLE,
461 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
462 .mwState = MW_NAV_STATE_NONE,
463 .mwError = MW_NAV_ERROR_NONE,
464 .onEvent = {
465 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
466 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
467 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
468 }
469 },
470
471 [NAV_STATE_COURSE_HOLD_IN_PROGRESS] = {
472 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
473 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
474 .timeoutMs = 10,
475 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW,
476 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
477 .mwState = MW_NAV_STATE_NONE,
478 .mwError = MW_NAV_ERROR_NONE,
479 .onEvent = {
480 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state
481 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
482 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
483 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
484 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
485 [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING,
486 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
487 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
488 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
489 }
490 },
491
492 [NAV_STATE_COURSE_HOLD_ADJUSTING] = {
493 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING,
494 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING,
495 .timeoutMs = 10,
496 .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
497 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
498 .mwState = MW_NAV_STATE_NONE,
499 .mwError = MW_NAV_ERROR_NONE,
500 .onEvent = {
501 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
502 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING,
503 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
504 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
505 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
506 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
507 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
508 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
509 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
510 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
511 }
512 },
513
514 /** CRUISE_3D mode ************************************************/
515 [NAV_STATE_CRUISE_INITIALIZE] = {
516 .persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE,
517 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE,
518 .timeoutMs = 0,
519 .stateFlags = NAV_REQUIRE_ANGLE,
520 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
521 .mwState = MW_NAV_STATE_NONE,
522 .mwError = MW_NAV_ERROR_NONE,
523 .onEvent = {
524 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
525 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
526 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
527 }
528 },
529
530 [NAV_STATE_CRUISE_IN_PROGRESS] = {
531 .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
532 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
533 .timeoutMs = 10,
534 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
535 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
536 .mwState = MW_NAV_STATE_NONE,
537 .mwError = MW_NAV_ERROR_NONE,
538 .onEvent = {
539 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state
540 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
541 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
542 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
543 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
544 [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING,
545 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
546 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
547 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
548 }
549 },
550
551 [NAV_STATE_CRUISE_ADJUSTING] = {
552 .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING,
553 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING,
554 .timeoutMs = 10,
555 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
556 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
557 .mwState = MW_NAV_STATE_NONE,
558 .mwError = MW_NAV_ERROR_NONE,
559 .onEvent = {
560 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
561 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING,
562 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
563 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
564 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
565 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
566 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
567 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
568 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
569 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
570 }
571 },
572
573 /** RTH_3D mode ************************************************/
574 [NAV_STATE_RTH_INITIALIZE] = {
575 .persistentId = NAV_PERSISTENT_ID_RTH_INITIALIZE,
576 .onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
577 .timeoutMs = 10,
578 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
579 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
580 .mwState = MW_NAV_STATE_RTH_START,
581 .mwError = MW_NAV_ERROR_NONE,
582 .onEvent = {
583 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state
584 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
585 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK,
586 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
587 [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
588 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
589 }
590 },
591
592 [NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = {
593 .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
594 .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
595 .timeoutMs = 10,
596 .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
597 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
598 .mwState = MW_NAV_STATE_RTH_CLIMB,
599 .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
600 .onEvent = {
601 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state
602 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME,
603 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
604 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
605 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
606 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
607 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
608 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
609 }
610 },
611
612 [NAV_STATE_RTH_TRACKBACK] = {
613 .persistentId = NAV_PERSISTENT_ID_RTH_TRACKBACK,
614 .onEntry = navOnEnteringState_NAV_STATE_RTH_TRACKBACK,
615 .timeoutMs = 10,
616 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
617 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
618 .mwState = MW_NAV_STATE_RTH_ENROUTE,
619 .mwError = MW_NAV_ERROR_NONE,
620 .onEvent = {
621 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_TRACKBACK, // re-process the state
622 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE,
623 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
624 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
625 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
626 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
627 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
628 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
629 }
630 },
631
632 [NAV_STATE_RTH_HEAD_HOME] = {
633 .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
634 .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
635 .timeoutMs = 10,
636 .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,
637 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
638 .mwState = MW_NAV_STATE_RTH_ENROUTE,
639 .mwError = MW_NAV_ERROR_NONE,
640 .onEvent = {
641 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state
642 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
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 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
648 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
649 [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
650 }
651 },
652
653 [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
654 .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
655 .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
656 .timeoutMs = 500,
657 .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,
658 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
659 .mwState = MW_NAV_STATE_LAND_SETTLE,
660 .mwError = MW_NAV_ERROR_NONE,
661 .onEvent = {
662 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
663 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
664 [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
665 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
666 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
667 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
668 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
669 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
670 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
671 }
672 },
673
674 [NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
675 .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
676 .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
677 .timeoutMs = 10,
678 .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,
679 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
680 .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
681 .mwError = MW_NAV_ERROR_NONE,
682 .onEvent = {
683 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
684 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
685 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
686 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
687 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
688 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
689 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
690 }
691 },
692
693 [NAV_STATE_RTH_LANDING] = {
694 .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
695 .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
696 .timeoutMs = 10,
697 .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,
698 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
699 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
700 .mwError = MW_NAV_ERROR_LANDING,
701 .onEvent = {
702 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state
703 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING,
704 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
705 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
706 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
707 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
708 [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
709 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
710 [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
711 }
712 },
713
714 [NAV_STATE_RTH_FINISHING] = {
715 .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
716 .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
717 .timeoutMs = 0,
718 .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,
719 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
720 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
721 .mwError = MW_NAV_ERROR_LANDING,
722 .onEvent = {
723 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED,
724 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
725 }
726 },
727
728 [NAV_STATE_RTH_FINISHED] = {
729 .persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
730 .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
731 .timeoutMs = 10,
732 .stateFlags = NAV_CTL_ALT | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
733 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
734 .mwState = MW_NAV_STATE_LANDED,
735 .mwError = MW_NAV_ERROR_NONE,
736 .onEvent = {
737 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
738 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
739 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
740 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
741 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
742 }
743 },
744
745 /** WAYPOINT mode ************************************************/
746 [NAV_STATE_WAYPOINT_INITIALIZE] = {
747 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE,
748 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
749 .timeoutMs = 0,
750 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
751 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
752 .mwState = MW_NAV_STATE_PROCESS_NEXT,
753 .mwError = MW_NAV_ERROR_NONE,
754 .onEvent = {
755 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
756 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
757 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
758 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
759 }
760 },
761
762 [NAV_STATE_WAYPOINT_PRE_ACTION] = {
763 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
764 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
765 .timeoutMs = 10,
766 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
767 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
768 .mwState = MW_NAV_STATE_PROCESS_NEXT,
769 .mwError = MW_NAV_ERROR_NONE,
770 .onEvent = {
771 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
772 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
773 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
774 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
775 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
776 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
777 }
778 },
779
780 [NAV_STATE_WAYPOINT_IN_PROGRESS] = {
781 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS,
782 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
783 .timeoutMs = 10,
784 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
785 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
786 .mwState = MW_NAV_STATE_WP_ENROUTE,
787 .mwError = MW_NAV_ERROR_NONE,
788 .onEvent = {
789 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state
790 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
791 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
792 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
793 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
794 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
795 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
796 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
797 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
798 }
799 },
800
801 [NAV_STATE_WAYPOINT_REACHED] = {
802 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED,
803 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
804 .timeoutMs = 10,
805 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
806 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
807 .mwState = MW_NAV_STATE_PROCESS_NEXT,
808 .mwError = MW_NAV_ERROR_NONE,
809 .onEvent = {
810 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
811 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
812 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
813 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
814 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
815 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
816 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
817 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
818 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
819 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
820 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
821 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
822 }
823 },
824
825 [NAV_STATE_WAYPOINT_HOLD_TIME] = {
826 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
827 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
828 .timeoutMs = 10,
829 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
830 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
831 .mwState = MW_NAV_STATE_HOLD_TIMED,
832 .mwError = MW_NAV_ERROR_NONE,
833 .onEvent = {
834 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state
835 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint
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_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
839 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
840 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
841 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
842 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
843 }
844 },
845
846 [NAV_STATE_WAYPOINT_RTH_LAND] = {
847 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
848 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
849 .timeoutMs = 10,
850 .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,
851 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
852 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
853 .mwError = MW_NAV_ERROR_LANDING,
854 .onEvent = {
855 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state
856 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
857 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
858 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
859 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
860 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
861 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
862 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
863 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
864 [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
865 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
866 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
867 }
868 },
869
870 [NAV_STATE_WAYPOINT_NEXT] = {
871 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT,
872 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT,
873 .timeoutMs = 0,
874 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
875 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
876 .mwState = MW_NAV_STATE_PROCESS_NEXT,
877 .mwError = MW_NAV_ERROR_NONE,
878 .onEvent = {
879 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
880 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
881 }
882 },
883
884 [NAV_STATE_WAYPOINT_FINISHED] = {
885 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
886 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
887 .timeoutMs = 10,
888 .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,
889 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
890 .mwState = MW_NAV_STATE_WP_ENROUTE,
891 .mwError = MW_NAV_ERROR_FINISH,
892 .onEvent = {
893 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, // re-process state
894 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
895 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
896 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
897 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
898 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
899 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
900 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
901 }
902 },
903
904 /** EMERGENCY LANDING ************************************************/
905 [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
906 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
907 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
908 .timeoutMs = 0,
909 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
910 .mapToFlightModes = 0,
911 .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
912 .mwError = MW_NAV_ERROR_LANDING,
913 .onEvent = {
914 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
915 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
916 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
917 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
918 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
919 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
920 }
921 },
922
923 [NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
924 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
925 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
926 .timeoutMs = 10,
927 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
928 .mapToFlightModes = 0,
929 .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
930 .mwError = MW_NAV_ERROR_LANDING,
931 .onEvent = {
932 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state
933 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
934 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
935 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
936 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
937 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
938 }
939 },
940
941 [NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
942 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
943 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
944 .timeoutMs = 10,
945 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
946 .mapToFlightModes = 0,
947 .mwState = MW_NAV_STATE_LANDED,
948 .mwError = MW_NAV_ERROR_LANDING,
949 .onEvent = {
950 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
951 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
952 }
953 },
954
955 [NAV_STATE_LAUNCH_INITIALIZE] = {
956 .persistentId = NAV_PERSISTENT_ID_LAUNCH_INITIALIZE,
957 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
958 .timeoutMs = 0,
959 .stateFlags = NAV_REQUIRE_ANGLE,
960 .mapToFlightModes = NAV_LAUNCH_MODE,
961 .mwState = MW_NAV_STATE_NONE,
962 .mwError = MW_NAV_ERROR_NONE,
963 .onEvent = {
964 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT,
965 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
966 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
967 }
968 },
969
970 [NAV_STATE_LAUNCH_WAIT] = {
971 .persistentId = NAV_PERSISTENT_ID_LAUNCH_WAIT,
972 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
973 .timeoutMs = 10,
974 .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
975 .mapToFlightModes = NAV_LAUNCH_MODE,
976 .mwState = MW_NAV_STATE_NONE,
977 .mwError = MW_NAV_ERROR_NONE,
978 .onEvent = {
979 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state
980 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS,
981 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
982 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
983 }
984 },
985
986 [NAV_STATE_LAUNCH_IN_PROGRESS] = {
987 .persistentId = NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS,
988 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
989 .timeoutMs = 10,
990 .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
991 .mapToFlightModes = NAV_LAUNCH_MODE,
992 .mwState = MW_NAV_STATE_NONE,
993 .mwError = MW_NAV_ERROR_NONE,
994 .onEvent = {
995 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state
996 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
997 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
998 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
999 }
1000 },
1001
1002 /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/
1003 [NAV_STATE_MIXERAT_INITIALIZE] = {
1004 .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE,
1005 .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE,
1006 .timeoutMs = 0,
1007 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
1008 .mapToFlightModes = NAV_ALTHOLD_MODE,
1009 .mwState = MW_NAV_STATE_NONE,
1010 .mwError = MW_NAV_ERROR_NONE,
1011 .onEvent = {
1012 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_MIXERAT_IN_PROGRESS,
1013 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
1014 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1015 }
1016 },
1017
1018 [NAV_STATE_MIXERAT_IN_PROGRESS] = {
1019 .persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS,
1020 .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS,
1021 .timeoutMs = 10,
1022 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
1023 .mapToFlightModes = NAV_ALTHOLD_MODE,
1024 .mwState = MW_NAV_STATE_NONE,
1025 .mwError = MW_NAV_ERROR_NONE,
1026 .onEvent = {
1027 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state
1028 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT,
1029 [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state
1030 [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state
1031 }
1032 },
1033 [NAV_STATE_MIXERAT_ABORT] = {
1034 .persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT,
1035 .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, //will not switch to its pending state
1036 .timeoutMs = 10,
1037 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
1038 .mapToFlightModes = NAV_ALTHOLD_MODE,
1039 .mwState = MW_NAV_STATE_NONE,
1040 .mwError = MW_NAV_ERROR_NONE,
1041 .onEvent = {
1042 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
1043 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1044
1045 }
1046 },
1047
1048 #ifdef USE_FW_AUTOLAND
1049 [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = {
1050 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
1051 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
1052 .timeoutMs = 10,
1053 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
1054 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
1055 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1056 .mwError = MW_NAV_ERROR_NONE,
1057 .onEvent = {
1058 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
1059 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_LOITER,
1060 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1061 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
1062 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
1063 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
1064 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
1065 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
1066 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
1067 }
1068 },
1069
1070 [NAV_STATE_FW_LANDING_LOITER] = {
1071 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
1072 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
1073 .timeoutMs = 10,
1074 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
1075 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
1076 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1077 .mwError = MW_NAV_ERROR_NONE,
1078 .onEvent = {
1079 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_LOITER,
1080 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_APPROACH,
1081 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1082 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
1083 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
1084 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
1085 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
1086 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
1087 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
1088 }
1089 },
1090
1091 [NAV_STATE_FW_LANDING_APPROACH] = {
1092 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH,
1093 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH,
1094 .timeoutMs = 10,
1095 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
1096 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
1097 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1098 .mwError = MW_NAV_ERROR_NONE,
1099 .onEvent = {
1100 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_APPROACH,
1101 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_GLIDE,
1102 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1103 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
1104 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
1105 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
1106 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
1107 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
1108 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
1109 }
1110 },
1111
1112 [NAV_STATE_FW_LANDING_GLIDE] = {
1113 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_GLIDE,
1114 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE,
1115 .timeoutMs = 10,
1116 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
1117 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
1118 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1119 .mwError = MW_NAV_ERROR_NONE,
1120 .onEvent = {
1121 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_GLIDE,
1122 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_FLARE,
1123 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1124 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
1125 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
1126 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
1127 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
1128 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
1129 [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
1130 }
1131 },
1132
1133 [NAV_STATE_FW_LANDING_FLARE] = {
1134 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_FLARE,
1135 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE,
1136 .timeoutMs = 10,
1137 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
1138 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
1139 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1140 .mwError = MW_NAV_ERROR_NONE,
1141 .onEvent = {
1142 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state
1143 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
1144 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1145 }
1146 },
1147 [NAV_STATE_FW_LANDING_ABORT] = {
1148 .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT,
1149 .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT,
1150 .timeoutMs = 10,
1151 .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,
1152 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
1153 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
1154 .mwError = MW_NAV_ERROR_NONE,
1155 .onEvent = {
1156 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_ABORT,
1157 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
1158 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
1159 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1160 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
1161 }
1162 },
1163 #endif
1164 };
1165
1166 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
1167 {
1168 return navFSM[state].stateFlags;
1169 }
1170
1171 flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
1172 {
1173 return navFSM[state].mapToFlightModes;
1174 }
1175
1176 navigationFSMStateFlags_t navGetCurrentStateFlags(void)
1177 {
1178 return navGetStateFlags(posControl.navState);
1179 }
1180
1181 static bool navTerrainFollowingRequested(void)
1182 {
1183 // Terrain following not supported on FIXED WING aircraft yet
1184 return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
1185 }
1186
1187 /*************************************************************************************************/
1188 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
1189 {
1190 UNUSED(previousState);
1191
1192 resetAltitudeController(false);
1193 resetHeadingController();
1194 resetPositionController();
1195 #ifdef USE_FW_AUTOLAND
1196 resetFwAutoland();
1197 #endif
1198
1199 return NAV_FSM_EVENT_NONE;
1200 }
1201
1202 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
1203 {
1204 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1205 const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
1206
1207 resetGCSFlags();
1208
1209 // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
1210 if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
1211 resetAltitudeController(navTerrainFollowingRequested());
1212 setupAltitudeController();
1213 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
1214 }
1215
1216 return NAV_FSM_EVENT_SUCCESS;
1217 }
1218
1219 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState)
1220 {
1221 UNUSED(previousState);
1222
1223 // If GCS was disabled - reset altitude setpoint
1224 if (posControl.flags.isGCSAssistedNavigationReset) {
1225 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
1226 resetGCSFlags();
1227 }
1228
1229 return NAV_FSM_EVENT_NONE;
1230 }
1231
1232 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
1233 {
1234 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1235 const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
1236
1237 resetGCSFlags();
1238
1239 // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
1240 if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
1241 resetAltitudeController(navTerrainFollowingRequested());
1242 setupAltitudeController();
1243 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
1244 }
1245
1246 // Prepare position controller if idle or current Mode NOT active in position hold state
1247 if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
1248 previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
1249 previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
1250 {
1251 resetPositionController();
1252
1253 fpVector3_t targetHoldPos;
1254 calculateInitialHoldPosition(&targetHoldPos);
1255 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1256 }
1257
1258 return NAV_FSM_EVENT_SUCCESS;
1259 }
1260
1261 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState)
1262 {
1263 UNUSED(previousState);
1264
1265 // If GCS was disabled - reset 2D pos setpoint
1266 if (posControl.flags.isGCSAssistedNavigationReset) {
1267 fpVector3_t targetHoldPos;
1268 calculateInitialHoldPosition(&targetHoldPos);
1269 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
1270 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1271 resetGCSFlags();
1272 }
1273
1274 return NAV_FSM_EVENT_NONE;
1275 }
1276
1277 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
1278 {
1279 UNUSED(previousState);
1280
1281 if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
1282 return NAV_FSM_EVENT_ERROR;
1283 }
1284
1285 DEBUG_SET(DEBUG_CRUISE, 0, 1);
1286 // Switch to IDLE if we do not have an healty position. Try the next iteration.
1287 if (checkForPositionSensorTimeout()) {
1288 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1289 }
1290
1291 resetPositionController();
1292
1293 if (STATE(AIRPLANE)) {
1294 posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
1295 } else { // Multicopter
1296 posControl.cruise.course = posControl.actualState.yaw;
1297 posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
1298 posControl.desiredState.pos = posControl.actualState.abs.pos;
1299 }
1300 posControl.cruise.previousCourse = posControl.cruise.course;
1301 posControl.cruise.lastCourseAdjustmentTime = 0;
1302
1303 return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
1304 }
1305
1306 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
1307 {
1308 UNUSED(previousState);
1309
1310 const timeMs_t currentTimeMs = millis();
1311
1312 // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
1313 if (checkForPositionSensorTimeout()) {
1314 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1315 }
1316
1317 DEBUG_SET(DEBUG_CRUISE, 0, 2);
1318 DEBUG_SET(DEBUG_CRUISE, 2, 0);
1319
1320 if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch
1321 return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
1322 }
1323
1324 const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
1325
1326 // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
1327 // We record the desired course and change the desired target in the meanwhile
1328 if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) {
1329 int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
1330 int16_t headingAdjustCommand = rcCommand[YAW];
1331 if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) {
1332 headingAdjustCommand = -rcCommand[ROLL];
1333 }
1334
1335 timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
1336 if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
1337 float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
1338 float centidegsPerIteration = rateTarget * MS2S(timeDifference);
1339 posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
1340 DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
1341 posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
1342 } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
1343 posControl.cruise.previousCourse = posControl.cruise.course;
1344 }
1345
1346 setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
1347
1348 return NAV_FSM_EVENT_NONE;
1349 }
1350
1351 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState)
1352 {
1353 UNUSED(previousState);
1354 DEBUG_SET(DEBUG_CRUISE, 0, 3);
1355
1356 // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
1357 if (posControl.flags.isAdjustingPosition) {
1358 posControl.cruise.course = posControl.actualState.cog; //store current course
1359 posControl.cruise.lastCourseAdjustmentTime = millis();
1360 return NAV_FSM_EVENT_NONE; // reprocess the state
1361 }
1362
1363 resetPositionController();
1364
1365 return NAV_FSM_EVENT_SUCCESS; // go back to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
1366 }
1367
1368 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
1369 {
1370 if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
1371 return NAV_FSM_EVENT_ERROR;
1372 }
1373
1374 navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
1375
1376 return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);
1377 }
1378
1379 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState)
1380 {
1381 navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1382
1383 return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState);
1384 }
1385
1386 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState)
1387 {
1388 navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1389
1390 return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState);
1391 }
1392
1393 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
1394 {
1395
1396 if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
1397 posControl.rthState.rthLinearDescentActive = false;
1398
1399 if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
1400 // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
1401 // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
1402 // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
1403 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1404 }
1405
1406 if (previousState != NAV_STATE_FW_LANDING_ABORT) {
1407 #ifdef USE_FW_AUTOLAND
1408 posControl.fwLandState.landAborted = false;
1409 #endif
1410 if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
1411 // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
1412 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1413 }
1414 }
1415
1416 // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
1417 if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
1418 // Prepare controllers
1419 resetPositionController();
1420 resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
1421 setupAltitudeController();
1422
1423 // If close to home - reset home position and land
1424 if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
1425 setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
1426 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1427
1428 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1429 }
1430 else {
1431 // Switch to RTH trackback
1432 bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
1433 (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
1434
1435 if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
1436 updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
1437 posControl.flags.rthTrackbackActive = true;
1438 calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
1439 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
1440 }
1441
1442 fpVector3_t targetHoldPos;
1443
1444 if (STATE(FIXED_WING_LEGACY)) {
1445 // Airplane - climbout before heading home
1446 if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
1447 // Spiral climb centered at xy of RTH activation
1448 calculateInitialHoldPosition(&targetHoldPos);
1449 } else {
1450 calculateFarAwayTarget(&targetHoldPos, posControl.actualState.cog, 100000.0f); // 1km away Linear climb
1451 }
1452 } else {
1453 // Multicopter, hover and climb
1454 calculateInitialHoldPosition(&targetHoldPos);
1455
1456 // Initialize RTH sanity check to prevent fly-aways on RTH
1457 // For airplanes this is delayed until climb-out is finished
1458 initializeRTHSanityChecker();
1459 }
1460
1461 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1462
1463 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
1464 }
1465 }
1466 /* Position sensor failure timeout - land. Land immediately if failsafe RTH and timeout disabled (set to 0) */
1467 else if (checkForPositionSensorTimeout() || (!navConfig()->general.pos_failure_timeout && posControl.flags.forcedRTHActivated)) {
1468 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1469 }
1470 /* No valid POS sensor but still within valid timeout - wait */
1471 return NAV_FSM_EVENT_NONE;
1472 }
1473
1474 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
1475 {
1476 UNUSED(previousState);
1477
1478 if (!STATE(ALTITUDE_CONTROL)) {
1479 //If altitude control is not a thing, switch to RTH in progress instead
1480 return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
1481 }
1482
1483 rthAltControlStickOverrideCheck(PITCH);
1484
1485 /* Position sensor failure timeout and not configured to ignore GPS loss - land */
1486 if ((posControl.flags.estHeadingStatus == EST_NONE) ||
1487 (checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) {
1488 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1489 }
1490
1491 const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
1492 const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0f) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
1493
1494 // If we reached desired initial RTH altitude or we don't want to climb first
1495 if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
1496
1497 // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
1498 if (STATE(FIXED_WING_LEGACY)) {
1499 initializeRTHSanityChecker();
1500 }
1501
1502 // Save initial home distance for future use
1503 posControl.rthState.rthInitialDistance = posControl.homeDistance;
1504 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1505
1506 if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
1507 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1508 }
1509 else {
1510 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1511 }
1512
1513 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
1514
1515 } else {
1516
1517 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1518
1519 /* For multi-rotors execute sanity check during initial ascent as well */
1520 if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
1521 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1522 }
1523
1524 // Climb to safe altitude and turn to correct direction
1525 // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
1526 // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
1527 if (STATE(FIXED_WING_LEGACY)) {
1528 tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
1529 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1530 } else {
1531 tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
1532
1533 if (navConfig()->general.flags.rth_tail_first) {
1534 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1535 } else {
1536 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1537 }
1538 }
1539
1540 return NAV_FSM_EVENT_NONE;
1541 }
1542 }
1543
1544 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState)
1545 {
1546 UNUSED(previousState);
1547
1548 /* If position sensors unavailable - land immediately */
1549 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
1550 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1551 }
1552
1553 if (posControl.flags.estPosStatus >= EST_USABLE) {
1554 const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
1555 #ifdef USE_MULTI_FUNCTIONS
1556 const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
1557 #else
1558 const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
1559 #endif
1560 const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
1561 (overrideTrackback && !posControl.flags.forcedRTHActivated);
1562
1563 if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
1564 posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
1565 posControl.flags.rthTrackbackActive = false;
1566 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point
1567 }
1568
1569 if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
1570 posControl.activeRthTBPointIndex--;
1571
1572 if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
1573 posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
1574 }
1575 calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
1576
1577 if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) {
1578 posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
1579 }
1580 } else {
1581 setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1582 }
1583 }
1584
1585 return NAV_FSM_EVENT_NONE;
1586 }
1587
1588 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
1589 {
1590 UNUSED(previousState);
1591
1592 rthAltControlStickOverrideCheck(PITCH);
1593
1594 /* If position sensors unavailable - land immediately */
1595 if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) {
1596 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1597 }
1598
1599 if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){
1600 return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
1601 }
1602
1603 if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) {
1604 // Check linear descent status
1605 uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
1606
1607 if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
1608 posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
1609 posControl.rthState.rthLinearDescentActive = true;
1610 }
1611 }
1612
1613 // If we have position sensor - continue home
1614 if ((posControl.flags.estPosStatus >= EST_USABLE)) {
1615 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
1616
1617 if (isWaypointReached(tmpHomePos, 0)) {
1618 // Successfully reached position target - update XYZ-position
1619 setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1620
1621 posControl.landingDelay = 0;
1622
1623 if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
1624 posControl.rthState.rthLinearDescentActive = false;
1625
1626 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1627 } else {
1628 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
1629 return NAV_FSM_EVENT_NONE;
1630 }
1631 }
1632 /* Position sensor failure timeout - land */
1633 else if (checkForPositionSensorTimeout()) {
1634 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1635 }
1636 /* No valid POS sensor but still within valid timeout - wait */
1637 return NAV_FSM_EVENT_NONE;
1638 }
1639
1640 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
1641 {
1642 UNUSED(previousState);
1643
1644 //On ROVER and BOAT we immediately switch to the next event
1645 if (!STATE(ALTITUDE_CONTROL)) {
1646 return NAV_FSM_EVENT_SUCCESS;
1647 }
1648
1649 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1650 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1651 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1652 }
1653
1654 // Action delay before landing if in FS and option enabled
1655 bool pauseLanding = false;
1656 navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
1657 if ((allow == NAV_RTH_ALLOW_LANDING_ALWAYS || allow == NAV_RTH_ALLOW_LANDING_FS_ONLY) && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) {
1658 if (posControl.landingDelay == 0)
1659 posControl.landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay);
1660
1661 batteryState_e batteryState = getBatteryState();
1662
1663 if (millis() < posControl.landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL)
1664 pauseLanding = true;
1665 else
1666 posControl.landingDelay = 0;
1667 }
1668
1669 // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue
1670 // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
1671 if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) {
1672 resetLandingDetector(); // force reset landing detector just in case
1673 updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
1674 return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
1675 } else {
1676 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
1677 setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1678 return NAV_FSM_EVENT_NONE;
1679 }
1680 }
1681
1682 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
1683 {
1684 UNUSED(previousState);
1685
1686 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1687 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1688 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1689 }
1690
1691 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
1692 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1693
1694 return NAV_FSM_EVENT_NONE;
1695 }
1696
1697 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
1698 {
1699 #ifndef USE_FW_AUTOLAND
1700 UNUSED(previousState);
1701 #endif
1702
1703 //On ROVER and BOAT we immediately switch to the next event
1704 if (!STATE(ALTITUDE_CONTROL)) {
1705 return NAV_FSM_EVENT_SUCCESS;
1706 }
1707
1708 if (!ARMING_FLAG(ARMED) || STATE(LANDING_DETECTED)) {
1709 return NAV_FSM_EVENT_SUCCESS;
1710 }
1711
1712 /* If position sensors unavailable - land immediately (wait for timeout on GPS)
1713 * Continue to check for RTH sanity during landing */
1714 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1715 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1716 }
1717
1718 if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){
1719 return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
1720 }
1721
1722 #ifdef USE_FW_AUTOLAND
1723 if (STATE(AIRPLANE)) {
1724 int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
1725 #ifdef USE_MULTI_MISSION
1726 missionIdx = posControl.loadedMultiMissionIndex - 1;
1727 #endif
1728
1729 #ifdef USE_SAFE_HOME
1730 shIdx = posControl.safehomeState.index;
1731 missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
1732 #endif
1733 if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) {
1734
1735 if (previousState == NAV_STATE_WAYPOINT_REACHED) {
1736 posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
1737 posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
1738 posControl.fwLandState.landWp = true;
1739 } else {
1740 posControl.fwLandState.landPos = posControl.safehomeState.nearestSafeHome;
1741 posControl.fwLandState.approachSettingIdx = shIdx;
1742 posControl.fwLandState.landWp = false;
1743 }
1744
1745 posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt;
1746 posControl.fwLandState.landAproachAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt;
1747 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
1748 }
1749 }
1750 #endif
1751
1752 float descentVelLimited = 0;
1753 int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
1754 fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint;
1755 uint32_t remainingDistance = calculateDistanceToDestination(&tmpHomePos);
1756
1757 // A safeguard - if surface altitude sensor is available and is reading < 50cm altitude - drop to min descend speed.
1758 // Also slow down to min descent speed during RTH MR landing if MR drifted too far away from home position.
1759 bool minDescentSpeedRequired = (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f) ||
1760 (FLIGHT_MODE(NAV_RTH_MODE) && STATE(MULTIROTOR) && remainingDistance > MR_RTH_LAND_MARGIN_CM);
1761
1762 // Do not allow descent velocity slower than -30cm/s so the landing detector works (limited by land_minalt_vspd).
1763 if (minDescentSpeedRequired) {
1764 descentVelLimited = navConfig()->general.land_minalt_vspd;
1765 } else {
1766 // Ramp down descent velocity from max speed at maxAlt altitude to min speed from minAlt to 0cm.
1767 float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
1768 navConfig()->general.land_slowdown_minalt + landingElevation,
1769 navConfig()->general.land_slowdown_maxalt + landingElevation,
1770 navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1771
1772 descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1773 }
1774
1775 updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
1776
1777 return NAV_FSM_EVENT_NONE;
1778 }
1779
1780 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState)
1781 {
1782 UNUSED(previousState);
1783
1784 //On ROVER and BOAT disarm immediately
1785 if (!STATE(ALTITUDE_CONTROL)) {
1786 disarm(DISARM_NAVIGATION);
1787 }
1788
1789 return NAV_FSM_EVENT_SUCCESS;
1790 }
1791
1792 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState)
1793 {
1794 // Stay in this state
1795 UNUSED(previousState);
1796
1797 if (STATE(ALTITUDE_CONTROL)) {
1798 updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
1799 }
1800
1801 // Prevent I-terms growing when already landed
1802 pidResetErrorAccumulators();
1803 return NAV_FSM_EVENT_NONE;
1804 }
1805
1806 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState)
1807 {
1808 UNUSED(previousState);
1809
1810 if (!posControl.waypointCount || !posControl.waypointListValid) {
1811 return NAV_FSM_EVENT_ERROR;
1812 }
1813
1814 // Prepare controllers
1815 resetPositionController();
1816 resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL
1817
1818 if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
1819 /* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
1820 Using p3 minimises the risk of saving an invalid counter if a mission is aborted */
1821 setupJumpCounters();
1822 posControl.activeWaypointIndex = posControl.startWpIndex;
1823 wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1824 }
1825
1826 if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
1827 posControl.wpMissionRestart = posControl.activeWaypointIndex > posControl.startWpIndex ? !posControl.wpMissionRestart : false;
1828 } else {
1829 posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
1830 }
1831
1832 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1833 }
1834
1835 static navigationFSMEvent_t nextForNonGeoStates(void)
1836 {
1837 /* simple helper for non-geographical states that just set other data */
1838 if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish.
1839 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1840 } else { // Finished non-geo, move to next WP
1841 posControl.activeWaypointIndex++;
1842 return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1843 }
1844 }
1845
1846 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
1847 {
1848 /* A helper function to do waypoint-specific action */
1849 UNUSED(previousState);
1850
1851 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1852 case NAV_WP_ACTION_HOLD_TIME:
1853 case NAV_WP_ACTION_WAYPOINT:
1854 case NAV_WP_ACTION_LAND:
1855 calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
1856 posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
1857 posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
1858 posControl.wpAltitudeReached = false;
1859 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1860
1861 case NAV_WP_ACTION_JUMP:
1862 // We use p3 as the volatile jump counter (p2 is the static value)
1863 if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
1864 if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
1865 resetJumpCounter();
1866 return nextForNonGeoStates();
1867 }
1868 else
1869 {
1870 posControl.waypointList[posControl.activeWaypointIndex].p3--;
1871 }
1872 }
1873 posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + posControl.startWpIndex;
1874 return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1875
1876 case NAV_WP_ACTION_SET_POI:
1877 if (STATE(MULTIROTOR)) {
1878 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
1879 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
1880 &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
1881 }
1882 return nextForNonGeoStates();
1883
1884 case NAV_WP_ACTION_SET_HEAD:
1885 if (STATE(MULTIROTOR)) {
1886 if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
1887 posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
1888 wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1889 } else {
1890 wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
1891 wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
1892 }
1893 }
1894 return nextForNonGeoStates();
1895
1896 case NAV_WP_ACTION_RTH:
1897 posControl.wpMissionRestart = true;
1898 return NAV_FSM_EVENT_SWITCH_TO_RTH;
1899 };
1900
1901 UNREACHABLE();
1902 }
1903
1904 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
1905 {
1906 UNUSED(previousState);
1907
1908 // If no position sensor available - land immediately
1909 if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
1910 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1911 case NAV_WP_ACTION_HOLD_TIME:
1912 case NAV_WP_ACTION_WAYPOINT:
1913 case NAV_WP_ACTION_LAND:
1914 if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
1915 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
1916 }
1917 else {
1918 fpVector3_t tmpWaypoint;
1919 tmpWaypoint.x = posControl.activeWaypoint.pos.x;
1920 tmpWaypoint.y = posControl.activeWaypoint.pos.y;
1921 tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
1922 posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
1923 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
1924 setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1925 if(STATE(MULTIROTOR)) {
1926 switch (wpHeadingControl.mode) {
1927 case NAV_WP_HEAD_MODE_NONE:
1928 break;
1929 case NAV_WP_HEAD_MODE_FIXED:
1930 setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
1931 break;
1932 case NAV_WP_HEAD_MODE_POI:
1933 setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
1934 break;
1935 }
1936 }
1937 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1938 }
1939 break;
1940
1941 case NAV_WP_ACTION_JUMP:
1942 case NAV_WP_ACTION_SET_HEAD:
1943 case NAV_WP_ACTION_SET_POI:
1944 case NAV_WP_ACTION_RTH:
1945 UNREACHABLE();
1946 }
1947 }
1948 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1949 else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
1950 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1951 }
1952
1953 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1954 }
1955
1956 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
1957 {
1958 UNUSED(previousState);
1959
1960 if (navConfig()->general.waypoint_enforce_altitude) {
1961 posControl.wpAltitudeReached = isWaypointAltitudeReached();
1962 }
1963
1964 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1965 case NAV_WP_ACTION_WAYPOINT:
1966 if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
1967 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1968 } else {
1969 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
1970 }
1971
1972 case NAV_WP_ACTION_JUMP:
1973 case NAV_WP_ACTION_SET_HEAD:
1974 case NAV_WP_ACTION_SET_POI:
1975 case NAV_WP_ACTION_RTH:
1976 UNREACHABLE();
1977
1978 case NAV_WP_ACTION_LAND:
1979 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
1980
1981 case NAV_WP_ACTION_HOLD_TIME:
1982 // Save the current time for the time the waypoint was reached
1983 posControl.wpReachedTime = millis();
1984 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1985 }
1986
1987 UNREACHABLE();
1988 }
1989
1990 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
1991 {
1992 UNUSED(previousState);
1993
1994 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1995 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
1996 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1997 }
1998
1999 if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
2000 // Adjust altitude to waypoint setting
2001 setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
2002
2003 posControl.wpAltitudeReached = isWaypointAltitudeReached();
2004
2005 if (posControl.wpAltitudeReached) {
2006 posControl.wpReachedTime = millis();
2007 } else {
2008 return NAV_FSM_EVENT_NONE;
2009 }
2010 }
2011
2012 timeMs_t currentTime = millis();
2013
2014 if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
2015 posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
2016 (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
2017 return NAV_FSM_EVENT_SUCCESS;
2018 }
2019
2020 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
2021 }
2022
2023 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
2024 {
2025 UNUSED(previousState);
2026
2027 const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
2028
2029 if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
2030 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
2031 } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
2032 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
2033 } else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
2034 // Landing controller returned success - invoke RTH finishing state and finish the waypoint
2035 navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
2036 return NAV_FSM_EVENT_SUCCESS;
2037 }
2038 else {
2039 return NAV_FSM_EVENT_NONE;
2040 }
2041 }
2042
2043 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
2044 {
2045 UNUSED(previousState);
2046
2047 if (isLastMissionWaypoint()) { // Last waypoint reached
2048 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
2049 }
2050 else {
2051 // Waypoint reached, do something and move on to next waypoint
2052 posControl.activeWaypointIndex++;
2053 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
2054 }
2055 }
2056
2057 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState)
2058 {
2059 UNUSED(previousState);
2060
2061 clearJumpCounters();
2062 posControl.wpMissionRestart = true;
2063
2064 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
2065 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
2066 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2067 }
2068
2069 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
2070 }
2071
2072 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
2073 {
2074 UNUSED(previousState);
2075
2076 #ifdef USE_FW_AUTOLAND
2077 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2078 #endif
2079
2080 if ((posControl.flags.estPosStatus >= EST_USABLE)) {
2081 resetPositionController();
2082 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
2083 }
2084
2085 // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
2086 // Make sure terrain following is not enabled
2087 resetAltitudeController(false);
2088
2089 return NAV_FSM_EVENT_SUCCESS;
2090 }
2091
2092 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
2093 {
2094 UNUSED(previousState);
2095
2096 // Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
2097 if (posControl.flags.estPosStatus >= EST_USABLE) {
2098 float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
2099 if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
2100 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
2101 }
2102 }
2103
2104 if (STATE(LANDING_DETECTED)) {
2105 return NAV_FSM_EVENT_SUCCESS;
2106 }
2107
2108 return NAV_FSM_EVENT_NONE;
2109 }
2110
2111 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
2112 {
2113 UNUSED(previousState);
2114
2115 return NAV_FSM_EVENT_NONE;
2116 }
2117
2118 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
2119 {
2120 const timeUs_t currentTimeUs = micros();
2121 UNUSED(previousState);
2122
2123 resetFixedWingLaunchController(currentTimeUs);
2124
2125 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT
2126 }
2127
2128 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState)
2129 {
2130 const timeUs_t currentTimeUs = micros();
2131 UNUSED(previousState);
2132
2133 // Continue immediately to launch in progress if manual launch throttle used
2134 if (navConfig()->fw.launch_manual_throttle) {
2135 return NAV_FSM_EVENT_SUCCESS;
2136 }
2137
2138 if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
2139 enableFixedWingLaunchController(currentTimeUs);
2140 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
2141 }
2142
2143 // abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle
2144 if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2145 abortFixedWingLaunch();
2146 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2147 }
2148
2149 return NAV_FSM_EVENT_NONE;
2150 }
2151
2152 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState)
2153 {
2154 UNUSED(previousState);
2155
2156 if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) {
2157 return NAV_FSM_EVENT_SUCCESS;
2158 }
2159
2160 return NAV_FSM_EVENT_NONE;
2161 }
2162
2163 navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE;
2164 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState)
2165 {
2166 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
2167
2168 // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
2169 if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP)) {
2170 resetAltitudeController(false);
2171 setupAltitudeController();
2172 }
2173 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
2174 navMixerATPendingState = previousState;
2175 return NAV_FSM_EVENT_SUCCESS;
2176 }
2177
2178 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState)
2179 {
2180 UNUSED(previousState);
2181 mixerProfileATRequest_e required_action;
2182 switch (navMixerATPendingState)
2183 {
2184 case NAV_STATE_RTH_HEAD_HOME:
2185 required_action = MIXERAT_REQUEST_RTH;
2186 break;
2187 case NAV_STATE_RTH_LANDING:
2188 required_action = MIXERAT_REQUEST_LAND;
2189 break;
2190 default:
2191 required_action = MIXERAT_REQUEST_NONE;
2192 break;
2193 }
2194 if (mixerATUpdateState(required_action)){
2195 // MixerAT is done, switch to next state
2196 resetPositionController();
2197 resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL
2198 mixerATUpdateState(MIXERAT_REQUEST_ABORT);
2199 switch (navMixerATPendingState)
2200 {
2201 case NAV_STATE_RTH_HEAD_HOME:
2202 setupAltitudeController();
2203 return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME;
2204 break;
2205 case NAV_STATE_RTH_LANDING:
2206 setupAltitudeController();
2207 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;
2208 break;
2209 default:
2210 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2211 break;
2212 }
2213 }
2214
2215 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
2216
2217 return NAV_FSM_EVENT_NONE;
2218 }
2219
2220 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState)
2221 {
2222 UNUSED(previousState);
2223 mixerATUpdateState(MIXERAT_REQUEST_ABORT);
2224 return NAV_FSM_EVENT_SUCCESS;
2225 }
2226
2227 #ifdef USE_FW_AUTOLAND
2228 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState)
2229 {
2230 UNUSED(previousState);
2231
2232 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2233 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2234 }
2235
2236 if (posControl.fwLandState.loiterStartTime == 0) {
2237 posControl.fwLandState.loiterStartTime = micros();
2238 }
2239
2240 if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
2241 updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
2242 posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
2243 return NAV_FSM_EVENT_SUCCESS;
2244 }
2245
2246 fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos;
2247 tmpHomePos.z = posControl.fwLandState.landAproachAltAgl;
2248 setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z);
2249
2250 return NAV_FSM_EVENT_NONE;
2251 }
2252
2253 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState)
2254 {
2255 UNUSED(previousState);
2256 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
2257 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
2258 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2259 }
2260
2261 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2262 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2263 }
2264
2265 if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) {
2266 if (isEstimatedWindSpeedValid()) {
2267
2268 uint16_t windAngle = 0;
2269 int32_t approachHeading = -1;
2270 float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle);
2271 windAngle = wrap_36000(windAngle + 18000);
2272
2273 // Ignore low wind speed, could be the error of the wind estimator
2274 if (windSpeed < navFwAutolandConfig()->maxTailwind) {
2275 if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1 != 0) {
2276 approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1));
2277 } else if ((fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2 != 0) ) {
2278 approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2));
2279 }
2280 } else {
2281 int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1), windAngle);
2282 int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2), windAngle);
2283
2284 if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
2285 heading2 = -1;
2286 }
2287
2288 if (heading1 == -1 && heading2 >= 0) {
2289 posControl.fwLandState.landingDirection = heading2;
2290 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
2291 } else if (heading1 >= 0 && heading2 == -1) {
2292 posControl.fwLandState.landingDirection = heading1;
2293 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
2294 } else {
2295 if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
2296 posControl.fwLandState.landingDirection = heading1;
2297 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
2298 } else {
2299 posControl.fwLandState.landingDirection = heading2;
2300 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
2301 }
2302 }
2303 }
2304
2305 if (posControl.fwLandState.landingDirection >= 0) {
2306 fpVector3_t tmpPos;
2307
2308 int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;
2309 int32_t dir = 0;
2310 if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
2311 dir = wrap_36000(ABS(approachHeading) - 9000);
2312 } else {
2313 dir = wrap_36000(ABS(approachHeading) + 9000);
2314 }
2315
2316 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, posControl.fwLandState.landingDirection, navFwAutolandConfig()->approachLength);
2317 tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
2318 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND] = tmpPos;
2319
2320 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, wrap_36000(posControl.fwLandState.landingDirection + 18000), navFwAutolandConfig()->approachLength);
2321 tmpPos.z = finalApproachAlt;
2322 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
2323
2324 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, MAX((uint32_t)navConfig()->fw.loiter_radius * 4, navFwAutolandConfig()->approachLength / 2));
2325 tmpPos.z = posControl.fwLandState.landAproachAltAgl;
2326 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
2327
2328 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH]);
2329 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_TURN;
2330 posControl.fwLandState.landState = FW_AUTOLAND_STATE_DOWNWIND;
2331
2332 return NAV_FSM_EVENT_SUCCESS;
2333 } else {
2334 posControl.fwLandState.loiterStartTime = micros();
2335 }
2336 } else {
2337 posControl.fwLandState.loiterStartTime = micros();
2338 }
2339 }
2340
2341 fpVector3_t tmpPoint = posControl.fwLandState.landPos;
2342 tmpPoint.z = posControl.fwLandState.landAproachAltAgl;
2343 setDesiredPosition(&tmpPoint, posControl.fwLandState.landPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
2344
2345 return NAV_FSM_EVENT_NONE;
2346 }
2347 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState)
2348 {
2349 UNUSED(previousState);
2350
2351 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
2352 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2353 }
2354
2355 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2356 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2357 }
2358
2359 if (isLandingDetected()) {
2360 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2361 disarm(DISARM_LANDING);
2362 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2363 }
2364
2365 if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
2366 resetPositionController();
2367 posControl.cruise.course = posControl.fwLandState.landingDirection;
2368 posControl.cruise.previousCourse = posControl.cruise.course;
2369 posControl.cruise.lastCourseAdjustmentTime = 0;
2370 posControl.fwLandState.landState = FW_AUTOLAND_STATE_GLIDE;
2371 return NAV_FSM_EVENT_SUCCESS;
2372 } else if (isWaypointReached(&posControl.fwLandState.landWaypoints[posControl.fwLandState.landCurrentWp], &posControl.activeWaypoint.bearing)) {
2373 if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_TURN) {
2374 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND]);
2375 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH;
2376 posControl.fwLandState.landState = FW_AUTOLAND_STATE_BASE_LEG;
2377 return NAV_FSM_EVENT_NONE;
2378 } else if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) {
2379 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND], NULL);
2380 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_LAND;
2381 posControl.fwLandState.landState = FW_AUTOLAND_STATE_FINAL_APPROACH;
2382 return NAV_FSM_EVENT_NONE;
2383 }
2384 }
2385
2386 fpVector3_t tmpWaypoint;
2387 tmpWaypoint.x = posControl.activeWaypoint.pos.x;
2388 tmpWaypoint.y = posControl.activeWaypoint.pos.y;
2389 tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
2390 posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
2391 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
2392 setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
2393
2394 return NAV_FSM_EVENT_NONE;
2395 }
2396
2397 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState)
2398 {
2399 UNUSED(previousState);
2400
2401 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2402 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2403 }
2404
2405 if (getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
2406 posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
2407 return NAV_FSM_EVENT_SUCCESS;
2408 }
2409
2410 if (isLandingDetected()) {
2411 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2412 disarm(DISARM_LANDING);
2413 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2414 }
2415
2416 setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
2417 return NAV_FSM_EVENT_NONE;
2418 }
2419
2420 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState)
2421 {
2422 UNUSED(previousState);
2423
2424 if (isLandingDetected()) {
2425 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2426 disarm(DISARM_LANDING);
2427 return NAV_FSM_EVENT_SUCCESS;
2428 }
2429 setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
2430
2431 return NAV_FSM_EVENT_NONE;
2432 }
2433
2434 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
2435 {
2436 UNUSED(previousState);
2437 posControl.fwLandState.landAborted = true;
2438 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2439
2440 return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
2441 }
2442 #endif
2443
2444 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
2445 {
2446 navigationFSMState_t previousState;
2447
2448 previousState = posControl.navState;
2449 if (posControl.navState != newState) {
2450 posControl.navState = newState;
2451 posControl.navPersistentId = navFSM[newState].persistentId;
2452 }
2453 return previousState;
2454 }
2455
2456 static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
2457 {
2458 const timeMs_t currentMillis = millis();
2459 navigationFSMState_t previousState = NAV_STATE_UNDEFINED;
2460 static timeMs_t lastStateProcessTime = 0;
2461
2462 /* Process new injected event if event defined,
2463 * otherwise process timeout event if defined */
2464 if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
2465 /* Update state */
2466 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
2467 } else if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
2468 ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
2469 /* Update state */
2470 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
2471 }
2472
2473 if (previousState) { /* If state updated call new state's entry function */
2474 while (navFSM[posControl.navState].onEntry) {
2475 navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
2476
2477 if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
2478 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
2479 }
2480 else {
2481 break;
2482 }
2483 }
2484
2485 lastStateProcessTime = currentMillis;
2486 }
2487
2488 /* Update public system state information */
2489 NAV_Status.mode = MW_GPS_MODE_NONE;
2490
2491 if (ARMING_FLAG(ARMED)) {
2492 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
2493
2494 if (navStateFlags & NAV_AUTO_RTH) {
2495 NAV_Status.mode = MW_GPS_MODE_RTH;
2496 }
2497 else if (navStateFlags & NAV_AUTO_WP) {
2498 NAV_Status.mode = MW_GPS_MODE_NAV;
2499 }
2500 else if (navStateFlags & NAV_CTL_EMERG) {
2501 NAV_Status.mode = MW_GPS_MODE_EMERG;
2502 }
2503 else if (navStateFlags & NAV_CTL_POS) {
2504 NAV_Status.mode = MW_GPS_MODE_HOLD;
2505 }
2506 }
2507
2508 NAV_Status.state = navFSM[posControl.navState].mwState;
2509 NAV_Status.error = navFSM[posControl.navState].mwError;
2510
2511 NAV_Status.flags = 0;
2512 if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
2513 if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
2514
2515 NAV_Status.activeWpIndex = posControl.activeWaypointIndex - posControl.startWpIndex;
2516 NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1;
2517
2518 NAV_Status.activeWpAction = 0;
2519 if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
2520 NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
2521 }
2522 }
2523
2524 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
2525 {
2526 posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos;
2527
2528 switch (mode) {
2529 case RTH_HOME_ENROUTE_INITIAL:
2530 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
2531 break;
2532
2533 case RTH_HOME_ENROUTE_PROPORTIONAL:
2534 {
2535 float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
2536 if (rthTotalDistanceToTravel >= 100) {
2537 float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
2538 posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
2539 }
2540 else {
2541 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2542 }
2543 }
2544 break;
2545
2546 case RTH_HOME_ENROUTE_FINAL:
2547 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2548 break;
2549
2550 case RTH_HOME_FINAL_HOVER:
2551 if (navConfig()->general.rth_home_altitude) {
2552 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
2553 }
2554 else {
2555 // If home altitude not defined - fall back to final ENROUTE altitude
2556 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2557 }
2558 break;
2559
2560 case RTH_HOME_FINAL_LAND:
2561 // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
2562 if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
2563 posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm
2564 if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
2565 posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used
2566 }
2567 }
2568 break;
2569 }
2570
2571 return &posControl.rthState.homeTmpWaypoint;
2572 }
2573
2574 /*-----------------------------------------------------------
2575 * Detects if thrust vector is facing downwards
2576 *-----------------------------------------------------------*/
2577 bool isThrustFacingDownwards(void)
2578 {
2579 // Tilt angle <= 80 deg; cos(80) = 0.17364817766693034885171662676931
2580 return (calculateCosTiltAngle() >= 0.173648178f);
2581 }
2582
2583 /*-----------------------------------------------------------
2584 * Checks if position sensor (GPS) is failing for a specified timeout (if enabled)
2585 *-----------------------------------------------------------*/
2586 bool checkForPositionSensorTimeout(void)
2587 {
2588 if (navConfig()->general.pos_failure_timeout) {
2589 if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
2590 return true;
2591 }
2592 else {
2593 return false;
2594 }
2595 }
2596 else {
2597 // Timeout not defined, never fail
2598 return false;
2599 }
2600 }
2601
2602 /*-----------------------------------------------------------
2603 * Processes an update to XY-position and velocity
2604 *-----------------------------------------------------------*/
2605 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
2606 {
2607 posControl.actualState.abs.pos.x = newX;
2608 posControl.actualState.abs.pos.y = newY;
2609 posControl.actualState.abs.vel.x = newVelX;
2610 posControl.actualState.abs.vel.y = newVelY;
2611
2612 posControl.actualState.agl.pos.x = newX;
2613 posControl.actualState.agl.pos.y = newY;
2614 posControl.actualState.agl.vel.x = newVelX;
2615 posControl.actualState.agl.vel.y = newVelY;
2616
2617 posControl.actualState.velXY = calc_length_pythagorean_2D(newVelX, newVelY);
2618
2619 // CASE 1: POS & VEL valid
2620 if (estPosValid && estVelValid) {
2621 posControl.flags.estPosStatus = EST_TRUSTED;
2622 posControl.flags.estVelStatus = EST_TRUSTED;
2623 posControl.flags.horizontalPositionDataNew = true;
2624 posControl.lastValidPositionTimeMs = millis();
2625 }
2626 // CASE 1: POS invalid, VEL valid
2627 else if (!estPosValid && estVelValid) {
2628 posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
2629 posControl.flags.estVelStatus = EST_TRUSTED;
2630 posControl.flags.horizontalPositionDataNew = true;
2631 posControl.lastValidPositionTimeMs = millis();
2632 }
2633 // CASE 3: can't use pos/vel data
2634 else {
2635 posControl.flags.estPosStatus = EST_NONE;
2636 posControl.flags.estVelStatus = EST_NONE;
2637 posControl.flags.horizontalPositionDataNew = false;
2638 }
2639
2640 //Update blackbox data
2641 navLatestActualPosition[X] = newX;
2642 navLatestActualPosition[Y] = newY;
2643 navActualVelocity[X] = constrain(newVelX, -32678, 32767);
2644 navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
2645 }
2646
2647 /*-----------------------------------------------------------
2648 * Processes an update to Z-position and velocity
2649 *-----------------------------------------------------------*/
2650 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
2651 {
2652 posControl.actualState.abs.pos.z = newAltitude;
2653 posControl.actualState.abs.vel.z = newVelocity;
2654
2655 posControl.actualState.agl.pos.z = surfaceDistance;
2656 posControl.actualState.agl.vel.z = surfaceVelocity;
2657
2658 // Update altitude that would be used when executing RTH
2659 if (estimateValid) {
2660 updateDesiredRTHAltitude();
2661
2662 // If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
2663 if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
2664 // If we are in terrain-following modes - signal that we should update the surface tracking setpoint
2665 // NONE/USABLE means that we were flying blind, now we should lock to surface
2666 //updateSurfaceTrackingSetpoint();
2667 }
2668
2669 posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
2670 posControl.flags.estAltStatus = EST_TRUSTED;
2671 posControl.flags.verticalPositionDataNew = true;
2672 posControl.lastValidAltitudeTimeMs = millis();
2673 /* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
2674 posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
2675 }
2676 else {
2677 posControl.flags.estAltStatus = EST_NONE;
2678 posControl.flags.estAglStatus = EST_NONE;
2679 posControl.flags.verticalPositionDataNew = false;
2680 posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
2681 }
2682
2683 if (ARMING_FLAG(ARMED)) {
2684 if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
2685 if (posControl.actualState.surfaceMin > 0) {
2686 posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
2687 }
2688 else {
2689 posControl.actualState.surfaceMin = surfaceDistance;
2690 }
2691 }
2692 }
2693 else {
2694 posControl.actualState.surfaceMin = -1;
2695 }
2696
2697 //Update blackbox data
2698 navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
2699 navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
2700 }
2701
2702 /*-----------------------------------------------------------
2703 * Processes an update to estimated heading
2704 *-----------------------------------------------------------*/
2705 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
2706 {
2707 /* Update heading. Check if we're acquiring a valid heading for the
2708 * first time and update home heading accordingly.
2709 */
2710
2711 navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
2712
2713 #ifdef USE_DEV_TOOLS
2714 if (systemConfig()->groundTestMode && STATE(AIRPLANE)) {
2715 newEstHeading = EST_TRUSTED;
2716 }
2717 #endif
2718 if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
2719 (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
2720 (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
2721
2722 // Home was stored using the fake heading (assuming boot as 0deg). Calculate
2723 // the offset from the fake to the actual yaw and apply the same rotation
2724 // to the home point.
2725 int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
2726 posControl.rthState.homePosition.heading += fakeToRealYawOffset;
2727 posControl.rthState.homePosition.heading = wrap_36000(posControl.rthState.homePosition.heading);
2728
2729 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2730 }
2731
2732 posControl.actualState.yaw = newHeading;
2733 posControl.actualState.cog = newGroundCourse;
2734 posControl.flags.estHeadingStatus = newEstHeading;
2735
2736 /* Precompute sin/cos of yaw angle */
2737 posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2738 posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2739 }
2740
2741 /*-----------------------------------------------------------
2742 * Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
2743 *-----------------------------------------------------------*/
2744 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
2745 {
2746 return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
2747 }
2748
2749 /*-----------------------------------------------------------
2750 * Calculates distance and bearing to destination point
2751 *-----------------------------------------------------------*/
2752 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
2753 {
2754 return calc_length_pythagorean_2D(deltaX, deltaY);
2755 }
2756
2757 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
2758 {
2759 return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
2760 }
2761
2762 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
2763 {
2764 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2765 const float deltaX = destinationPos->x - posvel->pos.x;
2766 const float deltaY = destinationPos->y - posvel->pos.y;
2767
2768 return calculateDistanceFromDelta(deltaX, deltaY);
2769 }
2770
2771 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
2772 {
2773 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2774 const float deltaX = destinationPos->x - posvel->pos.x;
2775 const float deltaY = destinationPos->y - posvel->pos.y;
2776
2777 return calculateBearingFromDelta(deltaX, deltaY);
2778 }
2779
2780 int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos)
2781 {
2782 const float deltaX = endPos->x - startPos->x;
2783 const float deltaY = endPos->y - startPos->y;
2784
2785 return calculateBearingFromDelta(deltaX, deltaY);
2786 }
2787
2788 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) // NOT USED ANYWHERE
2789 {
2790 if (posControl.flags.estPosStatus == EST_NONE ||
2791 posControl.flags.estHeadingStatus == EST_NONE) {
2792
2793 return false;
2794 }
2795
2796 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2797 const float deltaX = destinationPos->x - posvel->pos.x;
2798 const float deltaY = destinationPos->y - posvel->pos.y;
2799
2800 result->distance = calculateDistanceFromDelta(deltaX, deltaY);
2801 result->bearing = calculateBearingFromDelta(deltaX, deltaY);
2802 return true;
2803 }
2804
2805 static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
2806 {
2807 // Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
2808 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
2809 navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
2810
2811 if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
2812 uint8_t nextWpIndex = posControl.activeWaypointIndex + 1;
2813 if (nextWpAction == NAV_WP_ACTION_JUMP) {
2814 if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
2815 posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
2816 nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + posControl.startWpIndex;
2817 } else if (posControl.activeWaypointIndex + 2 <= posControl.startWpIndex + posControl.waypointCount - 1) {
2818 if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
2819 nextWpIndex++;
2820 } else {
2821 return false; // give up - too complicated
2822 }
2823 }
2824 }
2825 mapWaypointToLocalPosition(nextWpPos, &posControl.waypointList[nextWpIndex], 0);
2826 return true;
2827 }
2828 }
2829
2830 return false; // no position available
2831 }
2832
2833 /*-----------------------------------------------------------
2834 * Check if waypoint is/was reached.
2835 * waypointBearing stores initial bearing to waypoint
2836 *-----------------------------------------------------------*/
2837 static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
2838 {
2839 posControl.wpDistance = calculateDistanceToDestination(waypointPos);
2840
2841 // Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius
2842 // Check within 10% margin of circular loiter radius
2843 if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) {
2844 return true;
2845 }
2846
2847 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
2848 // If WP turn smoothing CUT option used WP is reached when start of turn is initiated
2849 if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
2850 posControl.flags.wpTurnSmoothingActive = false;
2851 return true;
2852 }
2853 // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
2854 // Same method for turn smoothing option but relative bearing set at 60 degrees
2855 uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
2856 if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
2857 return true;
2858 }
2859 }
2860
2861 return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
2862 }
2863
2864 bool isWaypointAltitudeReached(void)
2865 {
2866 return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < navConfig()->general.waypoint_enforce_altitude;
2867 }
2868
2869 static void updateHomePositionCompatibility(void)
2870 {
2871 geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
2872 GPS_distanceToHome = posControl.homeDistance * 0.01f;
2873 GPS_directionToHome = posControl.homeDirection * 0.01f;
2874 }
2875
2876 // Backdoor for RTH estimator
2877 float getFinalRTHAltitude(void)
2878 {
2879 return posControl.rthState.rthFinalAltitude;
2880 }
2881
2882 /*-----------------------------------------------------------
2883 * Update the RTH Altitudes
2884 *-----------------------------------------------------------*/
2885 static void updateDesiredRTHAltitude(void)
2886 {
2887 if (ARMING_FLAG(ARMED)) {
2888 if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
2889 || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
2890 switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
2891 case NAV_RTH_CLIMB_STAGE_AT_LEAST:
2892 posControl.rthState.rthClimbStageAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2893 break;
2894 case NAV_RTH_CLIMB_STAGE_EXTRA:
2895 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2896 break;
2897 }
2898
2899 switch (navConfig()->general.flags.rth_alt_control_mode) {
2900 case NAV_RTH_NO_ALT:
2901 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2902 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2903 break;
2904
2905 case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
2906 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
2907 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2908 break;
2909
2910 case NAV_RTH_MAX_ALT:
2911 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z);
2912 if (navConfig()->general.rth_altitude > 0) {
2913 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude);
2914 }
2915 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2916 break;
2917
2918 case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
2919 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2920 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2921 break;
2922
2923 case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
2924 default:
2925 posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2926 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2927 }
2928
2929 if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) {
2930 posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
2931 }
2932 }
2933 } else {
2934 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
2935 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2936 posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
2937 }
2938 }
2939
2940 /*-----------------------------------------------------------
2941 * RTH sanity test logic
2942 *-----------------------------------------------------------*/
2943 void initializeRTHSanityChecker(void)
2944 {
2945 const timeMs_t currentTimeMs = millis();
2946
2947 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2948 posControl.rthSanityChecker.rthSanityOK = true;
2949 posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2950 }
2951
2952 bool validateRTHSanityChecker(void)
2953 {
2954 const timeMs_t currentTimeMs = millis();
2955
2956 // Ability to disable sanity checker
2957 if (navConfig()->general.rth_abort_threshold == 0) {
2958 return true;
2959 }
2960
2961 // Check at 10Hz rate
2962 if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
2963 const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2964 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2965
2966 if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) {
2967 posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
2968 } else {
2969 // If while doing RTH we got even farther away from home - RTH is doing something crazy
2970 posControl.rthSanityChecker.rthSanityOK = (currentDistanceToHome - posControl.rthSanityChecker.minimalDistanceToHome) < navConfig()->general.rth_abort_threshold;
2971 }
2972 }
2973
2974 return posControl.rthSanityChecker.rthSanityOK;
2975 }
2976
2977 /*-----------------------------------------------------------
2978 * Reset home position to current position
2979 *-----------------------------------------------------------*/
2980 void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
2981 {
2982 // XY-position
2983 if ((useMask & NAV_POS_UPDATE_XY) != 0) {
2984 posControl.rthState.homePosition.pos.x = pos->x;
2985 posControl.rthState.homePosition.pos.y = pos->y;
2986 if (homeFlags & NAV_HOME_VALID_XY) {
2987 posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
2988 } else {
2989 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
2990 }
2991 }
2992
2993 // Z-position
2994 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
2995 posControl.rthState.homePosition.pos.z = pos->z;
2996 if (homeFlags & NAV_HOME_VALID_Z) {
2997 posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
2998 } else {
2999 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
3000 }
3001 }
3002
3003 // Heading
3004 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
3005 // Heading
3006 posControl.rthState.homePosition.heading = heading;
3007 if (homeFlags & NAV_HOME_VALID_HEADING) {
3008 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
3009 } else {
3010 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING;
3011 }
3012 }
3013
3014 posControl.homeDistance = 0;
3015 posControl.homeDirection = 0;
3016
3017 // Update target RTH altitude as a waypoint above home
3018 updateDesiredRTHAltitude();
3019
3020 // Reset RTH sanity checker for new home position if RTH active
3021 if (FLIGHT_MODE(NAV_RTH_MODE)) {
3022 initializeRTHSanityChecker();
3023 }
3024
3025 updateHomePositionCompatibility();
3026 ENABLE_STATE(GPS_FIX_HOME);
3027 }
3028
3029 static navigationHomeFlags_t navigationActualStateHomeValidity(void)
3030 {
3031 navigationHomeFlags_t flags = 0;
3032
3033 if (posControl.flags.estPosStatus >= EST_USABLE) {
3034 flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
3035 }
3036
3037 if (posControl.flags.estHeadingStatus >= EST_USABLE) {
3038 flags |= NAV_HOME_VALID_HEADING;
3039 }
3040
3041 return flags;
3042 }
3043
3044 #if defined(USE_SAFE_HOME)
3045 void checkSafeHomeState(bool shouldBeEnabled)
3046 {
3047 bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
3048 (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
3049 #ifdef USE_MULTI_FUNCTIONS
3050 safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
3051 #endif
3052
3053 if (safehomeNotApplicable) {
3054 shouldBeEnabled = false;
3055 } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
3056 // if safehomes are only used with failsafe and we're trying to enable safehome
3057 // then enable the safehome only with failsafe
3058 shouldBeEnabled = posControl.flags.forcedRTHActivated;
3059 }
3060 // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
3061 if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
3062 return;
3063 }
3064 if (shouldBeEnabled) {
3065 // set home to safehome
3066 setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
3067 posControl.safehomeState.isApplied = true;
3068 } else {
3069 // set home to original arming point
3070 setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
3071 posControl.safehomeState.isApplied = false;
3072 }
3073 // if we've changed the home position, update the distance and direction
3074 updateHomePosition();
3075 }
3076
3077 /***********************************************************
3078 * See if there are any safehomes near where we are arming.
3079 * If so, save the nearest one in case we need it later for RTH.
3080 **********************************************************/
3081 bool findNearestSafeHome(void)
3082 {
3083 posControl.safehomeState.index = -1;
3084 uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
3085 uint32_t distance_to_current;
3086 fpVector3_t currentSafeHome;
3087 gpsLocation_t shLLH;
3088 shLLH.alt = 0;
3089 for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
3090 if (!safeHomeConfig(i)->enabled)
3091 continue;
3092
3093 shLLH.lat = safeHomeConfig(i)->lat;
3094 shLLH.lon = safeHomeConfig(i)->lon;
3095 geoConvertGeodeticToLocal(&currentSafeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
3096 distance_to_current = calculateDistanceToDestination(&currentSafeHome);
3097 if (distance_to_current < nearest_safehome_distance) {
3098 // this safehome is the nearest so far - keep track of it.
3099 posControl.safehomeState.index = i;
3100 nearest_safehome_distance = distance_to_current;
3101 posControl.safehomeState.nearestSafeHome = currentSafeHome;
3102 }
3103 }
3104 if (posControl.safehomeState.index >= 0) {
3105 posControl.safehomeState.distance = nearest_safehome_distance;
3106 } else {
3107 posControl.safehomeState.distance = 0;
3108 }
3109 return posControl.safehomeState.distance > 0;
3110 }
3111 #endif
3112
3113 /*-----------------------------------------------------------
3114 * Update home position, calculate distance and bearing to home
3115 *-----------------------------------------------------------*/
3116 void updateHomePosition(void)
3117 {
3118 // Disarmed and have a valid position, constantly update home before first arm (depending on setting)
3119 // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm)
3120 static bool setHome = false;
3121 navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING;
3122
3123 if (!ARMING_FLAG(ARMED)) {
3124 if (posControl.flags.estPosStatus >= EST_USABLE) {
3125 const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
3126 setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
3127 switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
3128 case NAV_RESET_NEVER:
3129 break;
3130 case NAV_RESET_ON_FIRST_ARM:
3131 setHome |= !ARMING_FLAG(WAS_EVER_ARMED);
3132 break;
3133 case NAV_RESET_ON_EACH_ARM:
3134 setHome = true;
3135 break;
3136 }
3137 }
3138 }
3139 else {
3140 static bool isHomeResetAllowed = false;
3141 // If pilot so desires he may reset home position to current position
3142 if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
3143 if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
3144 homeUpdateFlags = 0;
3145 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);
3146 setHome = true;
3147 isHomeResetAllowed = false;
3148 }
3149 }
3150 else {
3151 isHomeResetAllowed = true;
3152 }
3153
3154 // Update distance and direction to home if armed (home is not updated when armed)
3155 if (STATE(GPS_FIX_HOME)) {
3156 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
3157 posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
3158 posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
3159 updateHomePositionCompatibility();
3160 }
3161
3162 setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm
3163 }
3164
3165 if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) {
3166 #if defined(USE_SAFE_HOME)
3167 findNearestSafeHome();
3168 #endif
3169 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
3170
3171 if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) {
3172 posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm
3173 }
3174 // save the current location in case it is replaced by a safehome or HOME_RESET
3175 posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
3176 setHome = false;
3177 }
3178 }
3179
3180 /* -----------------------------------------------------------
3181 * Override RTH preset altitude and Climb First option
3182 * using Pitch/Roll stick held for > 1 seconds
3183 * Climb First override limited to Fixed Wing only
3184 * Roll also cancels RTH trackback on Fixed Wing and Multirotor
3185 *-----------------------------------------------------------*/
3186 static bool rthAltControlStickOverrideCheck(unsigned axis)
3187 {
3188 if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated ||
3189 (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
3190 return false;
3191 }
3192 static timeMs_t rthOverrideStickHoldStartTime[2];
3193
3194 if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
3195 timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis];
3196
3197 if (!rthOverrideStickHoldStartTime[axis]) {
3198 rthOverrideStickHoldStartTime[axis] = millis();
3199 } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec
3200 if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude
3201 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
3202 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
3203 return true;
3204 } else if (axis == ROLL) { // ROLL right to override climb first
3205 return true;
3206 }
3207 }
3208 } else {
3209 rthOverrideStickHoldStartTime[axis] = 0;
3210 }
3211
3212 return false;
3213 }
3214
3215 /* ---------------------------------------------------
3216 * If climb stage is being used, see if it is time to
3217 * transiton in to turn.
3218 * Limited to fixed wing only.
3219 * --------------------------------------------------- */
3220 bool rthClimbStageActiveAndComplete(void) {
3221 if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
3222 if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
3223 return true;
3224 }
3225 }
3226
3227 return false;
3228 }
3229
3230 /* --------------------------------------------------------------------------------
3231 * == RTH Trackback ==
3232 * Saves track during flight which is used during RTH to back track
3233 * along arrival route rather than immediately heading directly toward home.
3234 * Max desired trackback distance set by user or limited by number of available points.
3235 * Reverts to normal RTH heading direct to home when end of track reached.
3236 * Trackpoints logged with precedence for course/altitude changes. Distance based changes
3237 * only logged if no course/altitude changes logged over an extended distance.
3238 * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
3239 * --------------------------------------------------------------------------------- */
3240 static void updateRthTrackback(bool forceSaveTrackPoint)
3241 {
3242 static bool suspendTracking = false;
3243 bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
3244 if (!fwLoiterIsActive && suspendTracking) {
3245 suspendTracking = false;
3246 }
3247
3248 if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
3249 return;
3250 }
3251
3252 // Record trackback points based on significant change in course/altitude until
3253 // points limit reached. Overwrite older points from then on.
3254 if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
3255 static int32_t previousTBTripDist; // cm
3256 static int16_t previousTBCourse; // degrees
3257 static int16_t previousTBAltitude; // meters
3258 static uint8_t distanceCounter = 0;
3259 bool saveTrackpoint = forceSaveTrackPoint;
3260 bool GPSCourseIsValid = isGPSHeadingValid();
3261
3262 // start recording when some distance from home, 50m seems reasonable.
3263 if (posControl.activeRthTBPointIndex < 0) {
3264 saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
3265
3266 previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
3267 previousTBTripDist = posControl.totalTripDistance;
3268 } else {
3269 // Minimum distance increment between course change track points when GPS course valid - set to 10m
3270 const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10);
3271
3272 // Altitude change
3273 if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) { // meters
3274 saveTrackpoint = true;
3275 } else if (distanceIncrement && GPSCourseIsValid) {
3276 // Course change - set to 45 degrees
3277 if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
3278 saveTrackpoint = true;
3279 } else if (distanceCounter >= 9) {
3280 // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change
3281 // and deviation from projected course path > 20m
3282 float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]);
3283
3284 fpVector3_t virtualCoursePoint;
3285 virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
3286 virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
3287
3288 saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20);
3289 }
3290 distanceCounter++;
3291 previousTBTripDist = posControl.totalTripDistance;
3292 } else if (!GPSCourseIsValid) {
3293 // if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m
3294 saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
3295 previousTBTripDist = posControl.totalTripDistance;
3296 }
3297
3298 // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
3299 if (distanceCounter && fwLoiterIsActive) {
3300 saveTrackpoint = suspendTracking = true;
3301 }
3302 }
3303
3304 // when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
3305 if (saveTrackpoint) {
3306 if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // wraparound to start
3307 posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0;
3308 } else {
3309 posControl.activeRthTBPointIndex++;
3310 if (posControl.rthTBWrapAroundCounter > -1) { // track wraparound overwrite position after store first filled
3311 posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex;
3312 }
3313 }
3314 posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos;
3315
3316 posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex;
3317 previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
3318 previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
3319 distanceCounter = 0;
3320 }
3321 }
3322 }
3323
3324 static fpVector3_t * rthGetTrackbackPos(void)
3325 {
3326 // ensure trackback altitude never lower than altitude of start point
3327 if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) {
3328 posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z;
3329 }
3330
3331 return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex];
3332 }
3333
3334 /*-----------------------------------------------------------
3335 * Update flight statistics
3336 *-----------------------------------------------------------*/
3337 static void updateNavigationFlightStatistics(void)
3338 {
3339 static timeMs_t previousTimeMs = 0;
3340 const timeMs_t currentTimeMs = millis();
3341 const timeDelta_t timeDeltaMs = currentTimeMs - previousTimeMs;
3342 previousTimeMs = currentTimeMs;
3343
3344 if (ARMING_FLAG(ARMED)) {
3345 posControl.totalTripDistance += posControl.actualState.velXY * MS2S(timeDeltaMs);
3346 }
3347 }
3348
3349 /*
3350 * Total travel distance in cm
3351 */
3352 uint32_t getTotalTravelDistance(void)
3353 {
3354 return lrintf(posControl.totalTripDistance);
3355 }
3356
3357 /*-----------------------------------------------------------
3358 * Calculate platform-specific hold position (account for deceleration)
3359 *-----------------------------------------------------------*/
3360 void calculateInitialHoldPosition(fpVector3_t * pos)
3361 {
3362 if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
3363 calculateFixedWingInitialHoldPosition(pos);
3364 }
3365 else {
3366 calculateMulticopterInitialHoldPosition(pos);
3367 }
3368 }
3369
3370 /*-----------------------------------------------------------
3371 * Set active XYZ-target and desired heading
3372 *-----------------------------------------------------------*/
3373 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
3374 {
3375 // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
3376 if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
3377 posControl.desiredState.pos.x = pos->x;
3378 posControl.desiredState.pos.y = pos->y;
3379 }
3380
3381 // Z-position
3382 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
3383 updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
3384 posControl.desiredState.pos.z = pos->z;
3385 }
3386
3387 // Heading
3388 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
3389 // Heading
3390 posControl.desiredState.yaw = yaw;
3391 }
3392 else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) {
3393 posControl.desiredState.yaw = calculateBearingToDestination(pos);
3394 }
3395 else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
3396 posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
3397 }
3398 }
3399
3400 void calculateFarAwayPos(fpVector3_t *farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance)
3401 {
3402 farAwayPos->x = start->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
3403 farAwayPos->y = start->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
3404 farAwayPos->z = start->z;
3405 }
3406
3407 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
3408 {
3409 calculateFarAwayPos(farAwayPos, &navGetCurrentActualPositionAndVelocity()->pos, bearing, distance);
3410 }
3411
3412 /*-----------------------------------------------------------
3413 * NAV land detector
3414 *-----------------------------------------------------------*/
3415 void updateLandingStatus(timeMs_t currentTimeMs)
3416 {
3417 if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
3418 return; // no point using this with a fixed wing if not set to disarm
3419 }
3420
3421 static timeMs_t lastUpdateTimeMs = 0;
3422 if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz
3423 return;
3424 }
3425 lastUpdateTimeMs = currentTimeMs;
3426
3427 DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
3428 DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
3429
3430 if (!ARMING_FLAG(ARMED)) {
3431 if (STATE(LANDING_DETECTED)) {
3432 landingDetectorIsActive = false;
3433 }
3434 resetLandingDetector();
3435
3436 if (!IS_RC_MODE_ACTIVE(BOXARM)) {
3437 DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
3438 }
3439 return;
3440 }
3441
3442 if (!landingDetectorIsActive) {
3443 if (isFlightDetected()) {
3444 landingDetectorIsActive = true;
3445 resetLandingDetector();
3446 }
3447 } else if (STATE(LANDING_DETECTED)) {
3448 pidResetErrorAccumulators();
3449 if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) {
3450 ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
3451 disarm(DISARM_LANDING);
3452 } else if (!navigationInAutomaticThrottleMode()) {
3453 // for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
3454 landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
3455 }
3456 } else if (isLandingDetected()) {
3457 ENABLE_STATE(LANDING_DETECTED);
3458 }
3459 }
3460
3461 bool isLandingDetected(void)
3462 {
3463 return STATE(AIRPLANE) ? isFixedWingLandingDetected() : isMulticopterLandingDetected();
3464 }
3465
3466 void resetLandingDetector(void)
3467 {
3468 DISABLE_STATE(LANDING_DETECTED);
3469 posControl.flags.resetLandingDetector = true;
3470 }
3471
3472 void resetLandingDetectorActiveState(void)
3473 {
3474 landingDetectorIsActive = false;
3475 }
3476
3477 bool isFlightDetected(void)
3478 {
3479 return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
3480 }
3481
3482 bool isProbablyStillFlying(void)
3483 {
3484 bool inFlightSanityCheck;
3485 if (STATE(MULTIROTOR)) {
3486 inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
3487 } else {
3488 inFlightSanityCheck = isGPSHeadingValid();
3489 }
3490
3491 return landingDetectorIsActive && inFlightSanityCheck;
3492 }
3493
3494 /*-----------------------------------------------------------
3495 * Z-position controller
3496 *-----------------------------------------------------------*/
3497 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
3498 {
3499 #define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
3500
3501 static timeUs_t lastUpdateTimeUs;
3502 timeUs_t currentTimeUs = micros();
3503
3504 // Terrain following uses different altitude measurement
3505 const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
3506
3507 if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
3508 /* ROC_TO_ALT_CONSTANT - constant climb rate
3509 * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
3510 * Rate reduction starts at distance from target altitude of 5 x climb rate */
3511
3512 if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
3513 const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
3514 const float absClimbRate = fabsf(desiredClimbRate);
3515 const uint16_t maxRateCutoffAlt = absClimbRate * 5;
3516 const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
3517 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
3518
3519 desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
3520 }
3521
3522 /*
3523 * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
3524 * In other words, when altitude is reached, allow it only to shrink
3525 */
3526 if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
3527 desiredClimbRate = 0;
3528 }
3529
3530 if (STATE(FIXED_WING_LEGACY)) {
3531 // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
3532 float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
3533 static bool targetHoldActive = false;
3534
3535 if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
3536 // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
3537 if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
3538 posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
3539 targetHoldActive = false;
3540 } else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
3541 posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
3542 targetHoldActive = true;
3543 }
3544 } else {
3545 targetHoldActive = false;
3546 }
3547 }
3548 else {
3549 // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
3550 posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
3551 }
3552 } else { // ROC_TO_ALT_RESET or zero desired climbrate
3553 posControl.desiredState.pos.z = altitudeToUse;
3554 }
3555
3556 lastUpdateTimeUs = currentTimeUs;
3557 }
3558
3559 static void resetAltitudeController(bool useTerrainFollowing)
3560 {
3561 // Set terrain following flag
3562 posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
3563
3564 if (STATE(FIXED_WING_LEGACY)) {
3565 resetFixedWingAltitudeController();
3566 }
3567 else {
3568 resetMulticopterAltitudeController();
3569 }
3570 }
3571
3572 static void setupAltitudeController(void)
3573 {
3574 if (STATE(FIXED_WING_LEGACY)) {
3575 setupFixedWingAltitudeController();
3576 }
3577 else {
3578 setupMulticopterAltitudeController();
3579 }
3580 }
3581
3582 static bool adjustAltitudeFromRCInput(void)
3583 {
3584 if (STATE(FIXED_WING_LEGACY)) {
3585 return adjustFixedWingAltitudeFromRCInput();
3586 }
3587 else {
3588 return adjustMulticopterAltitudeFromRCInput();
3589 }
3590 }
3591
3592 /*-----------------------------------------------------------
3593 * Jump Counter support functions
3594 *-----------------------------------------------------------*/
3595 static void setupJumpCounters(void)
3596 {
3597 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
3598 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
3599 posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
3600 }
3601 }
3602 }
3603
3604 static void resetJumpCounter(void)
3605 {
3606 // reset the volatile counter from the set / static value
3607 posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p2;
3608 }
3609
3610 static void clearJumpCounters(void)
3611 {
3612 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
3613 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
3614 posControl.waypointList[wp].p3 = 0;
3615 }
3616 }
3617 }
3618
3619
3620
3621 /*-----------------------------------------------------------
3622 * Heading controller (pass-through to MAG mode)
3623 *-----------------------------------------------------------*/
3624 static void resetHeadingController(void)
3625 {
3626 if (STATE(FIXED_WING_LEGACY)) {
3627 resetFixedWingHeadingController();
3628 }
3629 else {
3630 resetMulticopterHeadingController();
3631 }
3632 }
3633
3634 static bool adjustHeadingFromRCInput(void)
3635 {
3636 if (STATE(FIXED_WING_LEGACY)) {
3637 return adjustFixedWingHeadingFromRCInput();
3638 }
3639 else {
3640 return adjustMulticopterHeadingFromRCInput();
3641 }
3642 }
3643
3644 /*-----------------------------------------------------------
3645 * XY Position controller
3646 *-----------------------------------------------------------*/
3647 static void resetPositionController(void)
3648 {
3649 if (STATE(FIXED_WING_LEGACY)) {
3650 resetFixedWingPositionController();
3651 }
3652 else {
3653 resetMulticopterPositionController();
3654 resetMulticopterBrakingMode();
3655 }
3656 }
3657
3658 static bool adjustPositionFromRCInput(void)
3659 {
3660 bool retValue;
3661
3662 if (STATE(FIXED_WING_LEGACY)) {
3663 retValue = adjustFixedWingPositionFromRCInput();
3664 }
3665 else {
3666
3667 const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
3668 const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
3669
3670 retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
3671 }
3672
3673 return retValue;
3674 }
3675
3676 /*-----------------------------------------------------------
3677 * WP controller
3678 *-----------------------------------------------------------*/
3679 void resetGCSFlags(void)
3680 {
3681 posControl.flags.isGCSAssistedNavigationReset = false;
3682 posControl.flags.isGCSAssistedNavigationEnabled = false;
3683 }
3684
3685 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
3686 {
3687 /* Default waypoint to send */
3688 wpData->action = NAV_WP_ACTION_RTH;
3689 wpData->lat = 0;
3690 wpData->lon = 0;
3691 wpData->alt = 0;
3692 wpData->p1 = 0;
3693 wpData->p2 = 0;
3694 wpData->p3 = 0;
3695 wpData->flag = NAV_WP_FLAG_LAST;
3696
3697 // WP #0 - special waypoint - HOME
3698 if (wpNumber == 0) {
3699 if (STATE(GPS_FIX_HOME)) {
3700 wpData->lat = GPS_home.lat;
3701 wpData->lon = GPS_home.lon;
3702 wpData->alt = GPS_home.alt;
3703 }
3704 }
3705 // WP #255 - special waypoint - directly get actualPosition
3706 else if (wpNumber == 255) {
3707 gpsLocation_t wpLLH;
3708
3709 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
3710
3711 wpData->lat = wpLLH.lat;
3712 wpData->lon = wpLLH.lon;
3713 wpData->alt = wpLLH.alt;
3714 }
3715 // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode
3716 else if (wpNumber == 254) {
3717 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3718
3719 if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
3720 gpsLocation_t wpLLH;
3721
3722 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
3723
3724 wpData->lat = wpLLH.lat;
3725 wpData->lon = wpLLH.lon;
3726 wpData->alt = wpLLH.alt;
3727 }
3728 }
3729 // WP #1 - #60 - common waypoints - pre-programmed mission
3730 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
3731 if (wpNumber <= getWaypointCount()) {
3732 *wpData = posControl.waypointList[wpNumber - 1 + (ARMING_FLAG(ARMED) ? posControl.startWpIndex : 0)];
3733 if(wpData->action == NAV_WP_ACTION_JUMP) {
3734 wpData->p1 += 1; // make WP # (vice index)
3735 }
3736 }
3737 }
3738 }
3739
3740 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
3741 {
3742 gpsLocation_t wpLLH;
3743 navWaypointPosition_t wpPos;
3744
3745 // Pre-fill structure to convert to local coordinates
3746 wpLLH.lat = wpData->lat;
3747 wpLLH.lon = wpData->lon;
3748 wpLLH.alt = wpData->alt;
3749
3750 // WP #0 - special waypoint - HOME
3751 if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
3752 // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
3753 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
3754 setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
3755 }
3756 // WP #255 - special waypoint - directly set desiredPosition
3757 // Only valid when armed and in poshold mode
3758 else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
3759 ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
3760 (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
3761 // Convert to local coordinates
3762 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
3763
3764 navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
3765
3766 // If we received global altitude == 0, use current altitude
3767 if (wpData->alt != 0) {
3768 waypointUpdateFlags |= NAV_POS_UPDATE_Z;
3769 }
3770
3771 if (wpData->p1 > 0 && wpData->p1 < 360) {
3772 waypointUpdateFlags |= NAV_POS_UPDATE_HEADING;
3773 }
3774
3775 setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
3776 }
3777 // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
3778 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
3779 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 ) {
3780 // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
3781 static int8_t nonGeoWaypointCount = 0;
3782
3783 if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
3784 if (wpNumber == 1) {
3785 resetWaypointList();
3786 }
3787 posControl.waypointList[wpNumber - 1] = *wpData;
3788 if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
3789 nonGeoWaypointCount += 1;
3790 if(wpData->action == NAV_WP_ACTION_JUMP) {
3791 posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
3792 }
3793 }
3794
3795 posControl.waypointCount = wpNumber;
3796 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
3797 posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
3798 if (posControl.waypointListValid) {
3799 nonGeoWaypointCount = 0;
3800 }
3801 }
3802 }
3803 }
3804 }
3805
3806 void resetWaypointList(void)
3807 {
3808 posControl.waypointCount = 0;
3809 posControl.waypointListValid = false;
3810 posControl.geoWaypointCount = 0;
3811 posControl.startWpIndex = 0;
3812 #ifdef USE_MULTI_MISSION
3813 posControl.totalMultiMissionWpCount = 0;
3814 posControl.loadedMultiMissionIndex = 0;
3815 posControl.multiMissionCount = 0;
3816 #endif
3817 }
3818
3819 bool isWaypointListValid(void)
3820 {
3821 return posControl.waypointListValid;
3822 }
3823
3824 int getWaypointCount(void)
3825 {
3826 uint8_t waypointCount = posControl.waypointCount;
3827 #ifdef USE_MULTI_MISSION
3828 if (!ARMING_FLAG(ARMED) && posControl.totalMultiMissionWpCount) {
3829 waypointCount = posControl.totalMultiMissionWpCount;
3830 }
3831 #endif
3832 return waypointCount;
3833 }
3834
3835 #ifdef USE_MULTI_MISSION
3836 void selectMultiMissionIndex(int8_t increment)
3837 {
3838 if (posControl.multiMissionCount > 1) { // stick selection only active when multi mission loaded
3839 navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 1, posControl.multiMissionCount);
3840 }
3841 }
3842
3843 void loadSelectedMultiMission(uint8_t missionIndex)
3844 {
3845 uint8_t missionCount = 1;
3846 posControl.waypointCount = 0;
3847 posControl.geoWaypointCount = 0;
3848
3849 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3850 if (missionCount == missionIndex) {
3851 /* store details of selected mission: start wp index, mission wp count, geo wp count */
3852 if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
3853 posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
3854 posControl.waypointList[i].action == NAV_WP_ACTION_JUMP)) {
3855 posControl.geoWaypointCount++;
3856 }
3857 // mission start WP
3858 if (posControl.waypointCount == 0) {
3859 posControl.waypointCount = 1; // start marker only, value unimportant (but not 0)
3860 posControl.startWpIndex = i;
3861 }
3862 // mission end WP
3863 if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
3864 posControl.waypointCount = i - posControl.startWpIndex + 1;
3865 break;
3866 }
3867 } else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
3868 missionCount++;
3869 }
3870 }
3871
3872 posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
3873 posControl.activeWaypointIndex = posControl.startWpIndex;
3874 }
3875
3876 bool updateWpMissionChange(void)
3877 {
3878 /* Function only called when ARMED */
3879
3880 if (posControl.multiMissionCount < 2 || posControl.wpPlannerActiveWPIndex || FLIGHT_MODE(NAV_WP_MODE)) {
3881 return true;
3882 }
3883
3884 uint8_t setMissionIndex = navConfig()->general.waypoint_multi_mission_index;
3885 if (!(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) || isAdjustmentFunctionSelected(ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX))) {
3886 /* reload mission if mission index changed */
3887 if (posControl.loadedMultiMissionIndex != setMissionIndex) {
3888 loadSelectedMultiMission(setMissionIndex);
3889 }
3890 return true;
3891 }
3892
3893 static bool toggleFlag = false;
3894 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) {
3895 if (setMissionIndex == posControl.multiMissionCount) {
3896 navConfigMutable()->general.waypoint_multi_mission_index = 1;
3897 } else {
3898 selectMultiMissionIndex(1);
3899 }
3900 toggleFlag = false;
3901 } else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
3902 toggleFlag = true;
3903 }
3904 return false; // block WP mode while changing mission when armed
3905 }
3906
3907 bool checkMissionCount(int8_t waypoint)
3908 {
3909 if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
3910 posControl.multiMissionCount += 1; // count up no missions in multi mission WP file
3911 if (waypoint != NAV_MAX_WAYPOINTS - 1) {
3912 return (nonVolatileWaypointList(waypoint + 1)->flag == NAV_WP_FLAG_LAST &&
3913 nonVolatileWaypointList(waypoint + 1)->action ==NAV_WP_ACTION_RTH);
3914 // end of multi mission file if successive NAV_WP_FLAG_LAST and default action (RTH)
3915 }
3916 }
3917 return false;
3918 }
3919 #endif // multi mission
3920 #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
3921 bool loadNonVolatileWaypointList(bool clearIfLoaded)
3922 {
3923 /* Don't load if armed or mission planner active */
3924 if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
3925 return false;
3926 }
3927
3928 // if forced and waypoints are already loaded, just unload them.
3929 if (clearIfLoaded && posControl.waypointCount > 0) {
3930 resetWaypointList();
3931 return false;
3932 }
3933 #ifdef USE_MULTI_MISSION
3934 /* Reset multi mission index to 1 if exceeds number of available missions */
3935 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
3936 navConfigMutable()->general.waypoint_multi_mission_index = 1;
3937 }
3938 #endif
3939 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3940 setWaypoint(i + 1, nonVolatileWaypointList(i));
3941 #ifdef USE_MULTI_MISSION
3942 /* count up number of missions and exit after last multi mission */
3943 if (checkMissionCount(i)) {
3944 break;
3945 }
3946 }
3947 posControl.totalMultiMissionWpCount = posControl.waypointCount;
3948 loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
3949
3950 /* Mission sanity check failed - reset the list
3951 * Also reset if no selected mission loaded (shouldn't happen) */
3952 if (!posControl.waypointListValid || !posControl.waypointCount) {
3953 #else
3954 // check this is the last waypoint
3955 if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
3956 break;
3957 }
3958 }
3959
3960 // Mission sanity check failed - reset the list
3961 if (!posControl.waypointListValid) {
3962 #endif
3963 resetWaypointList();
3964 }
3965
3966 return posControl.waypointListValid;
3967 }
3968
3969 bool saveNonVolatileWaypointList(void)
3970 {
3971 if (ARMING_FLAG(ARMED) || !posControl.waypointListValid)
3972 return false;
3973
3974 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3975 getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
3976 }
3977 #ifdef USE_MULTI_MISSION
3978 navConfigMutable()->general.waypoint_multi_mission_index = 1; // reset selected mission to 1 when new entries saved
3979 #endif
3980 saveConfigAndNotify();
3981
3982 return true;
3983 }
3984 #endif
3985
3986 #if defined(USE_SAFE_HOME)
3987
3988 void resetSafeHomes(void)
3989 {
3990 memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
3991 }
3992 #endif
3993
3994 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
3995 {
3996 gpsLocation_t wpLLH;
3997
3998 /* Default to home position if lat & lon = 0 or HOME flag set
3999 * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
4000 if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
4001 wpLLH.lat = GPS_home.lat;
4002 wpLLH.lon = GPS_home.lon;
4003 } else {
4004 wpLLH.lat = waypoint->lat;
4005 wpLLH.lon = waypoint->lon;
4006 }
4007 wpLLH.alt = waypoint->alt;
4008
4009 geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
4010 }
4011
4012 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
4013 {
4014 // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
4015 if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
4016 posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
4017 } else {
4018 posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
4019 }
4020 posControl.activeWaypoint.nextTurnAngle = -1; // no turn angle set (-1), will be set by WP mode as required
4021
4022 posControl.activeWaypoint.pos = *pos;
4023
4024 // Set desired position to next waypoint (XYZ-controller)
4025 setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
4026 }
4027
4028 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
4029 {
4030 return ((datumFlag & NAV_WP_MSL_DATUM) == NAV_WP_MSL_DATUM) ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
4031 }
4032
4033 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
4034 {
4035 fpVector3_t localPos;
4036 mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
4037 calculateAndSetActiveWaypointToLocalPosition(&localPos);
4038
4039 if (navConfig()->fw.wp_turn_smoothing) {
4040 fpVector3_t posNextWp;
4041 if (getLocalPosNextWaypoint(&posNextWp)) {
4042 int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
4043 posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
4044 }
4045 }
4046 }
4047
4048 /* Checks if active waypoint is last in mission */
4049 bool isLastMissionWaypoint(void)
4050 {
4051 return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.startWpIndex + posControl.waypointCount - 1) ||
4052 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
4053 }
4054
4055 /* Checks if Nav hold position is active */
4056 bool isNavHoldPositionActive(void)
4057 {
4058 // WP mode last WP hold and Timed hold positions
4059 if (FLIGHT_MODE(NAV_WP_MODE)) {
4060 return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
4061 }
4062 // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
4063 // Also hold position during emergency landing if position valid
4064 return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
4065 FLIGHT_MODE(NAV_POSHOLD_MODE) ||
4066 navigationIsExecutingAnEmergencyLanding();
4067 }
4068
4069 float getActiveSpeed(void)
4070 {
4071 /* Currently only applicable for multicopter */
4072
4073 // Speed limit for modes where speed manually controlled
4074 if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
4075 return navConfig()->general.max_manual_speed;
4076 }
4077
4078 uint16_t waypointSpeed = navConfig()->general.auto_speed;
4079
4080 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
4081 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)) {
4082 float wpSpecificSpeed = 0.0f;
4083 if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
4084 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
4085 else
4086 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
4087
4088 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
4089 waypointSpeed = wpSpecificSpeed;
4090 } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
4091 waypointSpeed = navConfig()->general.max_auto_speed;
4092 }
4093 }
4094 }
4095
4096 return waypointSpeed;
4097 }
4098
4099 bool isWaypointNavTrackingActive(void)
4100 {
4101 // NAV_WP_MODE flag used rather than state flag NAV_AUTO_WP to ensure heading to initial waypoint
4102 // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
4103 // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
4104
4105 return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
4106 }
4107
4108 /*-----------------------------------------------------------
4109 * Process adjustments to alt, pos and yaw controllers
4110 *-----------------------------------------------------------*/
4111 static void processNavigationRCAdjustments(void)
4112 {
4113 /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
4114 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
4115
4116 if (FLIGHT_MODE(FAILSAFE_MODE)) {
4117 if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
4118 resetMulticopterBrakingMode();
4119 }
4120 posControl.flags.isAdjustingAltitude = false;
4121 posControl.flags.isAdjustingPosition = false;
4122 posControl.flags.isAdjustingHeading = false;
4123
4124 return;
4125 }
4126
4127 posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
4128 posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
4129 posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
4130 }
4131
4132 /*-----------------------------------------------------------
4133 * A main function to call position controllers at loop rate
4134 *-----------------------------------------------------------*/
4135 void applyWaypointNavigationAndAltitudeHold(void)
4136 {
4137 const timeUs_t currentTimeUs = micros();
4138
4139 //Updata blackbox data
4140 navFlags = 0;
4141 if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
4142 if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
4143 if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
4144 if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
4145 #if defined(NAV_GPS_GLITCH_DETECTION)
4146 if (isGPSGlitchDetected()) navFlags |= (1 << 4);
4147 #endif
4148 if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
4149
4150 // Reset all navigation requests - NAV controllers will set them if necessary
4151 DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
4152
4153 // No navigation when disarmed
4154 if (!ARMING_FLAG(ARMED)) {
4155 // If we are disarmed, abort forced RTH or Emergency Landing
4156 posControl.flags.forcedRTHActivated = false;
4157 posControl.flags.forcedEmergLandingActivated = false;
4158 posControl.flags.manualEmergLandActive = false;
4159 // ensure WP missions always restart from first waypoint after disarm
4160 posControl.activeWaypointIndex = posControl.startWpIndex;
4161 // Reset RTH trackback
4162 posControl.activeRthTBPointIndex = -1;
4163 posControl.flags.rthTrackbackActive = false;
4164 posControl.rthTBWrapAroundCounter = -1;
4165
4166 return;
4167 }
4168
4169 /* Reset flags */
4170 posControl.flags.horizontalPositionDataConsumed = false;
4171 posControl.flags.verticalPositionDataConsumed = false;
4172
4173 #ifdef USE_FW_AUTOLAND
4174 if (!isFwLandInProgess()) {
4175 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
4176 }
4177 #endif
4178
4179 /* Process controllers */
4180 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
4181 if (STATE(ROVER) || STATE(BOAT)) {
4182 applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
4183 } else if (STATE(FIXED_WING_LEGACY)) {
4184 applyFixedWingNavigationController(navStateFlags, currentTimeUs);
4185 }
4186 else {
4187 applyMulticopterNavigationController(navStateFlags, currentTimeUs);
4188 }
4189
4190 /* Consume position data */
4191 if (posControl.flags.horizontalPositionDataConsumed)
4192 posControl.flags.horizontalPositionDataNew = false;
4193
4194 if (posControl.flags.verticalPositionDataConsumed)
4195 posControl.flags.verticalPositionDataNew = false;
4196
4197 //Update blackbox data
4198 if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
4199 if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
4200 if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
4201
4202 navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
4203 navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
4204 navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
4205
4206 navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
4207 }
4208
4209 /*-----------------------------------------------------------
4210 * Set CF's FLIGHT_MODE from current NAV_MODE
4211 *-----------------------------------------------------------*/
4212 void switchNavigationFlightModes(void)
4213 {
4214 const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
4215 const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
4216 DISABLE_FLIGHT_MODE(disabledFlightModes);
4217 ENABLE_FLIGHT_MODE(enabledNavFlightModes);
4218 }
4219
4220 /*-----------------------------------------------------------
4221 * desired NAV_MODE from combination of FLIGHT_MODE flags
4222 *-----------------------------------------------------------*/
4223 static bool canActivateAltHoldMode(void)
4224 {
4225 return (posControl.flags.estAltStatus >= EST_USABLE);
4226 }
4227
4228 static bool canActivatePosHoldMode(void)
4229 {
4230 return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
4231 }
4232
4233 static bool canActivateNavigationModes(void)
4234 {
4235 return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
4236 }
4237
4238 static bool isWaypointMissionValid(void)
4239 {
4240 return posControl.waypointListValid && (posControl.waypointCount > 0);
4241 }
4242
4243 void checkManualEmergencyLandingControl(bool forcedActivation)
4244 {
4245 static timeMs_t timeout = 0;
4246 static int8_t counter = 0;
4247 static bool toggle;
4248 timeMs_t currentTimeMs = millis();
4249
4250 if (timeout && currentTimeMs > timeout) {
4251 timeout += 1000;
4252 counter -= counter ? 1 : 0;
4253 if (!counter) {
4254 timeout = 0;
4255 }
4256 }
4257 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
4258 if (!timeout && toggle) {
4259 timeout = currentTimeMs + 4000;
4260 }
4261 counter += toggle;
4262 toggle = false;
4263 } else {
4264 toggle = true;
4265 }
4266
4267 // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
4268 if (counter >= 5 || forcedActivation) {
4269 counter = 0;
4270 posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
4271
4272 if (!posControl.flags.manualEmergLandActive) {
4273 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4274 }
4275 }
4276 }
4277
4278 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
4279 {
4280 static bool canActivateLaunchMode = false;
4281
4282 //We can switch modes only when ARMED
4283 if (ARMING_FLAG(ARMED)) {
4284 // Ask failsafe system if we can use navigation system
4285 if (failsafeBypassNavigation()) {
4286 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4287 }
4288
4289 // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
4290 const bool canActivateAltHold = canActivateAltHoldMode();
4291 const bool canActivatePosHold = canActivatePosHoldMode();
4292 const bool canActivateNavigation = canActivateNavigationModes();
4293 const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
4294 #ifdef USE_SAFE_HOME
4295 checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
4296 #endif
4297 // deactivate rth trackback if RTH not active
4298 if (posControl.flags.rthTrackbackActive) {
4299 posControl.flags.rthTrackbackActive = isExecutingRTH;
4300 }
4301
4302 /* Emergency landing controlled manually by rapid switching of Poshold mode.
4303 * Landing toggled ON or OFF for each Poshold activation sequence */
4304 checkManualEmergencyLandingControl(false);
4305
4306 /* Emergency landing triggered by failsafe Landing or manually initiated */
4307 if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
4308 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
4309 }
4310
4311 /* Keep Emergency landing mode active once triggered.
4312 * If caused by sensor failure - landing auto cancelled if sensors working again or when WP and RTH deselected or if Althold selected.
4313 * If caused by RTH Sanity Checking - landing cancelled if RTH deselected.
4314 * Remains active if failsafe active regardless of mode selections */
4315 if (navigationIsExecutingAnEmergencyLanding()) {
4316 bool autonomousNavIsPossible = canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME);
4317 bool emergLandingCancel = (!autonomousNavIsPossible &&
4318 ((IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) || !(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH)))) ||
4319 (autonomousNavIsPossible && !IS_RC_MODE_ACTIVE(BOXNAVRTH));
4320
4321 if ((!posControl.rthSanityChecker.rthSanityOK || !autonomousNavIsPossible) && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) {
4322 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
4323 }
4324 }
4325 posControl.rthSanityChecker.rthSanityOK = true;
4326
4327 /* WP mission activation control:
4328 * canActivateWaypoint & waypointWasActivated are used to prevent WP mission
4329 * auto restarting after interruption by Manual or RTH modes.
4330 * WP mode must be deselected before it can be reactivated again. */
4331 static bool waypointWasActivated = false;
4332 const bool isWpMissionLoaded = isWaypointMissionValid();
4333 bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner
4334
4335 if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
4336 canActivateWaypoint = false;
4337 if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4338 canActivateWaypoint = true;
4339 waypointWasActivated = false;
4340 }
4341 }
4342
4343 /* Airplane specific modes */
4344 if (STATE(AIRPLANE)) {
4345 // LAUNCH mode has priority over any other NAV mode
4346 if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
4347 if (canActivateLaunchMode) {
4348 canActivateLaunchMode = false;
4349 return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
4350 }
4351 else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
4352 // Make sure we don't bail out to IDLE
4353 return NAV_FSM_EVENT_NONE;
4354 }
4355 }
4356 else {
4357 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low or throttle stick < launch idle throttle
4358 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
4359 if (abortLaunchAllowed()) {
4360 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4361 } else {
4362 return NAV_FSM_EVENT_NONE;
4363 }
4364 }
4365 }
4366
4367 /* Soaring mode, disables altitude control in Position hold and Course hold modes.
4368 * Pitch allowed to freefloat within defined Angle mode deadband */
4369 if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
4370 ENABLE_FLIGHT_MODE(SOARING_MODE);
4371 } else {
4372 DISABLE_FLIGHT_MODE(SOARING_MODE);
4373 }
4374 }
4375
4376 /* If we request forced RTH - attempt to activate it no matter what
4377 * This might switch to emergency landing controller if GPS is unavailable */
4378 if (posControl.flags.forcedRTHActivated) {
4379 return NAV_FSM_EVENT_SWITCH_TO_RTH;
4380 }
4381
4382 /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded.
4383 * WP prevented from falling back to RTH if WP mission planner is active */
4384 const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive;
4385 if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
4386 // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
4387 // Without this loss of any of the canActivateNavigation && canActivateAltHold
4388 // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
4389 // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc)
4390 if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
4391 return NAV_FSM_EVENT_SWITCH_TO_RTH;
4392 }
4393 }
4394
4395 // MANUAL mode has priority over WP/PH/AH
4396 if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
4397 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4398 }
4399
4400 // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
4401 // Also check multimission mission change status before activating WP mode.
4402 #ifdef USE_MULTI_MISSION
4403 if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
4404 #else
4405 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
4406 #endif
4407 if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
4408 waypointWasActivated = true;
4409 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
4410 }
4411 }
4412
4413 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
4414 if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
4415 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
4416 }
4417
4418 // CRUISE has priority over COURSE_HOLD and AH
4419 if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
4420 if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
4421 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
4422 }
4423
4424 // PH has priority over COURSE_HOLD
4425 // CRUISE has priority on AH
4426 if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
4427 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
4428 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
4429 }
4430
4431 if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) {
4432 return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
4433 }
4434 }
4435
4436 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
4437 if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
4438 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
4439 }
4440 } else {
4441 // 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)
4442 canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid()));
4443 }
4444
4445 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4446 }
4447
4448 /*-----------------------------------------------------------
4449 * An indicator that throttle tilt compensation is forced
4450 *-----------------------------------------------------------*/
4451 bool navigationRequiresThrottleTiltCompensation(void)
4452 {
4453 return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
4454 }
4455
4456 /*-----------------------------------------------------------
4457 * An indicator that ANGLE mode must be forced per NAV requirement
4458 *-----------------------------------------------------------*/
4459 bool navigationRequiresAngleMode(void)
4460 {
4461 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
4462 return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
4463 }
4464
4465 /*-----------------------------------------------------------
4466 * An indicator that TURN ASSISTANCE is required for navigation
4467 *-----------------------------------------------------------*/
4468 bool navigationRequiresTurnAssistance(void)
4469 {
4470 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
4471 if (STATE(FIXED_WING_LEGACY)) {
4472 // For airplanes turn assistant is always required when controlling position
4473 return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
4474 }
4475
4476 return false;
4477 }
4478
4479 /**
4480 * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
4481 */
4482 int8_t navigationGetHeadingControlState(void)
4483 {
4484 // For airplanes report as manual heading control
4485 if (STATE(FIXED_WING_LEGACY)) {
4486 return NAV_HEADING_CONTROL_MANUAL;
4487 }
4488
4489 // For multirotors it depends on navigation system mode
4490 // Course hold requires Auto Control to update heading hold target whilst RC adjustment active
4491 if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
4492 if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
4493 return NAV_HEADING_CONTROL_MANUAL;
4494 }
4495
4496 return NAV_HEADING_CONTROL_AUTO;
4497 }
4498
4499 return NAV_HEADING_CONTROL_NONE;
4500 }
4501
4502 bool navigationTerrainFollowingEnabled(void)
4503 {
4504 return posControl.flags.isTerrainFollowEnabled;
4505 }
4506
4507 uint32_t distanceToFirstWP(void)
4508 {
4509 fpVector3_t startingWaypointPos;
4510 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.startWpIndex], GEO_ALT_RELATIVE);
4511 return calculateDistanceToDestination(&startingWaypointPos);
4512 }
4513
4514 bool navigationPositionEstimateIsHealthy(void)
4515 {
4516 return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
4517 }
4518
4519 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
4520 {
4521 const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) ||
4522 IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
4523
4524 if (usedBypass) {
4525 *usedBypass = false;
4526 }
4527
4528 // Apply extra arming safety only if pilot has any of GPS modes configured
4529 if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !navigationPositionEstimateIsHealthy()) {
4530 if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
4531 (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
4532 if (usedBypass) {
4533 *usedBypass = true;
4534 }
4535 return NAV_ARMING_BLOCKER_NONE;
4536 }
4537 return NAV_ARMING_BLOCKER_MISSING_GPS_FIX;
4538 }
4539
4540 // Don't allow arming if any of NAV modes is active
4541 if (!ARMING_FLAG(ARMED) && navBoxModesEnabled) {
4542 return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
4543 }
4544
4545 // Don't allow arming if first waypoint is farther than configured safe distance
4546 if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
4547 if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
4548 return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
4549 }
4550 }
4551
4552 /*
4553 * Don't allow arming if any of JUMP waypoint has invalid settings
4554 * First WP can't be JUMP
4555 * Can't jump to immediately adjacent WPs (pointless)
4556 * Can't jump beyond WP list
4557 * Only jump to geo-referenced WP types
4558 */
4559 if (posControl.waypointCount) {
4560 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){
4561 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
4562 if (wp == posControl.startWpIndex || posControl.waypointList[wp].p1 >= posControl.waypointCount ||
4563 (posControl.waypointList[wp].p1 > (wp - posControl.startWpIndex - 2) && posControl.waypointList[wp].p1 < (wp - posControl.startWpIndex + 2)) || posControl.waypointList[wp].p2 < -1) {
4564 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
4565 }
4566
4567 /* check for target geo-ref sanity */
4568 uint16_t target = posControl.waypointList[wp].p1 + posControl.startWpIndex;
4569 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)) {
4570 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
4571 }
4572 }
4573 }
4574 }
4575
4576 return NAV_ARMING_BLOCKER_NONE;
4577 }
4578
4579 /**
4580 * Indicate ready/not ready status
4581 */
4582 static void updateReadyStatus(void)
4583 {
4584 static bool posReadyBeepDone = false;
4585
4586 /* Beep out READY_BEEP once when position lock is firstly acquired and HOME set */
4587 if (navigationPositionEstimateIsHealthy() && !posReadyBeepDone) {
4588 beeper(BEEPER_READY_BEEP);
4589 posReadyBeepDone = true;
4590 }
4591 }
4592
4593 void updateFlightBehaviorModifiers(void)
4594 {
4595 if (posControl.flags.isGCSAssistedNavigationEnabled && !IS_RC_MODE_ACTIVE(BOXGCSNAV)) {
4596 posControl.flags.isGCSAssistedNavigationReset = true;
4597 }
4598
4599 posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
4600 }
4601
4602 /* On the fly WP mission planner mode allows WP missions to be setup during navigation.
4603 * Uses the WP mode switch to save WP at current location (WP mode disabled when active)
4604 * Mission can be flown after mission planner mode switched off and saved after disarm. */
4605
4606 void updateWpMissionPlanner(void)
4607 {
4608 static timeMs_t resetTimerStart = 0;
4609 if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
4610 const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
4611
4612 posControl.flags.wpMissionPlannerActive = true;
4613 if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
4614 posControl.waypointCount = posControl.wpPlannerActiveWPIndex = 0;
4615 posControl.waypointListValid = false;
4616 posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
4617 }
4618 if (positionTrusted && posControl.wpMissionPlannerStatus != WP_PLAN_FULL) {
4619 missionPlannerSetWaypoint();
4620 } else {
4621 posControl.wpMissionPlannerStatus = posControl.wpMissionPlannerStatus == WP_PLAN_FULL ? WP_PLAN_FULL : WP_PLAN_WAIT;
4622 }
4623 } else if (posControl.flags.wpMissionPlannerActive) {
4624 posControl.flags.wpMissionPlannerActive = false;
4625 posControl.activeWaypointIndex = 0;
4626 resetTimerStart = millis();
4627 }
4628 }
4629
4630 void missionPlannerSetWaypoint(void)
4631 {
4632 static bool boxWPModeIsReset = true;
4633
4634 boxWPModeIsReset = !boxWPModeIsReset ? !IS_RC_MODE_ACTIVE(BOXNAVWP) : boxWPModeIsReset; // only able to save new WP when WP mode reset
4635 posControl.wpMissionPlannerStatus = boxWPModeIsReset ? boxWPModeIsReset : posControl.wpMissionPlannerStatus; // hold save status until WP mode reset
4636
4637 if (!boxWPModeIsReset || !IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4638 return;
4639 }
4640
4641 if (!posControl.wpPlannerActiveWPIndex) { // reset existing mission data before adding first WP
4642 resetWaypointList();
4643 }
4644
4645 gpsLocation_t wpLLH;
4646 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
4647
4648 posControl.waypointList[posControl.wpPlannerActiveWPIndex].action = 1;
4649 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lat = wpLLH.lat;
4650 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
4651 posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
4652 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
4653 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 |= NAV_WP_ALTMODE; // use absolute altitude datum
4654 posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
4655 posControl.waypointListValid = true;
4656
4657 if (posControl.wpPlannerActiveWPIndex) {
4658 posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
4659 }
4660
4661 posControl.wpPlannerActiveWPIndex += 1;
4662 posControl.waypointCount = posControl.geoWaypointCount = posControl.wpPlannerActiveWPIndex;
4663 posControl.wpMissionPlannerStatus = posControl.waypointCount == NAV_MAX_WAYPOINTS ? WP_PLAN_FULL : WP_PLAN_OK;
4664 boxWPModeIsReset = false;
4665 }
4666
4667 /**
4668 * Process NAV mode transition and WP/RTH state machine
4669 * Update rate: RX (data driven or 50Hz)
4670 */
4671 void updateWaypointsAndNavigationMode(void)
4672 {
4673 /* Initiate home position update */
4674 updateHomePosition();
4675
4676 /* Update flight statistics */
4677 updateNavigationFlightStatistics();
4678
4679 /* Update NAV ready status */
4680 updateReadyStatus();
4681
4682 // Update flight behaviour modifiers
4683 updateFlightBehaviorModifiers();
4684
4685 // Process switch to a different navigation mode (if needed)
4686 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4687
4688 // Process pilot's RC input to adjust behaviour
4689 processNavigationRCAdjustments();
4690
4691 // Map navMode back to enabled flight modes
4692 switchNavigationFlightModes();
4693
4694 // Update WP mission planner
4695 updateWpMissionPlanner();
4696
4697 // Update RTH trackback
4698 updateRthTrackback(false);
4699
4700 //Update Blackbox data
4701 navCurrentState = (int16_t)posControl.navPersistentId;
4702 }
4703
4704 /*-----------------------------------------------------------
4705 * NAV main control functions
4706 *-----------------------------------------------------------*/
4707 void navigationUsePIDs(void)
4708 {
4709 /** Multicopter PIDs */
4710 // Brake time parameter
4711 posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f;
4712
4713 // Position controller expo (taret vel expo for MC)
4714 posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f);
4715
4716 // Initialize position hold P-controller
4717 for (int axis = 0; axis < 2; axis++) {
4718 navPidInit(
4719 &posControl.pids.pos[axis],
4720 (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
4721 0.0f,
4722 0.0f,
4723 0.0f,
4724 NAV_DTERM_CUT_HZ,
4725 0.0f
4726 );
4727
4728 navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
4729 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
4730 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
4731 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
4732 pidProfile()->navVelXyDTermLpfHz,
4733 0.0f
4734 );
4735 }
4736
4737 /*
4738 * Set coefficients used in MC VEL_XY
4739 */
4740 multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f;
4741 multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f;
4742 multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f;
4743
4744 #ifdef USE_MR_BRAKING_MODE
4745 multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;
4746 #endif
4747
4748 // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
4749 navPidInit(
4750 &posControl.pids.pos[Z],
4751 (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
4752 0.0f,
4753 0.0f,
4754 0.0f,
4755 NAV_DTERM_CUT_HZ,
4756 0.0f
4757 );
4758
4759 navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
4760 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
4761 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
4762 0.0f,
4763 NAV_VEL_Z_DERIVATIVE_CUT_HZ,
4764 NAV_VEL_Z_ERROR_CUT_HZ
4765 );
4766
4767 // Initialize surface tracking PID
4768 navPidInit(&posControl.pids.surface, 2.0f,
4769 0.0f,
4770 0.0f,
4771 0.0f,
4772 NAV_DTERM_CUT_HZ,
4773 0.0f
4774 );
4775
4776 /** Airplane PIDs */
4777 // Initialize fixed wing PID controllers
4778 navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
4779 (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
4780 (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
4781 0.0f,
4782 NAV_DTERM_CUT_HZ,
4783 0.0f
4784 );
4785
4786 navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
4787 (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
4788 (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
4789 0.0f,
4790 NAV_DTERM_CUT_HZ,
4791 0.0f
4792 );
4793
4794 navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
4795 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
4796 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
4797 0.0f,
4798 2.0f,
4799 0.0f
4800 );
4801 }
4802
4803 void navigationInit(void)
4804 {
4805 /* Initial state */
4806 posControl.navState = NAV_STATE_IDLE;
4807
4808 posControl.flags.horizontalPositionDataNew = false;
4809 posControl.flags.verticalPositionDataNew = false;
4810
4811 posControl.flags.estAltStatus = EST_NONE;
4812 posControl.flags.estPosStatus = EST_NONE;
4813 posControl.flags.estVelStatus = EST_NONE;
4814 posControl.flags.estHeadingStatus = EST_NONE;
4815 posControl.flags.estAglStatus = EST_NONE;
4816
4817 posControl.flags.forcedRTHActivated = false;
4818 posControl.flags.forcedEmergLandingActivated = false;
4819 posControl.waypointCount = 0;
4820 posControl.activeWaypointIndex = 0;
4821 posControl.waypointListValid = false;
4822 posControl.wpPlannerActiveWPIndex = 0;
4823 posControl.flags.wpMissionPlannerActive = false;
4824 posControl.startWpIndex = 0;
4825 posControl.safehomeState.isApplied = false;
4826 #ifdef USE_MULTI_MISSION
4827 posControl.multiMissionCount = 0;
4828 #endif
4829 /* Set initial surface invalid */
4830 posControl.actualState.surfaceMin = -1.0f;
4831
4832 /* Reset statistics */
4833 posControl.totalTripDistance = 0.0f;
4834
4835 /* Use system config */
4836 navigationUsePIDs();
4837
4838 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
4839 /* configure WP missions at boot */
4840 #ifdef USE_MULTI_MISSION
4841 for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { // check number missions in NVM
4842 if (checkMissionCount(i)) {
4843 break;
4844 }
4845 }
4846 /* set index to 1 if saved mission index > available missions */
4847 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
4848 navConfigMutable()->general.waypoint_multi_mission_index = 1;
4849 }
4850 #endif
4851 /* load mission on boot */
4852 if (navConfig()->general.waypoint_load_on_boot) {
4853 loadNonVolatileWaypointList(false);
4854 }
4855 #endif
4856 }
4857
4858 /*-----------------------------------------------------------
4859 * Access to estimated position/velocity data
4860 *-----------------------------------------------------------*/
4861 float getEstimatedActualVelocity(int axis)
4862 {
4863 return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
4864 }
4865
4866 float getEstimatedActualPosition(int axis)
4867 {
4868 return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
4869 }
4870
4871 /*-----------------------------------------------------------
4872 * Ability to execute RTH on external event
4873 *-----------------------------------------------------------*/
4874 void activateForcedRTH(void)
4875 {
4876 abortFixedWingLaunch();
4877 posControl.flags.forcedRTHActivated = true;
4878 #ifdef USE_SAFE_HOME
4879 checkSafeHomeState(true);
4880 #endif
4881 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4882 }
4883
4884 void abortForcedRTH(void)
4885 {
4886 // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
4887 // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
4888 posControl.flags.forcedRTHActivated = false;
4889 #ifdef USE_SAFE_HOME
4890 checkSafeHomeState(false);
4891 #endif
4892 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4893 }
4894
4895 rthState_e getStateOfForcedRTH(void)
4896 {
4897 /* If forced RTH activated and in AUTO_RTH or EMERG state */
4898 if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) {
4899 if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
4900 return RTH_HAS_LANDED;
4901 }
4902 else {
4903 return RTH_IN_PROGRESS;
4904 }
4905 }
4906 else {
4907 return RTH_IDLE;
4908 }
4909 }
4910
4911 /*-----------------------------------------------------------
4912 * Ability to execute Emergency Landing on external event
4913 *-----------------------------------------------------------*/
4914 void activateForcedEmergLanding(void)
4915 {
4916 abortFixedWingLaunch();
4917 posControl.flags.forcedEmergLandingActivated = true;
4918 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4919 }
4920
4921 void abortForcedEmergLanding(void)
4922 {
4923 // Disable emergency landing and make sure we back out of navigation mode to IDLE
4924 // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update
4925 posControl.flags.forcedEmergLandingActivated = false;
4926 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4927 }
4928
4929 emergLandState_e getStateOfForcedEmergLanding(void)
4930 {
4931 /* If forced emergency landing activated and in EMERG state */
4932 if (posControl.flags.forcedEmergLandingActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_EMERG)) {
4933 if (posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
4934 return EMERG_LAND_HAS_LANDED;
4935 } else {
4936 return EMERG_LAND_IN_PROGRESS;
4937 }
4938 } else {
4939 return EMERG_LAND_IDLE;
4940 }
4941 }
4942
4943 bool isWaypointMissionRTHActive(void)
4944 {
4945 return (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) && IS_RC_MODE_ACTIVE(BOXNAVWP) &&
4946 !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
4947 }
4948
4949 bool navigationIsExecutingAnEmergencyLanding(void)
4950 {
4951 return navGetCurrentStateFlags() & NAV_CTL_EMERG;
4952 }
4953
4954 bool navigationInAutomaticThrottleMode(void)
4955 {
4956 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4957 return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) ||
4958 ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle);
4959 }
4960
4961 bool navigationIsControllingThrottle(void)
4962 {
4963 // Note that this makes a detour into mixer code to evaluate actual motor status
4964 return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE);
4965 }
4966
4967 bool navigationIsControllingAltitude(void) {
4968 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4969 return (stateFlags & NAV_CTL_ALT);
4970 }
4971
4972 bool navigationIsFlyingAutonomousMode(void)
4973 {
4974 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4975 return (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
4976 }
4977
4978 bool navigationRTHAllowsLanding(void)
4979 {
4980 // WP mission RTH landing setting
4981 if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
4982 return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;
4983 }
4984
4985 // normal RTH landing setting
4986 navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
4987 return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
4988 (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
4989 }
4990
4991 bool isNavLaunchEnabled(void)
4992 {
4993 return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE);
4994 }
4995
4996 bool abortLaunchAllowed(void)
4997 {
4998 // allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
4999 return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
5000 }
5001
5002 int32_t navigationGetHomeHeading(void)
5003 {
5004 return posControl.rthState.homePosition.heading;
5005 }
5006
5007 // returns m/s
5008 float calculateAverageSpeed(void) {
5009 float flightTime = getFlightTime();
5010 if (flightTime == 0.0f) return 0;
5011 return (float)getTotalTravelDistance() / (flightTime * 100);
5012 }
5013
5014 const navigationPIDControllers_t* getNavigationPIDControllers(void) {
5015 return &posControl.pids;
5016 }
5017
5018 bool isAdjustingPosition(void) {
5019 return posControl.flags.isAdjustingPosition;
5020 }
5021
5022 bool isAdjustingHeading(void) {
5023 return posControl.flags.isAdjustingHeading;
5024 }
5025
5026 int32_t getCruiseHeadingAdjustment(void) {
5027 return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
5028 }
5029
5030 int32_t navigationGetHeadingError(void)
5031 {
5032 return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
5033 }
5034
5035 int8_t navCheckActiveAngleHoldAxis(void)
5036 {
5037 int8_t activeAxis = -1;
5038
5039 if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
5040 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
5041 bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
5042
5043 if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
5044 activeAxis = FD_PITCH;
5045 } else if (altholdActive) {
5046 activeAxis = FD_ROLL;
5047 }
5048 }
5049
5050 return activeAxis;
5051 }
5052
5053 uint8_t getActiveWpNumber(void)
5054 {
5055 return NAV_Status.activeWpNumber;
5056 }
5057
5058 #ifdef USE_FW_AUTOLAND
5059
5060 static void resetFwAutoland(void)
5061 {
5062 posControl.fwLandState.landAltAgl = 0;
5063 posControl.fwLandState.landAproachAltAgl = 0;
5064 posControl.fwLandState.loiterStartTime = 0;
5065 posControl.fwLandState.approachSettingIdx = 0;
5066 posControl.fwLandState.landPosHeading = -1;
5067 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
5068 posControl.fwLandState.landWp = false;
5069 }
5070
5071 static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
5072 {
5073 if (approachHeading == 0) {
5074 return -1;
5075 }
5076
5077 int32_t windRelToRunway = wrap_36000(windAngle - ABS(approachHeading));
5078 // Headwind?
5079 if (windRelToRunway >= 27000 || windRelToRunway <= 9000) {
5080 return wrap_36000(ABS(approachHeading));
5081 }
5082
5083 if (approachHeading > 0) {
5084 return wrap_36000(approachHeading + 18000);
5085 }
5086
5087 return -1;
5088 }
5089
5090 static float getLandAltitude(void)
5091 {
5092 float altitude = -1;
5093 #ifdef USE_RANGEFINDER
5094 if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
5095 altitude = rangefinderGetLatestAltitude();
5096 }
5097 else
5098 #endif
5099 if (posControl.flags.estAglStatus >= EST_USABLE) {
5100 altitude = posControl.actualState.agl.pos.z;
5101 } else {
5102 altitude = posControl.actualState.abs.pos.z;
5103 }
5104 return altitude;
5105 }
5106
5107 static int32_t calcWindDiff(int32_t heading, int32_t windHeading)
5108 {
5109 return ABS(wrap_18000(windHeading - heading));
5110 }
5111
5112 static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
5113 {
5114 calculateAndSetActiveWaypointToLocalPosition(pos);
5115
5116 if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) {
5117 int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos);
5118 posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
5119 } else {
5120 posControl.activeWaypoint.nextTurnAngle = -1;
5121 }
5122
5123 posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
5124 posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
5125 posControl.wpAltitudeReached = false;
5126 }
5127
5128 void resetFwAutolandApproach(int8_t idx)
5129 {
5130 if (idx >= 0 && idx < MAX_FW_LAND_APPOACH_SETTINGS) {
5131 memset(fwAutolandApproachConfigMutable(idx), 0, sizeof(navFwAutolandApproach_t));
5132 } else {
5133 memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
5134 }
5135 }
5136
5137 bool isFwLandInProgess(void)
5138 {
5139 return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
5140 || posControl.navState == NAV_STATE_FW_LANDING_LOITER
5141 || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
5142 || posControl.navState == NAV_STATE_FW_LANDING_GLIDE
5143 || posControl.navState == NAV_STATE_FW_LANDING_FLARE;
5144 }
5145
5146 bool canFwLandCanceld(void)
5147 {
5148 return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
5149 || posControl.navState == NAV_STATE_FW_LANDING_LOITER
5150 || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
5151 || posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
5152 }
5153
5154 #endif
INAV: Navigation-enabled flight control software