?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge remote-tracking branch 'origin/master' into mmosca-mavlinkrc
[inav.git] / src / main / navigation / navigation.c
blob4ae4cf228dbb72af99d417d705b9085fe9084ae2
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.wpInitialAltitude = posControl.actualState.abs.pos.z;
1863 posControl.wpAltitudeReached = false;
1864 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1865
1866 case NAV_WP_ACTION_JUMP:
1867 // We use p3 as the volatile jump counter (p2 is the static value)
1868 if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
1869 if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
1870 resetJumpCounter();
1871 return nextForNonGeoStates();
1872 }
1873 else
1874 {
1875 posControl.waypointList[posControl.activeWaypointIndex].p3--;
1876 }
1877 }
1878 posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + posControl.startWpIndex;
1879 return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1880
1881 case NAV_WP_ACTION_SET_POI:
1882 if (STATE(MULTIROTOR)) {
1883 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
1884 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
1885 &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
1886 }
1887 return nextForNonGeoStates();
1888
1889 case NAV_WP_ACTION_SET_HEAD:
1890 if (STATE(MULTIROTOR)) {
1891 if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
1892 posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
1893 wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1894 } else {
1895 wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
1896 wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
1897 }
1898 }
1899 return nextForNonGeoStates();
1900
1901 case NAV_WP_ACTION_RTH:
1902 posControl.wpMissionRestart = true;
1903 return NAV_FSM_EVENT_SWITCH_TO_RTH;
1904 };
1905
1906 UNREACHABLE();
1907 }
1908
1909 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
1910 {
1911 UNUSED(previousState);
1912
1913 // If no position sensor available - land immediately
1914 if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
1915 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1916 case NAV_WP_ACTION_HOLD_TIME:
1917 case NAV_WP_ACTION_WAYPOINT:
1918 case NAV_WP_ACTION_LAND:
1919 if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
1920 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
1921 }
1922 else {
1923 fpVector3_t tmpWaypoint;
1924 tmpWaypoint.x = posControl.activeWaypoint.pos.x;
1925 tmpWaypoint.y = posControl.activeWaypoint.pos.y;
1926 /* Use linear climb/descent between WPs arriving at WP altitude when within 10% of total distance to WP */
1927 tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance),
1928 posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance,
1929 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
1930
1931 setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1932
1933 if(STATE(MULTIROTOR)) {
1934 switch (wpHeadingControl.mode) {
1935 case NAV_WP_HEAD_MODE_NONE:
1936 break;
1937 case NAV_WP_HEAD_MODE_FIXED:
1938 setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
1939 break;
1940 case NAV_WP_HEAD_MODE_POI:
1941 setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
1942 break;
1943 }
1944 }
1945 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1946 }
1947 break;
1948
1949 case NAV_WP_ACTION_JUMP:
1950 case NAV_WP_ACTION_SET_HEAD:
1951 case NAV_WP_ACTION_SET_POI:
1952 case NAV_WP_ACTION_RTH:
1953 UNREACHABLE();
1954 }
1955 }
1956 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1957 else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
1958 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1959 }
1960
1961 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1962 }
1963
1964 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
1965 {
1966 UNUSED(previousState);
1967
1968 if (navConfig()->general.waypoint_enforce_altitude) {
1969 posControl.wpAltitudeReached = isWaypointAltitudeReached();
1970 }
1971
1972 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1973 case NAV_WP_ACTION_WAYPOINT:
1974 if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
1975 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1976 } else {
1977 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
1978 }
1979
1980 case NAV_WP_ACTION_JUMP:
1981 case NAV_WP_ACTION_SET_HEAD:
1982 case NAV_WP_ACTION_SET_POI:
1983 case NAV_WP_ACTION_RTH:
1984 UNREACHABLE();
1985
1986 case NAV_WP_ACTION_LAND:
1987 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
1988
1989 case NAV_WP_ACTION_HOLD_TIME:
1990 // Save the current time for the time the waypoint was reached
1991 posControl.wpReachedTime = millis();
1992 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1993 }
1994
1995 UNREACHABLE();
1996 }
1997
1998 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
1999 {
2000 UNUSED(previousState);
2001
2002 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
2003 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
2004 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2005 }
2006
2007 if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
2008 // Adjust altitude to waypoint setting
2009 setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
2010
2011 posControl.wpAltitudeReached = isWaypointAltitudeReached();
2012
2013 if (posControl.wpAltitudeReached) {
2014 posControl.wpReachedTime = millis();
2015 } else {
2016 return NAV_FSM_EVENT_NONE;
2017 }
2018 }
2019
2020 timeMs_t currentTime = millis();
2021
2022 if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
2023 posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
2024 (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
2025 return NAV_FSM_EVENT_SUCCESS;
2026 }
2027
2028 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
2029 }
2030
2031 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
2032 {
2033 #ifdef USE_FW_AUTOLAND
2034 if (posControl.fwLandState.landAborted) {
2035 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
2036 }
2037 #endif
2038
2039 const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
2040
2041 if (landEvent == NAV_FSM_EVENT_SUCCESS) {
2042 // Landing controller returned success - invoke RTH finish states and finish the waypoint
2043 navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
2044 navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState);
2045 }
2046
2047 return landEvent;
2048 }
2049
2050 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
2051 {
2052 UNUSED(previousState);
2053
2054 if (isLastMissionWaypoint()) { // Last waypoint reached
2055 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
2056 }
2057 else {
2058 // Waypoint reached, do something and move on to next waypoint
2059 posControl.activeWaypointIndex++;
2060 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
2061 }
2062 }
2063
2064 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState)
2065 {
2066 UNUSED(previousState);
2067
2068 clearJumpCounters();
2069 posControl.wpMissionRestart = true;
2070
2071 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
2072 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
2073 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2074 }
2075
2076 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
2077 }
2078
2079 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
2080 {
2081 UNUSED(previousState);
2082
2083 #ifdef USE_FW_AUTOLAND
2084 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2085 #endif
2086
2087 if ((posControl.flags.estPosStatus >= EST_USABLE)) {
2088 resetPositionController();
2089 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
2090 }
2091
2092 // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
2093 // Make sure terrain following is not enabled
2094 resetAltitudeController(false);
2095
2096 return NAV_FSM_EVENT_SUCCESS;
2097 }
2098
2099 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
2100 {
2101 UNUSED(previousState);
2102
2103 // Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
2104 if (posControl.flags.estPosStatus >= EST_USABLE) {
2105 float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
2106 if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
2107 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
2108 }
2109 }
2110
2111 if (STATE(LANDING_DETECTED)) {
2112 return NAV_FSM_EVENT_SUCCESS;
2113 }
2114
2115 return NAV_FSM_EVENT_NONE;
2116 }
2117
2118 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
2119 {
2120 UNUSED(previousState);
2121
2122 return NAV_FSM_EVENT_NONE;
2123 }
2124
2125 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
2126 {
2127 const timeUs_t currentTimeUs = micros();
2128 UNUSED(previousState);
2129
2130 resetFixedWingLaunchController(currentTimeUs);
2131
2132 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT
2133 }
2134
2135 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState)
2136 {
2137 const timeUs_t currentTimeUs = micros();
2138 UNUSED(previousState);
2139
2140 // Continue immediately to launch in progress if manual launch throttle used
2141 if (navConfig()->fw.launch_manual_throttle) {
2142 return NAV_FSM_EVENT_SUCCESS;
2143 }
2144
2145 if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
2146 enableFixedWingLaunchController(currentTimeUs);
2147 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
2148 }
2149
2150 // abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle
2151 if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2152 abortFixedWingLaunch();
2153 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2154 }
2155
2156 return NAV_FSM_EVENT_NONE;
2157 }
2158
2159 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState)
2160 {
2161 UNUSED(previousState);
2162
2163 if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) {
2164 return NAV_FSM_EVENT_SUCCESS;
2165 }
2166
2167 return NAV_FSM_EVENT_NONE;
2168 }
2169
2170 navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE;
2171 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState)
2172 {
2173 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
2174
2175 // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
2176 if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP)) {
2177 resetAltitudeController(false);
2178 setupAltitudeController();
2179 }
2180 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
2181 navMixerATPendingState = previousState;
2182 return NAV_FSM_EVENT_SUCCESS;
2183 }
2184
2185 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState)
2186 {
2187 UNUSED(previousState);
2188 mixerProfileATRequest_e required_action;
2189 switch (navMixerATPendingState)
2190 {
2191 case NAV_STATE_RTH_HEAD_HOME:
2192 required_action = MIXERAT_REQUEST_RTH;
2193 break;
2194 case NAV_STATE_RTH_LANDING:
2195 required_action = MIXERAT_REQUEST_LAND;
2196 break;
2197 default:
2198 required_action = MIXERAT_REQUEST_NONE;
2199 break;
2200 }
2201 if (mixerATUpdateState(required_action)){
2202 // MixerAT is done, switch to next state
2203 resetPositionController();
2204 resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL
2205 mixerATUpdateState(MIXERAT_REQUEST_ABORT);
2206 switch (navMixerATPendingState)
2207 {
2208 case NAV_STATE_RTH_HEAD_HOME:
2209 setupAltitudeController();
2210 return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME;
2211 break;
2212 case NAV_STATE_RTH_LANDING:
2213 setupAltitudeController();
2214 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;
2215 break;
2216 default:
2217 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2218 break;
2219 }
2220 }
2221
2222 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
2223
2224 return NAV_FSM_EVENT_NONE;
2225 }
2226
2227 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState)
2228 {
2229 UNUSED(previousState);
2230 mixerATUpdateState(MIXERAT_REQUEST_ABORT);
2231 return NAV_FSM_EVENT_SUCCESS;
2232 }
2233
2234 #ifdef USE_FW_AUTOLAND
2235 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState)
2236 {
2237 UNUSED(previousState);
2238
2239 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2240 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2241 }
2242
2243 if (posControl.fwLandState.loiterStartTime == 0) {
2244 posControl.fwLandState.loiterStartTime = micros();
2245 }
2246
2247 if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
2248 updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
2249 posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
2250 return NAV_FSM_EVENT_SUCCESS;
2251 }
2252
2253 fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos;
2254 tmpHomePos.z = posControl.fwLandState.landAproachAltAgl;
2255 setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z);
2256
2257 return NAV_FSM_EVENT_NONE;
2258 }
2259
2260 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState)
2261 {
2262 UNUSED(previousState);
2263 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
2264 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
2265 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2266 }
2267
2268 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2269 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2270 }
2271
2272 if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) {
2273 if (isEstimatedWindSpeedValid()) {
2274
2275 uint16_t windAngle = 0;
2276 int32_t approachHeading = -1;
2277 float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle);
2278 windAngle = wrap_36000(windAngle + 18000);
2279
2280 // Ignore low wind speed, could be the error of the wind estimator
2281 if (windSpeed < navFwAutolandConfig()->maxTailwind) {
2282 if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1 != 0) {
2283 approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1));
2284 } else if ((fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2 != 0) ) {
2285 approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2));
2286 }
2287 } else {
2288 int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1), windAngle);
2289 int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2), windAngle);
2290
2291 if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
2292 heading2 = -1;
2293 }
2294
2295 if (heading1 == -1 && heading2 >= 0) {
2296 posControl.fwLandState.landingDirection = heading2;
2297 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
2298 } else if (heading1 >= 0 && heading2 == -1) {
2299 posControl.fwLandState.landingDirection = heading1;
2300 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
2301 } else {
2302 if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
2303 posControl.fwLandState.landingDirection = heading1;
2304 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
2305 } else {
2306 posControl.fwLandState.landingDirection = heading2;
2307 approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
2308 }
2309 }
2310 }
2311
2312 if (posControl.fwLandState.landingDirection >= 0) {
2313 fpVector3_t tmpPos;
2314
2315 int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;
2316 int32_t dir = 0;
2317 if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
2318 dir = wrap_36000(ABS(approachHeading) - 9000);
2319 } else {
2320 dir = wrap_36000(ABS(approachHeading) + 9000);
2321 }
2322
2323 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, posControl.fwLandState.landingDirection, navFwAutolandConfig()->approachLength);
2324 tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
2325 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND] = tmpPos;
2326
2327 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, wrap_36000(posControl.fwLandState.landingDirection + 18000), navFwAutolandConfig()->approachLength);
2328 tmpPos.z = finalApproachAlt;
2329 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
2330
2331 calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, MAX((uint32_t)navConfig()->fw.loiter_radius * 4, navFwAutolandConfig()->approachLength / 2));
2332 tmpPos.z = posControl.fwLandState.landAproachAltAgl;
2333 posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
2334
2335 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH]);
2336 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_TURN;
2337 posControl.fwLandState.landState = FW_AUTOLAND_STATE_DOWNWIND;
2338
2339 return NAV_FSM_EVENT_SUCCESS;
2340 } else {
2341 posControl.fwLandState.loiterStartTime = micros();
2342 }
2343 } else {
2344 posControl.fwLandState.loiterStartTime = micros();
2345 }
2346 }
2347
2348 fpVector3_t tmpPoint = posControl.fwLandState.landPos;
2349 tmpPoint.z = posControl.fwLandState.landAproachAltAgl;
2350 setDesiredPosition(&tmpPoint, posControl.fwLandState.landPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
2351
2352 return NAV_FSM_EVENT_NONE;
2353 }
2354 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState)
2355 {
2356 UNUSED(previousState);
2357
2358 if (STATE(LANDING_DETECTED)) {
2359 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
2360 }
2361
2362 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
2363 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
2364 }
2365
2366 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2367 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2368 }
2369
2370 if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
2371 resetPositionController();
2372 posControl.cruise.course = posControl.fwLandState.landingDirection;
2373 posControl.cruise.previousCourse = posControl.cruise.course;
2374 posControl.cruise.lastCourseAdjustmentTime = 0;
2375 posControl.fwLandState.landState = FW_AUTOLAND_STATE_GLIDE;
2376 return NAV_FSM_EVENT_SUCCESS;
2377 } else if (isWaypointReached(&posControl.fwLandState.landWaypoints[posControl.fwLandState.landCurrentWp], &posControl.activeWaypoint.bearing)) {
2378 if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_TURN) {
2379 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND]);
2380 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH;
2381 posControl.fwLandState.landState = FW_AUTOLAND_STATE_BASE_LEG;
2382 return NAV_FSM_EVENT_NONE;
2383 } else if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) {
2384 setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND], NULL);
2385 posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_LAND;
2386 posControl.fwLandState.landState = FW_AUTOLAND_STATE_FINAL_APPROACH;
2387 return NAV_FSM_EVENT_NONE;
2388 }
2389 }
2390
2391 fpVector3_t tmpWaypoint;
2392 tmpWaypoint.x = posControl.activeWaypoint.pos.x;
2393 tmpWaypoint.y = posControl.activeWaypoint.pos.y;
2394 tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
2395 posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
2396 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
2397 setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
2398
2399 return NAV_FSM_EVENT_NONE;
2400 }
2401
2402 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState)
2403 {
2404 UNUSED(previousState);
2405
2406 if (STATE(LANDING_DETECTED)) {
2407 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
2408 }
2409
2410 if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
2411 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
2412 }
2413
2414 if (getHwRangefinderStatus() == HW_SENSOR_OK && getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
2415 posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
2416 return NAV_FSM_EVENT_SUCCESS;
2417 }
2418
2419 setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
2420 return NAV_FSM_EVENT_NONE;
2421 }
2422
2423 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState)
2424 {
2425 UNUSED(previousState);
2426
2427 if (STATE(LANDING_DETECTED)) {
2428 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
2429 }
2430
2431 setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
2432
2433 return NAV_FSM_EVENT_NONE;
2434 }
2435
2436 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState)
2437 {
2438 UNUSED(previousState);
2439
2440 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2441
2442 return NAV_FSM_EVENT_NONE;
2443 }
2444
2445 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
2446 {
2447 UNUSED(previousState);
2448 posControl.fwLandState.landAborted = true;
2449 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
2450
2451 return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
2452 }
2453 #endif
2454
2455 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
2456 {
2457 navigationFSMState_t previousState;
2458
2459 previousState = posControl.navState;
2460 if (posControl.navState != newState) {
2461 posControl.navState = newState;
2462 posControl.navPersistentId = navFSM[newState].persistentId;
2463 }
2464 return previousState;
2465 }
2466
2467 static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
2468 {
2469 const timeMs_t currentMillis = millis();
2470 navigationFSMState_t previousState = NAV_STATE_UNDEFINED;
2471 static timeMs_t lastStateProcessTime = 0;
2472
2473 /* Process new injected event if event defined,
2474 * otherwise process timeout event if defined */
2475 if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
2476 /* Update state */
2477 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
2478 } else if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
2479 ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
2480 /* Update state */
2481 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
2482 }
2483
2484 if (previousState) { /* If state updated call new state's entry function */
2485 while (navFSM[posControl.navState].onEntry) {
2486 navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
2487
2488 if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
2489 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
2490 }
2491 else {
2492 break;
2493 }
2494 }
2495
2496 lastStateProcessTime = currentMillis;
2497 }
2498
2499 /* Update public system state information */
2500 NAV_Status.mode = MW_GPS_MODE_NONE;
2501
2502 if (ARMING_FLAG(ARMED)) {
2503 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
2504
2505 if (navStateFlags & NAV_AUTO_RTH) {
2506 NAV_Status.mode = MW_GPS_MODE_RTH;
2507 }
2508 else if (navStateFlags & NAV_AUTO_WP) {
2509 NAV_Status.mode = MW_GPS_MODE_NAV;
2510 }
2511 else if (navStateFlags & NAV_CTL_EMERG) {
2512 NAV_Status.mode = MW_GPS_MODE_EMERG;
2513 }
2514 else if (navStateFlags & NAV_CTL_POS) {
2515 NAV_Status.mode = MW_GPS_MODE_HOLD;
2516 }
2517 }
2518
2519 NAV_Status.state = navFSM[posControl.navState].mwState;
2520 NAV_Status.error = navFSM[posControl.navState].mwError;
2521
2522 NAV_Status.flags = 0;
2523 if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
2524 if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
2525
2526 NAV_Status.activeWpIndex = posControl.activeWaypointIndex - posControl.startWpIndex;
2527 NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1;
2528
2529 NAV_Status.activeWpAction = 0;
2530 if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
2531 NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
2532 }
2533 }
2534
2535 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
2536 {
2537 posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos;
2538
2539 switch (mode) {
2540 case RTH_HOME_ENROUTE_INITIAL:
2541 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
2542 break;
2543
2544 case RTH_HOME_ENROUTE_PROPORTIONAL:
2545 {
2546 float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
2547 if (rthTotalDistanceToTravel >= 100) {
2548 float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
2549 posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
2550 }
2551 else {
2552 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2553 }
2554 }
2555 break;
2556
2557 case RTH_HOME_ENROUTE_FINAL:
2558 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2559 break;
2560
2561 case RTH_HOME_FINAL_LOITER:
2562 if (navConfig()->general.rth_home_altitude) {
2563 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
2564 }
2565 else {
2566 // If home altitude not defined - fall back to final ENROUTE altitude
2567 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
2568 }
2569 break;
2570
2571 case RTH_HOME_FINAL_LAND:
2572 // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
2573 if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
2574 posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm
2575 if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
2576 posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used
2577 }
2578 }
2579 break;
2580 }
2581
2582 return &posControl.rthState.homeTmpWaypoint;
2583 }
2584
2585 /*-----------------------------------------------------------
2586 * Detects if thrust vector is facing downwards
2587 *-----------------------------------------------------------*/
2588 bool isThrustFacingDownwards(void)
2589 {
2590 // Tilt angle <= 80 deg; cos(80) = 0.17364817766693034885171662676931
2591 return (calculateCosTiltAngle() >= 0.173648178f);
2592 }
2593
2594 /*-----------------------------------------------------------
2595 * Checks if position sensor (GPS) is failing for a specified timeout (if enabled)
2596 *-----------------------------------------------------------*/
2597 bool checkForPositionSensorTimeout(void)
2598 {
2599 if (navConfig()->general.pos_failure_timeout) {
2600 if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
2601 return true;
2602 }
2603 else {
2604 return false;
2605 }
2606 }
2607 else {
2608 // Timeout not defined, never fail
2609 return false;
2610 }
2611 }
2612
2613 /*-----------------------------------------------------------
2614 * Processes an update to XY-position and velocity
2615 *-----------------------------------------------------------*/
2616 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
2617 {
2618 posControl.actualState.abs.pos.x = newX;
2619 posControl.actualState.abs.pos.y = newY;
2620 posControl.actualState.abs.vel.x = newVelX;
2621 posControl.actualState.abs.vel.y = newVelY;
2622
2623 posControl.actualState.agl.pos.x = newX;
2624 posControl.actualState.agl.pos.y = newY;
2625 posControl.actualState.agl.vel.x = newVelX;
2626 posControl.actualState.agl.vel.y = newVelY;
2627
2628 posControl.actualState.velXY = calc_length_pythagorean_2D(newVelX, newVelY);
2629
2630 // CASE 1: POS & VEL valid
2631 if (estPosValid && estVelValid) {
2632 posControl.flags.estPosStatus = EST_TRUSTED;
2633 posControl.flags.estVelStatus = EST_TRUSTED;
2634 posControl.flags.horizontalPositionDataNew = true;
2635 posControl.lastValidPositionTimeMs = millis();
2636 }
2637 // CASE 1: POS invalid, VEL valid
2638 else if (!estPosValid && estVelValid) {
2639 posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
2640 posControl.flags.estVelStatus = EST_TRUSTED;
2641 posControl.flags.horizontalPositionDataNew = true;
2642 posControl.lastValidPositionTimeMs = millis();
2643 }
2644 // CASE 3: can't use pos/vel data
2645 else {
2646 posControl.flags.estPosStatus = EST_NONE;
2647 posControl.flags.estVelStatus = EST_NONE;
2648 posControl.flags.horizontalPositionDataNew = false;
2649 }
2650
2651 //Update blackbox data
2652 navLatestActualPosition[X] = newX;
2653 navLatestActualPosition[Y] = newY;
2654 navActualVelocity[X] = constrain(newVelX, -32678, 32767);
2655 navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
2656 }
2657
2658 /*-----------------------------------------------------------
2659 * Processes an update to Z-position and velocity
2660 *-----------------------------------------------------------*/
2661 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
2662 {
2663 posControl.actualState.abs.pos.z = newAltitude;
2664 posControl.actualState.abs.vel.z = newVelocity;
2665
2666 posControl.actualState.agl.pos.z = surfaceDistance;
2667 posControl.actualState.agl.vel.z = surfaceVelocity;
2668
2669 // Update altitude that would be used when executing RTH
2670 if (estimateValid) {
2671 updateDesiredRTHAltitude();
2672
2673 // If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
2674 if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
2675 // If we are in terrain-following modes - signal that we should update the surface tracking setpoint
2676 // NONE/USABLE means that we were flying blind, now we should lock to surface
2677 //updateSurfaceTrackingSetpoint();
2678 }
2679
2680 posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
2681 posControl.flags.estAltStatus = EST_TRUSTED;
2682 posControl.flags.verticalPositionDataNew = true;
2683 posControl.lastValidAltitudeTimeMs = millis();
2684 /* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
2685 posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
2686 }
2687 else {
2688 posControl.flags.estAltStatus = EST_NONE;
2689 posControl.flags.estAglStatus = EST_NONE;
2690 posControl.flags.verticalPositionDataNew = false;
2691 posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
2692 }
2693
2694 if (ARMING_FLAG(ARMED)) {
2695 if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
2696 if (posControl.actualState.surfaceMin > 0) {
2697 posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
2698 }
2699 else {
2700 posControl.actualState.surfaceMin = surfaceDistance;
2701 }
2702 }
2703 }
2704 else {
2705 posControl.actualState.surfaceMin = -1;
2706 }
2707
2708 //Update blackbox data
2709 navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
2710 navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
2711 }
2712
2713 /*-----------------------------------------------------------
2714 * Processes an update to estimated heading
2715 *-----------------------------------------------------------*/
2716 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
2717 {
2718 /* Update heading. Check if we're acquiring a valid heading for the
2719 * first time and update home heading accordingly.
2720 */
2721
2722 navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
2723
2724 #ifdef USE_DEV_TOOLS
2725 if (systemConfig()->groundTestMode && STATE(AIRPLANE)) {
2726 newEstHeading = EST_TRUSTED;
2727 }
2728 #endif
2729 if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
2730 (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
2731 (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
2732
2733 // Home was stored using the fake heading (assuming boot as 0deg). Calculate
2734 // the offset from the fake to the actual yaw and apply the same rotation
2735 // to the home point.
2736 int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
2737 posControl.rthState.homePosition.heading += fakeToRealYawOffset;
2738 posControl.rthState.homePosition.heading = wrap_36000(posControl.rthState.homePosition.heading);
2739
2740 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2741 }
2742
2743 posControl.actualState.yaw = newHeading;
2744 posControl.actualState.cog = newGroundCourse;
2745 posControl.flags.estHeadingStatus = newEstHeading;
2746
2747 /* Precompute sin/cos of yaw angle */
2748 posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2749 posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2750 }
2751
2752 /*-----------------------------------------------------------
2753 * Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
2754 *-----------------------------------------------------------*/
2755 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
2756 {
2757 return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
2758 }
2759
2760 /*-----------------------------------------------------------
2761 * Calculates distance and bearing to destination point
2762 *-----------------------------------------------------------*/
2763 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
2764 {
2765 return calc_length_pythagorean_2D(deltaX, deltaY);
2766 }
2767
2768 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
2769 {
2770 return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
2771 }
2772
2773 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
2774 {
2775 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2776 const float deltaX = destinationPos->x - posvel->pos.x;
2777 const float deltaY = destinationPos->y - posvel->pos.y;
2778
2779 return calculateDistanceFromDelta(deltaX, deltaY);
2780 }
2781
2782 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
2783 {
2784 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2785 const float deltaX = destinationPos->x - posvel->pos.x;
2786 const float deltaY = destinationPos->y - posvel->pos.y;
2787
2788 return calculateBearingFromDelta(deltaX, deltaY);
2789 }
2790
2791 int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos)
2792 {
2793 const float deltaX = endPos->x - startPos->x;
2794 const float deltaY = endPos->y - startPos->y;
2795
2796 return calculateBearingFromDelta(deltaX, deltaY);
2797 }
2798
2799 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) // NOT USED ANYWHERE
2800 {
2801 if (posControl.flags.estPosStatus == EST_NONE ||
2802 posControl.flags.estHeadingStatus == EST_NONE) {
2803
2804 return false;
2805 }
2806
2807 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2808 const float deltaX = destinationPos->x - posvel->pos.x;
2809 const float deltaY = destinationPos->y - posvel->pos.y;
2810
2811 result->distance = calculateDistanceFromDelta(deltaX, deltaY);
2812 result->bearing = calculateBearingFromDelta(deltaX, deltaY);
2813 return true;
2814 }
2815
2816 static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
2817 {
2818 // Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
2819 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
2820 navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
2821
2822 if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
2823 uint8_t nextWpIndex = posControl.activeWaypointIndex + 1;
2824 if (nextWpAction == NAV_WP_ACTION_JUMP) {
2825 if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
2826 posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
2827 nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + posControl.startWpIndex;
2828 } else if (posControl.activeWaypointIndex + 2 <= posControl.startWpIndex + posControl.waypointCount - 1) {
2829 if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
2830 nextWpIndex++;
2831 } else {
2832 return false; // give up - too complicated
2833 }
2834 }
2835 }
2836 mapWaypointToLocalPosition(nextWpPos, &posControl.waypointList[nextWpIndex], 0);
2837 return true;
2838 }
2839 }
2840
2841 return false; // no position available
2842 }
2843
2844 /*-----------------------------------------------------------
2845 * Check if waypoint is/was reached.
2846 * 'waypointBearing' stores initial bearing to waypoint.
2847 *-----------------------------------------------------------*/
2848 bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
2849 {
2850 posControl.wpDistance = calculateDistanceToDestination(waypointPos);
2851
2852 // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing.
2853 // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active
2854 uint16_t relativeBearingTargetAngle = 10000;
2855
2856 if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) {
2857 // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated
2858 if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
2859 posControl.flags.wpTurnSmoothingActive = false;
2860 return true;
2861 }
2862 relativeBearingTargetAngle = 6000;
2863 }
2864
2865
2866 if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) {
2867 return true;
2868 }
2869
2870 return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
2871 }
2872
2873 bool isWaypointAltitudeReached(void)
2874 {
2875 return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < navConfig()->general.waypoint_enforce_altitude;
2876 }
2877
2878 static void updateHomePositionCompatibility(void)
2879 {
2880 geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
2881 GPS_distanceToHome = posControl.homeDistance * 0.01f;
2882 GPS_directionToHome = posControl.homeDirection * 0.01f;
2883 }
2884
2885 // Backdoor for RTH estimator
2886 float getFinalRTHAltitude(void)
2887 {
2888 return posControl.rthState.rthFinalAltitude;
2889 }
2890
2891 /*-----------------------------------------------------------
2892 * Update the RTH Altitudes
2893 *-----------------------------------------------------------*/
2894 static void updateDesiredRTHAltitude(void)
2895 {
2896 if (ARMING_FLAG(ARMED)) {
2897 if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
2898 || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
2899 switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
2900 case NAV_RTH_CLIMB_STAGE_AT_LEAST:
2901 posControl.rthState.rthClimbStageAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2902 break;
2903 case NAV_RTH_CLIMB_STAGE_EXTRA:
2904 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2905 break;
2906 }
2907
2908 switch (navConfig()->general.flags.rth_alt_control_mode) {
2909 case NAV_RTH_NO_ALT:
2910 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2911 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2912 break;
2913
2914 case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
2915 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
2916 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2917 break;
2918
2919 case NAV_RTH_MAX_ALT:
2920 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z);
2921 if (navConfig()->general.rth_altitude > 0) {
2922 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude);
2923 }
2924 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2925 break;
2926
2927 case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
2928 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2929 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2930 break;
2931
2932 case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
2933 default:
2934 posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2935 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2936 }
2937
2938 if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) {
2939 posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
2940 }
2941 }
2942 } else {
2943 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
2944 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2945 posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
2946 }
2947 }
2948
2949 /*-----------------------------------------------------------
2950 * RTH sanity test logic
2951 *-----------------------------------------------------------*/
2952 void initializeRTHSanityChecker(void)
2953 {
2954 const timeMs_t currentTimeMs = millis();
2955
2956 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2957 posControl.rthSanityChecker.rthSanityOK = true;
2958 posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2959 }
2960
2961 bool validateRTHSanityChecker(void)
2962 {
2963 const timeMs_t currentTimeMs = millis();
2964
2965 // Ability to disable sanity checker
2966 if (navConfig()->general.rth_abort_threshold == 0) {
2967 return true;
2968 }
2969
2970 #ifdef USE_GPS_FIX_ESTIMATION
2971 if (STATE(GPS_ESTIMATED_FIX)) {
2972 //disable sanity checks in GPS estimation mode
2973 //when estimated GPS fix is replaced with real fix, coordinates may jump
2974 posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
2975 //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
2976 posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
2977 return true;
2978 }
2979 #endif
2980
2981 // Check at 10Hz rate
2982 if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
2983 const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2984 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2985
2986 if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) {
2987 posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
2988 } else {
2989 // If while doing RTH we got even farther away from home - RTH is doing something crazy
2990 posControl.rthSanityChecker.rthSanityOK = (currentDistanceToHome - posControl.rthSanityChecker.minimalDistanceToHome) < navConfig()->general.rth_abort_threshold;
2991 }
2992 }
2993
2994 return posControl.rthSanityChecker.rthSanityOK;
2995 }
2996
2997 /*-----------------------------------------------------------
2998 * Reset home position to current position
2999 *-----------------------------------------------------------*/
3000 void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
3001 {
3002 // XY-position
3003 if ((useMask & NAV_POS_UPDATE_XY) != 0) {
3004 posControl.rthState.homePosition.pos.x = pos->x;
3005 posControl.rthState.homePosition.pos.y = pos->y;
3006 if (homeFlags & NAV_HOME_VALID_XY) {
3007 posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
3008 } else {
3009 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
3010 }
3011 }
3012
3013 // Z-position
3014 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
3015 posControl.rthState.homePosition.pos.z = pos->z;
3016 if (homeFlags & NAV_HOME_VALID_Z) {
3017 posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
3018 } else {
3019 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
3020 }
3021 }
3022
3023 // Heading
3024 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
3025 // Heading
3026 posControl.rthState.homePosition.heading = heading;
3027 if (homeFlags & NAV_HOME_VALID_HEADING) {
3028 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
3029 } else {
3030 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING;
3031 }
3032 }
3033
3034 posControl.homeDistance = 0;
3035 posControl.homeDirection = 0;
3036
3037 // Update target RTH altitude as a waypoint above home
3038 updateDesiredRTHAltitude();
3039
3040 // Reset RTH sanity checker for new home position if RTH active
3041 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) ) {
3042 initializeRTHSanityChecker();
3043 }
3044
3045 updateHomePositionCompatibility();
3046 ENABLE_STATE(GPS_FIX_HOME);
3047 }
3048
3049 static navigationHomeFlags_t navigationActualStateHomeValidity(void)
3050 {
3051 navigationHomeFlags_t flags = 0;
3052
3053 if (posControl.flags.estPosStatus >= EST_USABLE) {
3054 flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
3055 }
3056
3057 if (posControl.flags.estHeadingStatus >= EST_USABLE) {
3058 flags |= NAV_HOME_VALID_HEADING;
3059 }
3060
3061 return flags;
3062 }
3063
3064 #if defined(USE_SAFE_HOME)
3065 void checkSafeHomeState(bool shouldBeEnabled)
3066 {
3067 bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
3068 (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
3069 #ifdef USE_MULTI_FUNCTIONS
3070 safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
3071 #endif
3072
3073 if (safehomeNotApplicable) {
3074 shouldBeEnabled = false;
3075 } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
3076 // if safehomes are only used with failsafe and we're trying to enable safehome
3077 // then enable the safehome only with failsafe
3078 shouldBeEnabled = posControl.flags.forcedRTHActivated;
3079 }
3080 // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
3081 if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
3082 return;
3083 }
3084 if (shouldBeEnabled) {
3085 // set home to safehome
3086 setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
3087 posControl.safehomeState.isApplied = true;
3088 } else {
3089 // set home to original arming point
3090 setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
3091 posControl.safehomeState.isApplied = false;
3092 }
3093 // if we've changed the home position, update the distance and direction
3094 updateHomePosition();
3095 }
3096
3097 /***********************************************************
3098 * See if there are any safehomes near where we are arming.
3099 * If so, save the nearest one in case we need it later for RTH.
3100 **********************************************************/
3101 bool findNearestSafeHome(void)
3102 {
3103 posControl.safehomeState.index = -1;
3104 uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
3105 uint32_t distance_to_current;
3106 fpVector3_t currentSafeHome;
3107 gpsLocation_t shLLH;
3108 shLLH.alt = 0;
3109 for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
3110 if (!safeHomeConfig(i)->enabled)
3111 continue;
3112
3113 shLLH.lat = safeHomeConfig(i)->lat;
3114 shLLH.lon = safeHomeConfig(i)->lon;
3115 geoConvertGeodeticToLocal(&currentSafeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
3116 distance_to_current = calculateDistanceToDestination(&currentSafeHome);
3117 if (distance_to_current < nearest_safehome_distance) {
3118 // this safehome is the nearest so far - keep track of it.
3119 posControl.safehomeState.index = i;
3120 nearest_safehome_distance = distance_to_current;
3121 posControl.safehomeState.nearestSafeHome = currentSafeHome;
3122 }
3123 }
3124 if (posControl.safehomeState.index >= 0) {
3125 posControl.safehomeState.distance = nearest_safehome_distance;
3126 } else {
3127 posControl.safehomeState.distance = 0;
3128 }
3129 return posControl.safehomeState.distance > 0;
3130 }
3131 #endif
3132
3133 /*-----------------------------------------------------------
3134 * Update home position, calculate distance and bearing to home
3135 *-----------------------------------------------------------*/
3136 void updateHomePosition(void)
3137 {
3138 // Disarmed and have a valid position, constantly update home before first arm (depending on setting)
3139 // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm)
3140 static bool setHome = false;
3141 navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING;
3142
3143 if (!ARMING_FLAG(ARMED)) {
3144 if (posControl.flags.estPosStatus >= EST_USABLE) {
3145 const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
3146 setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
3147 switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
3148 case NAV_RESET_NEVER:
3149 break;
3150 case NAV_RESET_ON_FIRST_ARM:
3151 setHome |= !ARMING_FLAG(WAS_EVER_ARMED);
3152 break;
3153 case NAV_RESET_ON_EACH_ARM:
3154 setHome = true;
3155 break;
3156 }
3157 }
3158 }
3159 else {
3160 static bool isHomeResetAllowed = false;
3161 // If pilot so desires he may reset home position to current position
3162 if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
3163 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)) {
3164 homeUpdateFlags = 0;
3165 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);
3166 setHome = true;
3167 isHomeResetAllowed = false;
3168 }
3169 }
3170 else {
3171 isHomeResetAllowed = true;
3172 }
3173
3174 // Update distance and direction to home if armed (home is not updated when armed)
3175 if (STATE(GPS_FIX_HOME)) {
3176 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
3177 posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
3178 posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
3179 updateHomePositionCompatibility();
3180 }
3181
3182 setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm
3183 }
3184
3185 if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) {
3186 #if defined(USE_SAFE_HOME)
3187 findNearestSafeHome();
3188 #endif
3189 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
3190
3191 if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) {
3192 posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm
3193 }
3194 // save the current location in case it is replaced by a safehome or HOME_RESET
3195 posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
3196 setHome = false;
3197 }
3198 }
3199
3200 /* -----------------------------------------------------------
3201 * Override RTH preset altitude and Climb First option
3202 * using Pitch/Roll stick held for > 1 seconds
3203 * Climb First override limited to Fixed Wing only
3204 * Roll also cancels RTH trackback on Fixed Wing and Multirotor
3205 *-----------------------------------------------------------*/
3206 bool rthAltControlStickOverrideCheck(uint8_t axis)
3207 {
3208 if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated ||
3209 (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
3210 return false;
3211 }
3212
3213 static timeMs_t rthOverrideStickHoldStartTime[2];
3214
3215 if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
3216 timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis];
3217
3218 if (!rthOverrideStickHoldStartTime[axis]) {
3219 rthOverrideStickHoldStartTime[axis] = millis();
3220 } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec
3221 if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude
3222 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
3223 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
3224 return true;
3225 } else if (axis == ROLL) { // ROLL right to override climb first
3226 return true;
3227 }
3228 }
3229 } else {
3230 rthOverrideStickHoldStartTime[axis] = 0;
3231 }
3232
3233 return false;
3234 }
3235
3236 /* ---------------------------------------------------
3237 * If climb stage is being used, see if it is time to
3238 * transiton in to turn.
3239 * Limited to fixed wing only.
3240 * --------------------------------------------------- */
3241 bool rthClimbStageActiveAndComplete(void) {
3242 if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
3243 if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
3244 return true;
3245 }
3246 }
3247
3248 return false;
3249 }
3250
3251 /*-----------------------------------------------------------
3252 * Update flight statistics
3253 *-----------------------------------------------------------*/
3254 static void updateNavigationFlightStatistics(void)
3255 {
3256 static timeMs_t previousTimeMs = 0;
3257 const timeMs_t currentTimeMs = millis();
3258 const timeDelta_t timeDeltaMs = currentTimeMs - previousTimeMs;
3259 previousTimeMs = currentTimeMs;
3260
3261 if (ARMING_FLAG(ARMED)) {
3262 posControl.totalTripDistance += posControl.actualState.velXY * MS2S(timeDeltaMs);
3263 }
3264 }
3265
3266 /*
3267 * Total travel distance in cm
3268 */
3269 uint32_t getTotalTravelDistance(void)
3270 {
3271 return lrintf(posControl.totalTripDistance);
3272 }
3273
3274 /*-----------------------------------------------------------
3275 * Calculate platform-specific hold position (account for deceleration)
3276 *-----------------------------------------------------------*/
3277 void calculateInitialHoldPosition(fpVector3_t * pos)
3278 {
3279 if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
3280 calculateFixedWingInitialHoldPosition(pos);
3281 }
3282 else {
3283 calculateMulticopterInitialHoldPosition(pos);
3284 }
3285 }
3286
3287 /*-----------------------------------------------------------
3288 * Set active XYZ-target and desired heading
3289 *-----------------------------------------------------------*/
3290 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
3291 {
3292 // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
3293 if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
3294 posControl.desiredState.pos.x = pos->x;
3295 posControl.desiredState.pos.y = pos->y;
3296 }
3297
3298 // Z-position
3299 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
3300 updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET);
3301 }
3302
3303 // Heading
3304 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
3305 // Heading
3306 posControl.desiredState.yaw = yaw;
3307 }
3308 else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) {
3309 posControl.desiredState.yaw = calculateBearingToDestination(pos);
3310 }
3311 else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
3312 posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
3313 }
3314 }
3315
3316 void calculateFarAwayPos(fpVector3_t *farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance)
3317 {
3318 farAwayPos->x = start->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
3319 farAwayPos->y = start->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
3320 farAwayPos->z = start->z;
3321 }
3322
3323 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
3324 {
3325 calculateFarAwayPos(farAwayPos, &navGetCurrentActualPositionAndVelocity()->pos, bearing, distance);
3326 }
3327
3328 /*-----------------------------------------------------------
3329 * NAV land detector
3330 *-----------------------------------------------------------*/
3331 void updateLandingStatus(timeMs_t currentTimeMs)
3332 {
3333 static timeMs_t lastUpdateTimeMs = 0;
3334 if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz
3335 return;
3336 }
3337 lastUpdateTimeMs = currentTimeMs;
3338
3339 DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
3340 DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
3341
3342 if (!ARMING_FLAG(ARMED)) {
3343 if (STATE(LANDING_DETECTED)) {
3344 landingDetectorIsActive = false;
3345 }
3346 resetLandingDetector();
3347
3348 return;
3349 }
3350
3351 if (!landingDetectorIsActive) {
3352 if (isFlightDetected()) {
3353 landingDetectorIsActive = true;
3354 resetLandingDetector();
3355 }
3356 } else if (STATE(LANDING_DETECTED)) {
3357 pidResetErrorAccumulators();
3358 if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) {
3359 ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
3360 disarm(DISARM_LANDING);
3361 } else if (!navigationInAutomaticThrottleMode()) {
3362 if (STATE(AIRPLANE) && isFlightDetected()) {
3363 // Cancel landing detection flag if fixed wing redetected in flight
3364 resetLandingDetector();
3365 } else if (STATE(MULTIROTOR)) {
3366 // For multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
3367 landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
3368 }
3369 }
3370 } else if (isLandingDetected()) {
3371 ENABLE_STATE(LANDING_DETECTED);
3372 }
3373 }
3374
3375 bool isLandingDetected(void)
3376 {
3377 return STATE(AIRPLANE) ? isFixedWingLandingDetected() : isMulticopterLandingDetected();
3378 }
3379
3380 void resetLandingDetector(void)
3381 {
3382 DISABLE_STATE(LANDING_DETECTED);
3383 posControl.flags.resetLandingDetector = true;
3384 }
3385
3386 void resetLandingDetectorActiveState(void)
3387 {
3388 landingDetectorIsActive = false;
3389 }
3390
3391 bool isFlightDetected(void)
3392 {
3393 return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
3394 }
3395
3396 bool isProbablyStillFlying(void)
3397 {
3398 bool inFlightSanityCheck;
3399 if (STATE(MULTIROTOR)) {
3400 inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
3401 } else {
3402 inFlightSanityCheck = isGPSHeadingValid();
3403 }
3404
3405 return landingDetectorIsActive && inFlightSanityCheck;
3406 }
3407
3408 /*-----------------------------------------------------------
3409 * Z-position controller
3410 *-----------------------------------------------------------*/
3411 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
3412 {
3413 const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
3414 float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
3415
3416 if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
3417 if (posControl.flags.isAdjustingAltitude) {
3418 maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
3419 }
3420
3421 return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate);
3422 }
3423
3424 if (posControl.desiredState.climbRateDemand) {
3425 maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate);
3426 } else if (emergLandingIsActive) {
3427 maxClimbRate = navConfig()->general.emerg_descent_rate;
3428 }
3429
3430 const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
3431 float targetVel = 0.0f;
3432
3433 if (STATE(MULTIROTOR)) {
3434 targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
3435 } else {
3436 targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
3437 }
3438
3439 if (emergLandingIsActive && targetAltitudeError > -50.0f) {
3440 return -100.0f; // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target
3441 } else {
3442 return constrainf(targetVel, -maxClimbRate, maxClimbRate);
3443 }
3444 }
3445
3446 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
3447 {
3448 /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude then reducing down as altitude is reached.
3449 * Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0.
3450 *
3451 * ROC_TO_ALT_CURRENT - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
3452 * No climb rate or altitude target required.
3453 *
3454 * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
3455
3456 if (mode == ROC_TO_ALT_CURRENT) {
3457 posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
3458 desiredClimbRate = 0.0f;
3459 } else if (mode == ROC_TO_ALT_TARGET) {
3460 posControl.desiredState.pos.z = targetAltitude;
3461 }
3462
3463 posControl.desiredState.climbRateDemand = desiredClimbRate;
3464 posControl.flags.rocToAltMode = mode;
3465
3466 /*
3467 * If max altitude is set limit desired altitude and impose altitude limit for constant climbs unless climb rate is -ve.
3468 * Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit.
3469 */
3470 if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
3471 posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
3472
3473 if (mode != ROC_TO_ALT_CONSTANT || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) {
3474 return;
3475 }
3476
3477 if (posControl.flags.isAdjustingAltitude) {
3478 posControl.desiredState.pos.z = navConfig()->general.max_altitude;
3479 posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
3480 }
3481 }
3482 }
3483
3484 static void resetAltitudeController(bool useTerrainFollowing)
3485 {
3486 // Set terrain following flag
3487 posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
3488
3489 if (STATE(FIXED_WING_LEGACY)) {
3490 resetFixedWingAltitudeController();
3491 }
3492 else {
3493 resetMulticopterAltitudeController();
3494 }
3495 }
3496
3497 static void setupAltitudeController(void)
3498 {
3499 if (STATE(FIXED_WING_LEGACY)) {
3500 setupFixedWingAltitudeController();
3501 }
3502 else {
3503 setupMulticopterAltitudeController();
3504 }
3505 }
3506
3507 static bool adjustAltitudeFromRCInput(void)
3508 {
3509 if (STATE(FIXED_WING_LEGACY)) {
3510 return adjustFixedWingAltitudeFromRCInput();
3511 }
3512 else {
3513 return adjustMulticopterAltitudeFromRCInput();
3514 }
3515 }
3516
3517 /*-----------------------------------------------------------
3518 * Jump Counter support functions
3519 *-----------------------------------------------------------*/
3520 static void setupJumpCounters(void)
3521 {
3522 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
3523 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
3524 posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
3525 }
3526 }
3527 }
3528
3529 static void resetJumpCounter(void)
3530 {
3531 // reset the volatile counter from the set / static value
3532 posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p2;
3533 }
3534
3535 static void clearJumpCounters(void)
3536 {
3537 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++) {
3538 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
3539 posControl.waypointList[wp].p3 = 0;
3540 }
3541 }
3542 }
3543
3544
3545
3546 /*-----------------------------------------------------------
3547 * Heading controller (pass-through to MAG mode)
3548 *-----------------------------------------------------------*/
3549 static void resetHeadingController(void)
3550 {
3551 if (STATE(FIXED_WING_LEGACY)) {
3552 resetFixedWingHeadingController();
3553 }
3554 else {
3555 resetMulticopterHeadingController();
3556 }
3557 }
3558
3559 static bool adjustHeadingFromRCInput(void)
3560 {
3561 if (STATE(FIXED_WING_LEGACY)) {
3562 return adjustFixedWingHeadingFromRCInput();
3563 }
3564 else {
3565 return adjustMulticopterHeadingFromRCInput();
3566 }
3567 }
3568
3569 /*-----------------------------------------------------------
3570 * XY Position controller
3571 *-----------------------------------------------------------*/
3572 static void resetPositionController(void)
3573 {
3574 if (STATE(FIXED_WING_LEGACY)) {
3575 resetFixedWingPositionController();
3576 }
3577 else {
3578 resetMulticopterPositionController();
3579 resetMulticopterBrakingMode();
3580 }
3581 }
3582
3583 static bool adjustPositionFromRCInput(void)
3584 {
3585 bool retValue;
3586
3587 if (STATE(FIXED_WING_LEGACY)) {
3588 retValue = adjustFixedWingPositionFromRCInput();
3589 }
3590 else {
3591
3592 const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
3593 const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
3594
3595 retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
3596 }
3597
3598 return retValue;
3599 }
3600
3601 /*-----------------------------------------------------------
3602 * WP controller
3603 *-----------------------------------------------------------*/
3604 void resetGCSFlags(void)
3605 {
3606 posControl.flags.isGCSAssistedNavigationReset = false;
3607 posControl.flags.isGCSAssistedNavigationEnabled = false;
3608 }
3609
3610 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
3611 {
3612 /* Default waypoint to send */
3613 wpData->action = NAV_WP_ACTION_RTH;
3614 wpData->lat = 0;
3615 wpData->lon = 0;
3616 wpData->alt = 0;
3617 wpData->p1 = 0;
3618 wpData->p2 = 0;
3619 wpData->p3 = 0;
3620 wpData->flag = NAV_WP_FLAG_LAST;
3621
3622 // WP #0 - special waypoint - HOME
3623 if (wpNumber == 0) {
3624 if (STATE(GPS_FIX_HOME)) {
3625 wpData->lat = GPS_home.lat;
3626 wpData->lon = GPS_home.lon;
3627 wpData->alt = GPS_home.alt;
3628 }
3629 }
3630 // WP #255 - special waypoint - directly get actualPosition
3631 else if (wpNumber == 255) {
3632 gpsLocation_t wpLLH;
3633
3634 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
3635
3636 wpData->lat = wpLLH.lat;
3637 wpData->lon = wpLLH.lon;
3638 wpData->alt = wpLLH.alt;
3639 }
3640 // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode
3641 else if (wpNumber == 254) {
3642 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3643
3644 if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
3645 gpsLocation_t wpLLH;
3646
3647 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
3648
3649 wpData->lat = wpLLH.lat;
3650 wpData->lon = wpLLH.lon;
3651 wpData->alt = wpLLH.alt;
3652 }
3653 }
3654 // WP #1 - #60 - common waypoints - pre-programmed mission
3655 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
3656 if (wpNumber <= getWaypointCount()) {
3657 *wpData = posControl.waypointList[wpNumber - 1 + (ARMING_FLAG(ARMED) ? posControl.startWpIndex : 0)];
3658 if(wpData->action == NAV_WP_ACTION_JUMP) {
3659 wpData->p1 += 1; // make WP # (vice index)
3660 }
3661 }
3662 }
3663 }
3664
3665 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
3666 {
3667 gpsLocation_t wpLLH;
3668 navWaypointPosition_t wpPos;
3669
3670 // Pre-fill structure to convert to local coordinates
3671 wpLLH.lat = wpData->lat;
3672 wpLLH.lon = wpData->lon;
3673 wpLLH.alt = wpData->alt;
3674
3675 // WP #0 - special waypoint - HOME
3676 if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
3677 // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
3678 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
3679 setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
3680 }
3681 // WP #255 - special waypoint - directly set desiredPosition
3682 // Only valid when armed and in poshold mode
3683 else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
3684 ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
3685 (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
3686 // Convert to local coordinates
3687 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
3688
3689 navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
3690
3691 // If we received global altitude == 0, use current altitude
3692 if (wpData->alt != 0) {
3693 waypointUpdateFlags |= NAV_POS_UPDATE_Z;
3694 }
3695
3696 if (wpData->p1 > 0 && wpData->p1 < 360) {
3697 waypointUpdateFlags |= NAV_POS_UPDATE_HEADING;
3698 }
3699
3700 setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
3701 }
3702 // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
3703 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !FLIGHT_MODE(NAV_WP_MODE)) {
3704 // WP upload is not allowed why WP mode is active
3705 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 ) {
3706 // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
3707 static int8_t nonGeoWaypointCount = 0;
3708
3709 if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
3710 if (wpNumber == 1) {
3711 resetWaypointList();
3712 }
3713 posControl.waypointList[wpNumber - 1] = *wpData;
3714 if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
3715 nonGeoWaypointCount += 1;
3716 if(wpData->action == NAV_WP_ACTION_JUMP) {
3717 posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
3718 }
3719 }
3720
3721 posControl.waypointCount = wpNumber;
3722 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
3723 posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
3724 if (posControl.waypointListValid) {
3725 nonGeoWaypointCount = 0;
3726 // 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
3727 if (posControl.activeWaypointIndex > posControl.waypointCount) {
3728 posControl.activeWaypointIndex = 0;
3729 }
3730 }
3731 }
3732 }
3733 }
3734 }
3735
3736 void resetWaypointList(void)
3737 {
3738 posControl.waypointCount = 0;
3739 posControl.waypointListValid = false;
3740 posControl.geoWaypointCount = 0;
3741 posControl.startWpIndex = 0;
3742 #ifdef USE_MULTI_MISSION
3743 posControl.totalMultiMissionWpCount = 0;
3744 posControl.loadedMultiMissionIndex = 0;
3745 posControl.multiMissionCount = 0;
3746 #endif
3747 }
3748
3749 bool isWaypointListValid(void)
3750 {
3751 return posControl.waypointListValid;
3752 }
3753
3754 int getWaypointCount(void)
3755 {
3756 uint8_t waypointCount = posControl.waypointCount;
3757 #ifdef USE_MULTI_MISSION
3758 if (!ARMING_FLAG(ARMED) && posControl.totalMultiMissionWpCount) {
3759 waypointCount = posControl.totalMultiMissionWpCount;
3760 }
3761 #endif
3762 return waypointCount;
3763 }
3764
3765 #ifdef USE_MULTI_MISSION
3766 void selectMultiMissionIndex(int8_t increment)
3767 {
3768 if (posControl.multiMissionCount > 1) { // stick selection only active when multi mission loaded
3769 navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 1, posControl.multiMissionCount);
3770 }
3771 }
3772
3773 void loadSelectedMultiMission(uint8_t missionIndex)
3774 {
3775 uint8_t missionCount = 1;
3776 posControl.waypointCount = 0;
3777 posControl.geoWaypointCount = 0;
3778
3779 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3780 if (missionCount == missionIndex) {
3781 /* store details of selected mission: start wp index, mission wp count, geo wp count */
3782 if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
3783 posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
3784 posControl.waypointList[i].action == NAV_WP_ACTION_JUMP)) {
3785 posControl.geoWaypointCount++;
3786 }
3787 // mission start WP
3788 if (posControl.waypointCount == 0) {
3789 posControl.waypointCount = 1; // start marker only, value unimportant (but not 0)
3790 posControl.startWpIndex = i;
3791 }
3792 // mission end WP
3793 if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
3794 posControl.waypointCount = i - posControl.startWpIndex + 1;
3795 break;
3796 }
3797 } else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
3798 missionCount++;
3799 }
3800 }
3801
3802 posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
3803 posControl.activeWaypointIndex = posControl.startWpIndex;
3804 }
3805
3806 bool updateWpMissionChange(void)
3807 {
3808 /* Function only called when ARMED */
3809
3810 if (posControl.multiMissionCount < 2 || posControl.wpPlannerActiveWPIndex || FLIGHT_MODE(NAV_WP_MODE)) {
3811 return true;
3812 }
3813
3814 uint8_t setMissionIndex = navConfig()->general.waypoint_multi_mission_index;
3815 if (!(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) || isAdjustmentFunctionSelected(ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX))) {
3816 /* reload mission if mission index changed */
3817 if (posControl.loadedMultiMissionIndex != setMissionIndex) {
3818 loadSelectedMultiMission(setMissionIndex);
3819 }
3820 return true;
3821 }
3822
3823 static bool toggleFlag = false;
3824 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) {
3825 if (setMissionIndex == posControl.multiMissionCount) {
3826 navConfigMutable()->general.waypoint_multi_mission_index = 1;
3827 } else {
3828 selectMultiMissionIndex(1);
3829 }
3830 toggleFlag = false;
3831 } else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
3832 toggleFlag = true;
3833 }
3834 return false; // block WP mode while changing mission when armed
3835 }
3836
3837 bool checkMissionCount(int8_t waypoint)
3838 {
3839 if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
3840 posControl.multiMissionCount += 1; // count up no missions in multi mission WP file
3841 if (waypoint != NAV_MAX_WAYPOINTS - 1) {
3842 return (nonVolatileWaypointList(waypoint + 1)->flag == NAV_WP_FLAG_LAST &&
3843 nonVolatileWaypointList(waypoint + 1)->action ==NAV_WP_ACTION_RTH);
3844 // end of multi mission file if successive NAV_WP_FLAG_LAST and default action (RTH)
3845 }
3846 }
3847 return false;
3848 }
3849 #endif // multi mission
3850 #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
3851 bool loadNonVolatileWaypointList(bool clearIfLoaded)
3852 {
3853 /* Don't load if armed or mission planner active */
3854 if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
3855 return false;
3856 }
3857
3858 // if forced and waypoints are already loaded, just unload them.
3859 if (clearIfLoaded && posControl.waypointCount > 0) {
3860 resetWaypointList();
3861 return false;
3862 }
3863 #ifdef USE_MULTI_MISSION
3864 /* Reset multi mission index to 1 if exceeds number of available missions */
3865 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
3866 navConfigMutable()->general.waypoint_multi_mission_index = 1;
3867 }
3868 #endif
3869 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3870 setWaypoint(i + 1, nonVolatileWaypointList(i));
3871 #ifdef USE_MULTI_MISSION
3872 /* count up number of missions and exit after last multi mission */
3873 if (checkMissionCount(i)) {
3874 break;
3875 }
3876 }
3877 posControl.totalMultiMissionWpCount = posControl.waypointCount;
3878 loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
3879
3880 /* Mission sanity check failed - reset the list
3881 * Also reset if no selected mission loaded (shouldn't happen) */
3882 if (!posControl.waypointListValid || !posControl.waypointCount) {
3883 #else
3884 // check this is the last waypoint
3885 if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
3886 break;
3887 }
3888 }
3889
3890 // Mission sanity check failed - reset the list
3891 if (!posControl.waypointListValid) {
3892 #endif
3893 resetWaypointList();
3894 }
3895
3896 return posControl.waypointListValid;
3897 }
3898
3899 bool saveNonVolatileWaypointList(void)
3900 {
3901 if (ARMING_FLAG(ARMED) || !posControl.waypointListValid)
3902 return false;
3903
3904 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3905 getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
3906 }
3907 #ifdef USE_MULTI_MISSION
3908 navConfigMutable()->general.waypoint_multi_mission_index = 1; // reset selected mission to 1 when new entries saved
3909 #endif
3910 saveConfigAndNotify();
3911
3912 return true;
3913 }
3914 #endif
3915
3916 #if defined(USE_SAFE_HOME)
3917
3918 void resetSafeHomes(void)
3919 {
3920 memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
3921 }
3922 #endif
3923
3924 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
3925 {
3926 gpsLocation_t wpLLH;
3927
3928 /* Default to home position if lat & lon = 0 or HOME flag set
3929 * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
3930 if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
3931 wpLLH.lat = GPS_home.lat;
3932 wpLLH.lon = GPS_home.lon;
3933 } else {
3934 wpLLH.lat = waypoint->lat;
3935 wpLLH.lon = waypoint->lon;
3936 }
3937 wpLLH.alt = waypoint->alt;
3938
3939 geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
3940 }
3941
3942 void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos)
3943 {
3944 // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
3945 if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
3946 posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
3947 } else {
3948 posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
3949 }
3950 posControl.activeWaypoint.nextTurnAngle = -1; // no turn angle set (-1), will be set by WP mode as required
3951
3952 posControl.activeWaypoint.pos = *pos;
3953
3954 // Set desired position to next waypoint (XYZ-controller)
3955 setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
3956 }
3957
3958 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
3959 {
3960 return ((datumFlag & NAV_WP_MSL_DATUM) == NAV_WP_MSL_DATUM) ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
3961 }
3962
3963 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
3964 {
3965 fpVector3_t localPos;
3966 mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
3967 calculateAndSetActiveWaypointToLocalPosition(&localPos);
3968
3969 if (navConfig()->fw.wp_turn_smoothing) {
3970 fpVector3_t posNextWp;
3971 if (getLocalPosNextWaypoint(&posNextWp)) {
3972 int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
3973 posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
3974 }
3975 }
3976 }
3977
3978 /* Checks if active waypoint is last in mission */
3979 bool isLastMissionWaypoint(void)
3980 {
3981 return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.startWpIndex + posControl.waypointCount - 1) ||
3982 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
3983 }
3984
3985 /* Checks if Nav hold position is active */
3986 bool isNavHoldPositionActive(void)
3987 {
3988 /* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then
3989 * waypoints are assumed to be hold points by default unless excluded as defined here */
3990
3991 if (navGetCurrentStateFlags() & NAV_CTL_HOLD) {
3992 return true;
3993 }
3994
3995 // No hold required for basic WP type unless it's the last mission waypoint
3996 if (FLIGHT_MODE(NAV_WP_MODE)) {
3997 return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
3998 }
3999
4000 // No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are)
4001 return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
4002 }
4003
4004 float getActiveSpeed(void)
4005 {
4006 /* Currently only applicable for multicopter */
4007
4008 // Speed limit for modes where speed manually controlled
4009 if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
4010 return navConfig()->general.max_manual_speed;
4011 }
4012
4013 uint16_t waypointSpeed = navConfig()->general.auto_speed;
4014
4015 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
4016 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)) {
4017 float wpSpecificSpeed = 0.0f;
4018 if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
4019 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
4020 else
4021 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
4022
4023 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
4024 waypointSpeed = wpSpecificSpeed;
4025 } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
4026 waypointSpeed = navConfig()->general.max_auto_speed;
4027 }
4028 }
4029 }
4030
4031 return waypointSpeed;
4032 }
4033
4034 bool isWaypointNavTrackingActive(void)
4035 {
4036 // NAV_WP_MODE flag used rather than state flag NAV_AUTO_WP to ensure heading to initial waypoint
4037 // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
4038 // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
4039
4040 return FLIGHT_MODE(NAV_WP_MODE)
4041 || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
4042 || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
4043 }
4044
4045 /*-----------------------------------------------------------
4046 * Process adjustments to alt, pos and yaw controllers
4047 *-----------------------------------------------------------*/
4048 static void processNavigationRCAdjustments(void)
4049 {
4050 /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
4051 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
4052
4053 if (FLIGHT_MODE(FAILSAFE_MODE)) {
4054 if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
4055 resetMulticopterBrakingMode();
4056 }
4057 posControl.flags.isAdjustingAltitude = false;
4058 posControl.flags.isAdjustingPosition = false;
4059 posControl.flags.isAdjustingHeading = false;
4060
4061 return;
4062 }
4063
4064 posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
4065 posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
4066 posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
4067 }
4068
4069 /*-----------------------------------------------------------
4070 * A main function to call position controllers at loop rate
4071 *-----------------------------------------------------------*/
4072 void applyWaypointNavigationAndAltitudeHold(void)
4073 {
4074 const timeUs_t currentTimeUs = micros();
4075
4076 //Updata blackbox data
4077 navFlags = 0;
4078 if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
4079 if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
4080 if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
4081 if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
4082 // naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
4083 if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
4084
4085 // Reset all navigation requests - NAV controllers will set them if necessary
4086 DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
4087
4088 // No navigation when disarmed
4089 if (!ARMING_FLAG(ARMED)) {
4090 // If we are disarmed, abort forced RTH or Emergency Landing
4091 posControl.flags.forcedRTHActivated = false;
4092 posControl.flags.forcedEmergLandingActivated = false;
4093 posControl.flags.manualEmergLandActive = false;
4094 // ensure WP missions always restart from first waypoint after disarm
4095 posControl.activeWaypointIndex = posControl.startWpIndex;
4096 // Reset RTH trackback
4097 resetRthTrackBack();
4098
4099 return;
4100 }
4101
4102 /* Reset flags */
4103 posControl.flags.horizontalPositionDataConsumed = false;
4104 posControl.flags.verticalPositionDataConsumed = false;
4105
4106 #ifdef USE_FW_AUTOLAND
4107 if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
4108 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
4109 }
4110 #endif
4111
4112 /* Process controllers */
4113 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
4114 if (STATE(ROVER) || STATE(BOAT)) {
4115 applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
4116 } else if (STATE(FIXED_WING_LEGACY)) {
4117 applyFixedWingNavigationController(navStateFlags, currentTimeUs);
4118 }
4119 else {
4120 applyMulticopterNavigationController(navStateFlags, currentTimeUs);
4121 }
4122
4123 /* Consume position data */
4124 if (posControl.flags.horizontalPositionDataConsumed)
4125 posControl.flags.horizontalPositionDataNew = false;
4126
4127 if (posControl.flags.verticalPositionDataConsumed)
4128 posControl.flags.verticalPositionDataNew = false;
4129
4130 //Update blackbox data
4131 if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
4132 if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
4133 if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
4134
4135 navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
4136 navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
4137 navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
4138
4139 navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
4140 }
4141
4142 /*-----------------------------------------------------------
4143 * Set CF's FLIGHT_MODE from current NAV_MODE
4144 *-----------------------------------------------------------*/
4145 void switchNavigationFlightModes(void)
4146 {
4147 const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
4148 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);
4149 DISABLE_FLIGHT_MODE(disabledFlightModes);
4150 ENABLE_FLIGHT_MODE(enabledNavFlightModes);
4151 }
4152
4153 /*-----------------------------------------------------------
4154 * desired NAV_MODE from combination of FLIGHT_MODE flags
4155 *-----------------------------------------------------------*/
4156 static bool canActivateAltHoldMode(void)
4157 {
4158 return (posControl.flags.estAltStatus >= EST_USABLE);
4159 }
4160
4161 static bool canActivatePosHoldMode(void)
4162 {
4163 return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
4164 }
4165
4166 static bool canActivateNavigationModes(void)
4167 {
4168 return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
4169 }
4170
4171 static bool isWaypointMissionValid(void)
4172 {
4173 return posControl.waypointListValid && (posControl.waypointCount > 0);
4174 }
4175
4176 void checkManualEmergencyLandingControl(bool forcedActivation)
4177 {
4178 static timeMs_t timeout = 0;
4179 static int8_t counter = 0;
4180 static bool toggle;
4181 timeMs_t currentTimeMs = millis();
4182
4183 if (timeout && currentTimeMs > timeout) {
4184 timeout += 1000;
4185 counter -= counter ? 1 : 0;
4186 if (!counter) {
4187 timeout = 0;
4188 }
4189 }
4190 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
4191 if (!timeout && toggle) {
4192 timeout = currentTimeMs + 4000;
4193 }
4194 counter += toggle;
4195 toggle = false;
4196 } else {
4197 toggle = true;
4198 }
4199
4200 // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
4201 if (counter >= 5 || forcedActivation) {
4202 counter = 0;
4203 posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
4204
4205 if (!posControl.flags.manualEmergLandActive) {
4206 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4207 }
4208 }
4209 }
4210
4211 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
4212 {
4213 static bool canActivateLaunchMode = false;
4214
4215 //We can switch modes only when ARMED
4216 if (ARMING_FLAG(ARMED)) {
4217 // Ask failsafe system if we can use navigation system
4218 if (failsafeBypassNavigation()) {
4219 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4220 }
4221
4222 // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
4223 const bool canActivateAltHold = canActivateAltHoldMode();
4224 const bool canActivatePosHold = canActivatePosHoldMode();
4225 const bool canActivateNavigation = canActivateNavigationModes();
4226 const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
4227 #ifdef USE_SAFE_HOME
4228 checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
4229 #endif
4230 // deactivate rth trackback if RTH not active
4231 if (posControl.flags.rthTrackbackActive) {
4232 posControl.flags.rthTrackbackActive = isExecutingRTH;
4233 }
4234
4235 /* Emergency landing controlled manually by rapid switching of Poshold mode.
4236 * Landing toggled ON or OFF for each Poshold activation sequence */
4237 checkManualEmergencyLandingControl(false);
4238
4239 /* Emergency landing triggered by failsafe Landing or manually initiated */
4240 if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
4241 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
4242 }
4243
4244 /* Keep Emergency landing mode active once triggered.
4245 * If caused by sensor failure - landing auto cancelled if sensors working again or when WP and RTH deselected or if Althold selected.
4246 * If caused by RTH Sanity Checking - landing cancelled if RTH deselected.
4247 * Remains active if failsafe active regardless of mode selections */
4248 if (navigationIsExecutingAnEmergencyLanding()) {
4249 bool autonomousNavIsPossible = canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME);
4250 bool emergLandingCancel = (!autonomousNavIsPossible &&
4251 ((IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) || !(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH)))) ||
4252 (autonomousNavIsPossible && !IS_RC_MODE_ACTIVE(BOXNAVRTH));
4253
4254 if ((!posControl.rthSanityChecker.rthSanityOK || !autonomousNavIsPossible) && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) {
4255 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
4256 }
4257 }
4258 posControl.rthSanityChecker.rthSanityOK = true;
4259
4260 /* Airplane specific modes */
4261 if (STATE(AIRPLANE)) {
4262 // LAUNCH mode has priority over any other NAV mode
4263 if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
4264 if (canActivateLaunchMode) {
4265 canActivateLaunchMode = false;
4266 return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
4267 }
4268 else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
4269 // Make sure we don't bail out to IDLE
4270 return NAV_FSM_EVENT_NONE;
4271 }
4272 }
4273 else {
4274 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low or throttle stick < launch idle throttle
4275 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
4276 if (abortLaunchAllowed()) {
4277 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4278 } else {
4279 return NAV_FSM_EVENT_NONE;
4280 }
4281 }
4282 }
4283
4284 /* Soaring mode, disables altitude control in Position hold and Course hold modes.
4285 * Pitch allowed to freefloat within defined Angle mode deadband */
4286 if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
4287 ENABLE_FLIGHT_MODE(SOARING_MODE);
4288 } else {
4289 DISABLE_FLIGHT_MODE(SOARING_MODE);
4290 }
4291 }
4292
4293 /* If we request forced RTH - attempt to activate it no matter what
4294 * This might switch to emergency landing controller if GPS is unavailable */
4295 if (posControl.flags.forcedRTHActivated) {
4296 return NAV_FSM_EVENT_SWITCH_TO_RTH;
4297 }
4298
4299 /* WP mission activation control:
4300 * canActivateWaypoint & waypointWasActivated are used to prevent WP mission
4301 * auto restarting after interruption by Manual or RTH modes.
4302 * WP mode must be deselected before it can be reactivated again
4303 * WP Mode also inhibited when Mission Planner is active */
4304 static bool waypointWasActivated = false;
4305 bool canActivateWaypoint = isWaypointMissionValid();
4306 bool wpRthFallbackIsActive = false;
4307
4308 if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) {
4309 canActivateWaypoint = false;
4310 } else {
4311 if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
4312 canActivateWaypoint = false;
4313
4314 if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4315 canActivateWaypoint = true;
4316 waypointWasActivated = false;
4317 }
4318 }
4319
4320 wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint;
4321 }
4322
4323 /* Pilot-triggered RTH, also fall-back for WP if no mission is loaded.
4324 * Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
4325 * Without this loss of any of the canActivateNavigation && canActivateAltHold
4326 * will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
4327 * logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) */
4328 if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
4329 if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
4330 return NAV_FSM_EVENT_SWITCH_TO_RTH;
4331 }
4332 }
4333
4334 // MANUAL mode has priority over WP/PH/AH
4335 if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
4336 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4337 }
4338
4339 // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
4340 // Also check multimission mission change status before activating WP mode.
4341 #ifdef USE_MULTI_MISSION
4342 if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4343 #else
4344 if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4345 #endif
4346 if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
4347 waypointWasActivated = true;
4348 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
4349 }
4350 }
4351
4352 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
4353 if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
4354 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
4355 }
4356
4357 // CRUISE has priority over COURSE_HOLD and AH
4358 if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
4359 if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
4360 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
4361 }
4362
4363 // PH has priority over COURSE_HOLD
4364 // CRUISE has priority on AH
4365 if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
4366 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
4367 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
4368 }
4369
4370 if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) {
4371 return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
4372 }
4373 }
4374
4375 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
4376 if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
4377 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
4378 }
4379 } else {
4380 // 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)
4381 canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid()));
4382 }
4383
4384 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
4385 }
4386
4387 /*-----------------------------------------------------------
4388 * An indicator that throttle tilt compensation is forced
4389 *-----------------------------------------------------------*/
4390 bool navigationRequiresThrottleTiltCompensation(void)
4391 {
4392 return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
4393 }
4394
4395 /*-----------------------------------------------------------
4396 * An indicator that ANGLE mode must be forced per NAV requirement
4397 *-----------------------------------------------------------*/
4398 bool navigationRequiresAngleMode(void)
4399 {
4400 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
4401 return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
4402 }
4403
4404 /*-----------------------------------------------------------
4405 * An indicator that TURN ASSISTANCE is required for navigation
4406 *-----------------------------------------------------------*/
4407 bool navigationRequiresTurnAssistance(void)
4408 {
4409 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
4410 if (STATE(FIXED_WING_LEGACY)) {
4411 // For airplanes turn assistant is always required when controlling position
4412 return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
4413 }
4414
4415 return false;
4416 }
4417
4418 /**
4419 * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
4420 */
4421 int8_t navigationGetHeadingControlState(void)
4422 {
4423 // For airplanes report as manual heading control
4424 if (STATE(FIXED_WING_LEGACY)) {
4425 return NAV_HEADING_CONTROL_MANUAL;
4426 }
4427
4428 // For multirotors it depends on navigation system mode
4429 // Course hold requires Auto Control to update heading hold target whilst RC adjustment active
4430 if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
4431 if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
4432 return NAV_HEADING_CONTROL_MANUAL;
4433 }
4434
4435 return NAV_HEADING_CONTROL_AUTO;
4436 }
4437
4438 return NAV_HEADING_CONTROL_NONE;
4439 }
4440
4441 bool navigationTerrainFollowingEnabled(void)
4442 {
4443 return posControl.flags.isTerrainFollowEnabled;
4444 }
4445
4446 uint32_t distanceToFirstWP(void)
4447 {
4448 fpVector3_t startingWaypointPos;
4449 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.startWpIndex], GEO_ALT_RELATIVE);
4450 return calculateDistanceToDestination(&startingWaypointPos);
4451 }
4452
4453 bool navigationPositionEstimateIsHealthy(void)
4454 {
4455 return posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE && STATE(GPS_FIX_HOME);
4456 }
4457
4458 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
4459 {
4460 const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) ||
4461 IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
4462
4463 if (usedBypass) {
4464 *usedBypass = false;
4465 }
4466
4467 // Apply extra arming safety only if pilot has any of GPS modes configured
4468 if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !navigationPositionEstimateIsHealthy()) {
4469 if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
4470 (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
4471 if (usedBypass) {
4472 *usedBypass = true;
4473 }
4474 return NAV_ARMING_BLOCKER_NONE;
4475 }
4476 return NAV_ARMING_BLOCKER_MISSING_GPS_FIX;
4477 }
4478
4479 // Don't allow arming if any of NAV modes is active
4480 if (!ARMING_FLAG(ARMED) && navBoxModesEnabled) {
4481 return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
4482 }
4483
4484 // Don't allow arming if first waypoint is farther than configured safe distance
4485 if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
4486 if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
4487 return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
4488 }
4489 }
4490
4491 /*
4492 * Don't allow arming if any of JUMP waypoint has invalid settings
4493 * First WP can't be JUMP
4494 * Can't jump to immediately adjacent WPs (pointless)
4495 * Can't jump beyond WP list
4496 * Only jump to geo-referenced WP types
4497 */
4498 if (posControl.waypointCount) {
4499 for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){
4500 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
4501 if (wp == posControl.startWpIndex || posControl.waypointList[wp].p1 >= posControl.waypointCount ||
4502 (posControl.waypointList[wp].p1 > (wp - posControl.startWpIndex - 2) && posControl.waypointList[wp].p1 < (wp - posControl.startWpIndex + 2)) || posControl.waypointList[wp].p2 < -1) {
4503 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
4504 }
4505
4506 /* check for target geo-ref sanity */
4507 uint16_t target = posControl.waypointList[wp].p1 + posControl.startWpIndex;
4508 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)) {
4509 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
4510 }
4511 }
4512 }
4513 }
4514
4515 return NAV_ARMING_BLOCKER_NONE;
4516 }
4517
4518 /**
4519 * Indicate ready/not ready status
4520 */
4521 static void updateReadyStatus(void)
4522 {
4523 static bool posReadyBeepDone = false;
4524
4525 /* Beep out READY_BEEP once when position lock is firstly acquired and HOME set */
4526 if (navigationPositionEstimateIsHealthy() && !posReadyBeepDone) {
4527 beeper(BEEPER_READY_BEEP);
4528 posReadyBeepDone = true;
4529 }
4530 }
4531
4532 void updateFlightBehaviorModifiers(void)
4533 {
4534 if (posControl.flags.isGCSAssistedNavigationEnabled && !IS_RC_MODE_ACTIVE(BOXGCSNAV)) {
4535 posControl.flags.isGCSAssistedNavigationReset = true;
4536 }
4537
4538 posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
4539 }
4540
4541 /* On the fly WP mission planner mode allows WP missions to be setup during navigation.
4542 * Uses the WP mode switch to save WP at current location (WP mode disabled when active)
4543 * Mission can be flown after mission planner mode switched off and saved after disarm. */
4544
4545 void updateWpMissionPlanner(void)
4546 {
4547 static timeMs_t resetTimerStart = 0;
4548 if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
4549 const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
4550 #ifdef USE_GPS_FIX_ESTIMATION
4551 || STATE(GPS_ESTIMATED_FIX)
4552 #endif
4553 );
4554
4555 posControl.flags.wpMissionPlannerActive = true;
4556 if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
4557 posControl.waypointCount = posControl.wpPlannerActiveWPIndex = 0;
4558 posControl.waypointListValid = false;
4559 posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
4560 }
4561 if (positionTrusted && posControl.wpMissionPlannerStatus != WP_PLAN_FULL) {
4562 missionPlannerSetWaypoint();
4563 } else {
4564 posControl.wpMissionPlannerStatus = posControl.wpMissionPlannerStatus == WP_PLAN_FULL ? WP_PLAN_FULL : WP_PLAN_WAIT;
4565 }
4566 } else if (posControl.flags.wpMissionPlannerActive) {
4567 posControl.flags.wpMissionPlannerActive = false;
4568 posControl.activeWaypointIndex = 0;
4569 resetTimerStart = millis();
4570 }
4571 }
4572
4573 void missionPlannerSetWaypoint(void)
4574 {
4575 static bool boxWPModeIsReset = true;
4576
4577 boxWPModeIsReset = !boxWPModeIsReset ? !IS_RC_MODE_ACTIVE(BOXNAVWP) : boxWPModeIsReset; // only able to save new WP when WP mode reset
4578 posControl.wpMissionPlannerStatus = boxWPModeIsReset ? boxWPModeIsReset : posControl.wpMissionPlannerStatus; // hold save status until WP mode reset
4579
4580 if (!boxWPModeIsReset || !IS_RC_MODE_ACTIVE(BOXNAVWP)) {
4581 return;
4582 }
4583
4584 if (!posControl.wpPlannerActiveWPIndex) { // reset existing mission data before adding first WP
4585 resetWaypointList();
4586 }
4587
4588 gpsLocation_t wpLLH;
4589 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
4590
4591 posControl.waypointList[posControl.wpPlannerActiveWPIndex].action = 1;
4592 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lat = wpLLH.lat;
4593 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
4594 posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
4595 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
4596 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 |= NAV_WP_ALTMODE; // use absolute altitude datum
4597 posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
4598 posControl.waypointListValid = true;
4599
4600 if (posControl.wpPlannerActiveWPIndex) {
4601 posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
4602 }
4603
4604 posControl.wpPlannerActiveWPIndex += 1;
4605 posControl.waypointCount = posControl.geoWaypointCount = posControl.wpPlannerActiveWPIndex;
4606 posControl.wpMissionPlannerStatus = posControl.waypointCount == NAV_MAX_WAYPOINTS ? WP_PLAN_FULL : WP_PLAN_OK;
4607 boxWPModeIsReset = false;
4608 }
4609
4610 /**
4611 * Process NAV mode transition and WP/RTH state machine
4612 * Update rate: RX (data driven or 50Hz)
4613 */
4614 void updateWaypointsAndNavigationMode(void)
4615 {
4616 /* Initiate home position update */
4617 updateHomePosition();
4618
4619 /* Update flight statistics */
4620 updateNavigationFlightStatistics();
4621
4622 /* Update NAV ready status */
4623 updateReadyStatus();
4624
4625 // Update flight behaviour modifiers
4626 updateFlightBehaviorModifiers();
4627
4628 // Process switch to a different navigation mode (if needed)
4629 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4630
4631 // Process pilot's RC input to adjust behaviour
4632 processNavigationRCAdjustments();
4633
4634 // Map navMode back to enabled flight modes
4635 switchNavigationFlightModes();
4636
4637 // Update WP mission planner
4638 updateWpMissionPlanner();
4639
4640 // Update RTH trackback
4641 rthTrackBackUpdate(false);
4642
4643 //Update Blackbox data
4644 navCurrentState = (int16_t)posControl.navPersistentId;
4645 }
4646
4647 /*-----------------------------------------------------------
4648 * NAV main control functions
4649 *-----------------------------------------------------------*/
4650 void navigationUsePIDs(void)
4651 {
4652 /** Multicopter PIDs */
4653 // Brake time parameter
4654 posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f;
4655
4656 // Position controller expo (taret vel expo for MC)
4657 posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f);
4658
4659 // Initialize position hold P-controller
4660 for (int axis = 0; axis < 2; axis++) {
4661 navPidInit(
4662 &posControl.pids.pos[axis],
4663 (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
4664 0.0f,
4665 0.0f,
4666 0.0f,
4667 NAV_DTERM_CUT_HZ,
4668 0.0f
4669 );
4670
4671 navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
4672 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
4673 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
4674 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
4675 pidProfile()->navVelXyDTermLpfHz,
4676 0.0f
4677 );
4678 }
4679
4680 /*
4681 * Set coefficients used in MC VEL_XY
4682 */
4683 multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f;
4684 multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f;
4685 multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f;
4686
4687 #ifdef USE_MR_BRAKING_MODE
4688 multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;
4689 #endif
4690
4691 // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
4692 navPidInit(
4693 &posControl.pids.pos[Z],
4694 (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
4695 0.0f,
4696 0.0f,
4697 0.0f,
4698 NAV_DTERM_CUT_HZ,
4699 0.0f
4700 );
4701
4702 navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
4703 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
4704 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
4705 0.0f,
4706 NAV_VEL_Z_DERIVATIVE_CUT_HZ,
4707 NAV_VEL_Z_ERROR_CUT_HZ
4708 );
4709
4710 // Initialize surface tracking PID
4711 navPidInit(&posControl.pids.surface, 2.0f,
4712 0.0f,
4713 0.0f,
4714 0.0f,
4715 NAV_DTERM_CUT_HZ,
4716 0.0f
4717 );
4718
4719 /** Airplane PIDs */
4720 // Initialize fixed wing PID controllers
4721 navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
4722 (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
4723 (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
4724 0.0f,
4725 NAV_DTERM_CUT_HZ,
4726 0.0f
4727 );
4728
4729 navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
4730 (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
4731 (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
4732 0.0f,
4733 NAV_DTERM_CUT_HZ,
4734 0.0f
4735 );
4736
4737 navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
4738 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
4739 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
4740 0.0f,
4741 2.0f,
4742 0.0f
4743 );
4744 }
4745
4746 void navigationInit(void)
4747 {
4748 /* Initial state */
4749 posControl.navState = NAV_STATE_IDLE;
4750
4751 posControl.flags.horizontalPositionDataNew = false;
4752 posControl.flags.verticalPositionDataNew = false;
4753
4754 posControl.flags.estAltStatus = EST_NONE;
4755 posControl.flags.estPosStatus = EST_NONE;
4756 posControl.flags.estVelStatus = EST_NONE;
4757 posControl.flags.estHeadingStatus = EST_NONE;
4758 posControl.flags.estAglStatus = EST_NONE;
4759
4760 posControl.flags.forcedRTHActivated = false;
4761 posControl.flags.forcedEmergLandingActivated = false;
4762 posControl.waypointCount = 0;
4763 posControl.activeWaypointIndex = 0;
4764 posControl.waypointListValid = false;
4765 posControl.wpPlannerActiveWPIndex = 0;
4766 posControl.flags.wpMissionPlannerActive = false;
4767 posControl.startWpIndex = 0;
4768 posControl.safehomeState.isApplied = false;
4769 #ifdef USE_MULTI_MISSION
4770 posControl.multiMissionCount = 0;
4771 #endif
4772 /* Set initial surface invalid */
4773 posControl.actualState.surfaceMin = -1.0f;
4774
4775 /* Reset statistics */
4776 posControl.totalTripDistance = 0.0f;
4777
4778 /* Use system config */
4779 navigationUsePIDs();
4780
4781 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
4782 /* configure WP missions at boot */
4783 #ifdef USE_MULTI_MISSION
4784 for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { // check number missions in NVM
4785 if (checkMissionCount(i)) {
4786 break;
4787 }
4788 }
4789 /* set index to 1 if saved mission index > available missions */
4790 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
4791 navConfigMutable()->general.waypoint_multi_mission_index = 1;
4792 }
4793 #endif
4794 /* load mission on boot */
4795 if (navConfig()->general.waypoint_load_on_boot) {
4796 loadNonVolatileWaypointList(false);
4797 }
4798 #endif
4799 }
4800
4801 /*-----------------------------------------------------------
4802 * Access to estimated position/velocity data
4803 *-----------------------------------------------------------*/
4804 float getEstimatedActualVelocity(int axis)
4805 {
4806 return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
4807 }
4808
4809 float getEstimatedActualPosition(int axis)
4810 {
4811 return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
4812 }
4813
4814 /*-----------------------------------------------------------
4815 * Ability to execute RTH on external event
4816 *-----------------------------------------------------------*/
4817 void activateForcedRTH(void)
4818 {
4819 abortFixedWingLaunch();
4820 posControl.flags.forcedRTHActivated = true;
4821 #ifdef USE_SAFE_HOME
4822 checkSafeHomeState(true);
4823 #endif
4824 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4825 }
4826
4827 void abortForcedRTH(void)
4828 {
4829 // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
4830 // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
4831 posControl.flags.forcedRTHActivated = false;
4832 #ifdef USE_SAFE_HOME
4833 checkSafeHomeState(false);
4834 #endif
4835 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4836 }
4837
4838 rthState_e getStateOfForcedRTH(void)
4839 {
4840 /* If forced RTH activated and in AUTO_RTH, EMERG state or FW Auto Landing */
4841 if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) {
4842 if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED || posControl.navState == NAV_STATE_FW_LANDING_FINISHED) {
4843 return RTH_HAS_LANDED;
4844 }
4845 else {
4846 return RTH_IN_PROGRESS;
4847 }
4848 }
4849 else {
4850 return RTH_IDLE;
4851 }
4852 }
4853
4854 /*-----------------------------------------------------------
4855 * Ability to execute Emergency Landing on external event
4856 *-----------------------------------------------------------*/
4857 void activateForcedEmergLanding(void)
4858 {
4859 abortFixedWingLaunch();
4860 posControl.flags.forcedEmergLandingActivated = true;
4861 navProcessFSMEvents(selectNavEventFromBoxModeInput());
4862 }
4863
4864 void abortForcedEmergLanding(void)
4865 {
4866 // Disable emergency landing and make sure we back out of navigation mode to IDLE
4867 // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update
4868 posControl.flags.forcedEmergLandingActivated = false;
4869 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
4870 }
4871
4872 emergLandState_e getStateOfForcedEmergLanding(void)
4873 {
4874 /* If forced emergency landing activated and in EMERG state */
4875 if (posControl.flags.forcedEmergLandingActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_EMERG)) {
4876 if (posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
4877 return EMERG_LAND_HAS_LANDED;
4878 } else {
4879 return EMERG_LAND_IN_PROGRESS;
4880 }
4881 } else {
4882 return EMERG_LAND_IDLE;
4883 }
4884 }
4885
4886 bool isWaypointMissionRTHActive(void)
4887 {
4888 return (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) && IS_RC_MODE_ACTIVE(BOXNAVWP) &&
4889 !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
4890 }
4891
4892 bool navigationIsExecutingAnEmergencyLanding(void)
4893 {
4894 return navGetCurrentStateFlags() & NAV_CTL_EMERG;
4895 }
4896
4897 bool navigationInAutomaticThrottleMode(void)
4898 {
4899 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4900 return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) ||
4901 ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle);
4902 }
4903
4904 bool navigationIsControllingThrottle(void)
4905 {
4906 // Note that this makes a detour into mixer code to evaluate actual motor status
4907 return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE);
4908 }
4909
4910 bool navigationIsControllingAltitude(void) {
4911 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4912 return (stateFlags & NAV_CTL_ALT);
4913 }
4914
4915 bool navigationIsFlyingAutonomousMode(void)
4916 {
4917 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4918 return (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
4919 }
4920
4921 bool navigationRTHAllowsLanding(void)
4922 {
4923 #ifdef USE_FW_AUTOLAND
4924 if (posControl.fwLandState.landAborted) {
4925 return false;
4926 }
4927 #endif
4928
4929 // WP mission RTH landing setting
4930 if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
4931 return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;
4932 }
4933
4934 // normal RTH landing setting
4935 navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
4936 return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
4937 (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
4938 }
4939
4940 bool isNavLaunchEnabled(void)
4941 {
4942 return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE);
4943 }
4944
4945 bool abortLaunchAllowed(void)
4946 {
4947 // allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
4948 return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
4949 }
4950
4951 int32_t navigationGetHomeHeading(void)
4952 {
4953 return posControl.rthState.homePosition.heading;
4954 }
4955
4956 // returns m/s
4957 float calculateAverageSpeed(void) {
4958 float flightTime = getFlightTime();
4959 if (flightTime == 0.0f) return 0;
4960 return (float)getTotalTravelDistance() / (flightTime * 100);
4961 }
4962
4963 const navigationPIDControllers_t* getNavigationPIDControllers(void) {
4964 return &posControl.pids;
4965 }
4966
4967 bool isAdjustingPosition(void) {
4968 return posControl.flags.isAdjustingPosition;
4969 }
4970
4971 bool isAdjustingHeading(void) {
4972 return posControl.flags.isAdjustingHeading;
4973 }
4974
4975 int32_t getCruiseHeadingAdjustment(void) {
4976 return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
4977 }
4978
4979 int32_t navigationGetHeadingError(void)
4980 {
4981 return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
4982 }
4983
4984 int8_t navCheckActiveAngleHoldAxis(void)
4985 {
4986 int8_t activeAxis = -1;
4987
4988 if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
4989 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
4990 bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
4991
4992 if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
4993 activeAxis = FD_PITCH;
4994 } else if (altholdActive) {
4995 activeAxis = FD_ROLL;
4996 }
4997 }
4998
4999 return activeAxis;
5000 }
5001
5002 uint8_t getActiveWpNumber(void)
5003 {
5004 return NAV_Status.activeWpNumber;
5005 }
5006
5007 #ifdef USE_FW_AUTOLAND
5008
5009 static void resetFwAutoland(void)
5010 {
5011 posControl.fwLandState.landAltAgl = 0;
5012 posControl.fwLandState.landAproachAltAgl = 0;
5013 posControl.fwLandState.loiterStartTime = 0;
5014 posControl.fwLandState.approachSettingIdx = 0;
5015 posControl.fwLandState.landPosHeading = -1;
5016 posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
5017 posControl.fwLandState.landWp = false;
5018 }
5019
5020 static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
5021 {
5022 if (approachHeading == 0) {
5023 return -1;
5024 }
5025
5026 int32_t windRelToRunway = wrap_36000(windAngle - ABS(approachHeading));
5027 // Headwind?
5028 if (windRelToRunway >= 27000 || windRelToRunway <= 9000) {
5029 return wrap_36000(ABS(approachHeading));
5030 }
5031
5032 if (approachHeading > 0) {
5033 return wrap_36000(approachHeading + 18000);
5034 }
5035
5036 return -1;
5037 }
5038
5039 static float getLandAltitude(void)
5040 {
5041 float altitude = -1;
5042 #ifdef USE_RANGEFINDER
5043 if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
5044 altitude = rangefinderGetLatestAltitude();
5045 }
5046 else
5047 #endif
5048 if (posControl.flags.estAglStatus >= EST_USABLE) {
5049 altitude = posControl.actualState.agl.pos.z;
5050 } else {
5051 altitude = posControl.actualState.abs.pos.z;
5052 }
5053 return altitude;
5054 }
5055
5056 static int32_t calcWindDiff(int32_t heading, int32_t windHeading)
5057 {
5058 return ABS(wrap_18000(windHeading - heading));
5059 }
5060
5061 static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
5062 {
5063 calculateAndSetActiveWaypointToLocalPosition(pos);
5064
5065 if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) {
5066 int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos);
5067 posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
5068 } else {
5069 posControl.activeWaypoint.nextTurnAngle = -1;
5070 }
5071
5072 posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
5073 posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
5074 posControl.wpAltitudeReached = false;
5075 }
5076
5077 void resetFwAutolandApproach(int8_t idx)
5078 {
5079 if (idx >= 0 && idx < MAX_FW_LAND_APPOACH_SETTINGS) {
5080 memset(fwAutolandApproachConfigMutable(idx), 0, sizeof(navFwAutolandApproach_t));
5081 } else {
5082 memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
5083 }
5084 }
5085
5086 bool canFwLandingBeCancelled(void)
5087 {
5088 return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
5089 }
5090 #endif
5091 uint16_t getFlownLoiterRadius(void)
5092 {
5093 if (STATE(AIRPLANE) && navGetCurrentStateFlags() & NAV_CTL_HOLD) {
5094 return CENTIMETERS_TO_METERS(calculateDistanceToDestination(&posControl.desiredState.pos));
5095 }
5096
5097 return 0;
5098 }
INAV: Navigation-enabled flight control software