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