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