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