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