?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge remote-tracking branch 'upstream/master' into abo_RTH_sanity_fix
[inav.git] / src / main / navigation / navigation.c
blob6e789cf788ff3d422bcd34d22a33c7b90685c07c
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/rc_controls.h"
40 #include "fc/rc_modes.h"
41 #include "fc/runtime_config.h"
42 #ifdef USE_MULTI_MISSION
43 #include "fc/cli.h"
44 #endif
45 #include "fc/settings.h"
46
47 #include "flight/imu.h"
48 #include "flight/mixer.h"
49 #include "flight/pid.h"
50
51 #include "io/beeper.h"
52 #include "io/gps.h"
53
54 #include "navigation/navigation.h"
55 #include "navigation/navigation_private.h"
56
57 #include "rx/rx.h"
58
59 #include "sensors/sensors.h"
60 #include "sensors/acceleration.h"
61 #include "sensors/boardalignment.h"
62
63
64 // Multirotors:
65 #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)
66 #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
67 #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set
68 // Planes:
69 #define FW_RTH_CLIMB_OVERSHOOT_CM 100
70 #define FW_RTH_CLIMB_MARGIN_MIN_CM 100
71 #define FW_RTH_CLIMB_MARGIN_PERCENT 15
72
73 /*-----------------------------------------------------------
74 * Compatibility for home position
75 *-----------------------------------------------------------*/
76 gpsLocation_t GPS_home;
77 uint32_t GPS_distanceToHome; // distance to home point in meters
78 int16_t GPS_directionToHome; // direction to home point in degrees
79
80 fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
81
82 radar_pois_t radar_pois[RADAR_MAX_POIS];
83 #if defined(USE_SAFE_HOME)
84 int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
85 uint32_t safehome_distance = 0; // distance to the nearest safehome
86 fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming
87 bool safehome_applied = false; // whether the safehome has been applied to home.
88
89 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
90
91 #endif
92
93 // waypoint 254, 255 are special waypoints
94 STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
95
96 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
97 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1);
98 #endif
99
100 PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);
101
102 PG_RESET_TEMPLATE(navConfig_t, navConfig,
103 .general = {
104
105 .flags = {
106 .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT,
107 .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
108 .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
109 .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
110 .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT, // Climb first, turn after reaching safe altitude
111 .rth_climb_first_stage_mode = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_MODE_DEFAULT, // To determine how rth_climb_first_stage_altitude is used
112 .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT, // Ignore GPS loss on initial climb
113 .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT,
114 .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT,
115 .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
116 .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
117 .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
118 .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
119 .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
120 .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
121 .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
122 },
123
124 // General navigation parameters
125 .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
126 .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
127 .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
128 #ifdef USE_MULTI_MISSION
129 .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
130 #endif
131 .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
132 .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
133 .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
134 .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
135 .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
136 .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
137 .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
138 .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
139 .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
140 .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s
141 .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s
142 .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately
143 .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters
144 .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT, // altitude in centimeters
145 .rth_climb_first_stage_altitude = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_ALTITUDE_DEFAULT, // altitude in centimetres, 0= off
146 .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT, // centimeters - 500m should be safe for all aircraft
147 .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT, // max altitude in centimeters in terrain following mode
148 .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point
149 .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
150 },
151
152 // MC-specific
153 .mc = {
154 .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
155 .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
156
157 #ifdef USE_MR_BRAKING_MODE
158 .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
159 .braking_disengage_speed = SETTING_NAV_MC_BRAKING_DISENGAGE_SPEED_DEFAULT, // Stop when speed goes below 0.75m/s
160 .braking_timeout = SETTING_NAV_MC_BRAKING_TIMEOUT_DEFAULT, // Timeout barking after 2s
161 .braking_boost_factor = SETTING_NAV_MC_BRAKING_BOOST_FACTOR_DEFAULT, // A 100% boost by default
162 .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms
163 .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s
164 .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s
165 .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle
166 #endif
167
168 .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
169 .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
170 .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
171 },
172
173 // Fixed wing
174 .fw = {
175 .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
176 .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
177 .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
178 .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
179 .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
180 .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
181 .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
182 .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
183
184 //Fixed wing landing
185 .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
186
187 // Fixed wing launch
188 .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
189 .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
190 .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
191 .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
192 .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
193 .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
194 .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
195 .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
196 .launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
197 .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
198 .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
199 .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
200 .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
201 .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
202 .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
203 .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
204 .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
205 }
206 );
207
208 static navWapointHeading_t wpHeadingControl;
209 navigationPosControl_t posControl;
210 navSystemStatus_t NAV_Status;
211 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
212
213 // Blackbox states
214 int16_t navCurrentState;
215 int16_t navActualVelocity[3];
216 int16_t navDesiredVelocity[3];
217 int16_t navActualHeading;
218 int16_t navDesiredHeading;
219 int32_t navTargetPosition[3];
220 int32_t navLatestActualPosition[3];
221 int16_t navActualSurface;
222 uint16_t navFlags;
223 uint16_t navEPH;
224 uint16_t navEPV;
225 int16_t navAccNEU[3];
226 //End of blackbox states
227
228 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
229 static void updateDesiredRTHAltitude(void);
230 static void resetAltitudeController(bool useTerrainFollowing);
231 static void resetPositionController(void);
232 static void setupAltitudeController(void);
233 static void resetHeadingController(void);
234 void resetGCSFlags(void);
235
236 static void setupJumpCounters(void);
237 static void resetJumpCounter(void);
238 static void clearJumpCounters(void);
239
240 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
241 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
242 void calculateInitialHoldPosition(fpVector3_t * pos);
243 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
244 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
245 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
246 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
247 static navigationFSMEvent_t nextForNonGeoStates(void);
248 static bool isWaypointMissionValid(void);
249 void missionPlannerSetWaypoint(void);
250
251 void initializeRTHSanityChecker(void);
252 bool validateRTHSanityChecker(void);
253 void updateHomePosition(void);
254
255 static bool rthAltControlStickOverrideCheck(unsigned axis);
256
257 /*************************************************************************************************/
258 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
259 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
260 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
261 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
262 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
263 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState);
264 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState);
265 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState);
266 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState);
267 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState);
268 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState);
269 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
270 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
271 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
272 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
273 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
274 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
275 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
276 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
277 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
278 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
279 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
280 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
281 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
282 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
283 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
284 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
285 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
286 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
287 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
288 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
289 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
290 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
291
292 static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
293 /** Idle state ******************************************************/
294 [NAV_STATE_IDLE] = {
295 .persistentId = NAV_PERSISTENT_ID_IDLE,
296 .onEntry = navOnEnteringState_NAV_STATE_IDLE,
297 .timeoutMs = 0,
298 .stateFlags = 0,
299 .mapToFlightModes = 0,
300 .mwState = MW_NAV_STATE_NONE,
301 .mwError = MW_NAV_ERROR_NONE,
302 .onEvent = {
303 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
304 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
305 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
306 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
307 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
308 [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
309 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
310 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
311 }
312 },
313
314 /** ALTHOLD mode ***************************************************/
315 [NAV_STATE_ALTHOLD_INITIALIZE] = {
316 .persistentId = NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE,
317 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
318 .timeoutMs = 0,
319 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
320 .mapToFlightModes = NAV_ALTHOLD_MODE,
321 .mwState = MW_NAV_STATE_NONE,
322 .mwError = MW_NAV_ERROR_NONE,
323 .onEvent = {
324 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS,
325 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
326 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
327 }
328 },
329
330 [NAV_STATE_ALTHOLD_IN_PROGRESS] = {
331 .persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS,
332 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
333 .timeoutMs = 10,
334 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
335 .mapToFlightModes = NAV_ALTHOLD_MODE,
336 .mwState = MW_NAV_STATE_NONE,
337 .mwError = MW_NAV_ERROR_NONE,
338 .onEvent = {
339 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
340 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
341 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
342 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
343 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
344 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
345 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
346 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
347 }
348 },
349
350 /** POSHOLD_3D mode ************************************************/
351 [NAV_STATE_POSHOLD_3D_INITIALIZE] = {
352 .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
353 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
354 .timeoutMs = 0,
355 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
356 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
357 .mwState = MW_NAV_STATE_HOLD_INFINIT,
358 .mwError = MW_NAV_ERROR_NONE,
359 .onEvent = {
360 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS,
361 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
362 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
363 }
364 },
365
366 [NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
367 .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
368 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
369 .timeoutMs = 10,
370 .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,
371 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
372 .mwState = MW_NAV_STATE_HOLD_INFINIT,
373 .mwError = MW_NAV_ERROR_NONE,
374 .onEvent = {
375 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
376 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
377 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
378 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
379 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
380 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
381 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
382 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
383 }
384 },
385 /** CRUISE_HOLD mode ************************************************/
386 [NAV_STATE_COURSE_HOLD_INITIALIZE] = {
387 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE,
388 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE,
389 .timeoutMs = 0,
390 .stateFlags = NAV_REQUIRE_ANGLE,
391 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
392 .mwState = MW_NAV_STATE_NONE,
393 .mwError = MW_NAV_ERROR_NONE,
394 .onEvent = {
395 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
396 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
397 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
398 }
399 },
400
401 [NAV_STATE_COURSE_HOLD_IN_PROGRESS] = {
402 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
403 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
404 .timeoutMs = 10,
405 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
406 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
407 .mwState = MW_NAV_STATE_NONE,
408 .mwError = MW_NAV_ERROR_NONE,
409 .onEvent = {
410 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state
411 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
412 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
413 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
414 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
415 [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING,
416 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
417 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
418 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
419 }
420 },
421
422 [NAV_STATE_COURSE_HOLD_ADJUSTING] = {
423 .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING,
424 .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING,
425 .timeoutMs = 10,
426 .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
427 .mapToFlightModes = NAV_COURSE_HOLD_MODE,
428 .mwState = MW_NAV_STATE_NONE,
429 .mwError = MW_NAV_ERROR_NONE,
430 .onEvent = {
431 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
432 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING,
433 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
434 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
435 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
436 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
437 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
438 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
439 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
440 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
441 }
442 },
443
444 /** CRUISE_3D mode ************************************************/
445 [NAV_STATE_CRUISE_INITIALIZE] = {
446 .persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE,
447 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE,
448 .timeoutMs = 0,
449 .stateFlags = NAV_REQUIRE_ANGLE,
450 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
451 .mwState = MW_NAV_STATE_NONE,
452 .mwError = MW_NAV_ERROR_NONE,
453 .onEvent = {
454 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
455 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
456 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
457 }
458 },
459
460 [NAV_STATE_CRUISE_IN_PROGRESS] = {
461 .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
462 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
463 .timeoutMs = 10,
464 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
465 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
466 .mwState = MW_NAV_STATE_NONE,
467 .mwError = MW_NAV_ERROR_NONE,
468 .onEvent = {
469 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state
470 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
471 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
472 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
473 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
474 [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING,
475 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
476 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
477 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
478 }
479 },
480
481 [NAV_STATE_CRUISE_ADJUSTING] = {
482 .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING,
483 .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING,
484 .timeoutMs = 10,
485 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
486 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
487 .mwState = MW_NAV_STATE_NONE,
488 .mwError = MW_NAV_ERROR_NONE,
489 .onEvent = {
490 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
491 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING,
492 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
493 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
494 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
495 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
496 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
497 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
498 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
499 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
500 }
501 },
502
503 /** RTH_3D mode ************************************************/
504 [NAV_STATE_RTH_INITIALIZE] = {
505 .persistentId = NAV_PERSISTENT_ID_RTH_INITIALIZE,
506 .onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
507 .timeoutMs = 10,
508 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
509 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
510 .mwState = MW_NAV_STATE_RTH_START,
511 .mwError = MW_NAV_ERROR_NONE,
512 .onEvent = {
513 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state
514 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
515 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
516 [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
517 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
518 }
519 },
520
521 [NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = {
522 .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
523 .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
524 .timeoutMs = 10,
525 .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
526 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
527 .mwState = MW_NAV_STATE_RTH_CLIMB,
528 .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
529 .onEvent = {
530 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state
531 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME,
532 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
533 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
534 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
535 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
536 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
537 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
538 }
539 },
540
541 [NAV_STATE_RTH_HEAD_HOME] = {
542 .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
543 .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
544 .timeoutMs = 10,
545 .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,
546 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
547 .mwState = MW_NAV_STATE_RTH_ENROUTE,
548 .mwError = MW_NAV_ERROR_NONE,
549 .onEvent = {
550 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state
551 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
552 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
553 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
554 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
555 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
556 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
557 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
558 }
559 },
560
561 [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
562 .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
563 .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
564 .timeoutMs = 500,
565 .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,
566 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
567 .mwState = MW_NAV_STATE_LAND_SETTLE,
568 .mwError = MW_NAV_ERROR_NONE,
569 .onEvent = {
570 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
571 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
572 [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
573 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
574 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
575 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
576 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
577 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
578 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
579 }
580 },
581
582 [NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
583 .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
584 .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
585 .timeoutMs = 10,
586 .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,
587 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
588 .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
589 .mwError = MW_NAV_ERROR_NONE,
590 .onEvent = {
591 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
592 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
593 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
594 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
595 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
596 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
597 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
598 }
599 },
600
601 [NAV_STATE_RTH_LANDING] = {
602 .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
603 .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
604 .timeoutMs = 10,
605 .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,
606 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
607 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
608 .mwError = MW_NAV_ERROR_LANDING,
609 .onEvent = {
610 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state
611 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING,
612 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
613 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
614 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
615 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
616 }
617 },
618
619 [NAV_STATE_RTH_FINISHING] = {
620 .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
621 .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
622 .timeoutMs = 0,
623 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
624 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
625 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
626 .mwError = MW_NAV_ERROR_LANDING,
627 .onEvent = {
628 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED,
629 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
630 }
631 },
632
633 [NAV_STATE_RTH_FINISHED] = {
634 .persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
635 .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
636 .timeoutMs = 10,
637 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
638 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
639 .mwState = MW_NAV_STATE_LANDED,
640 .mwError = MW_NAV_ERROR_NONE,
641 .onEvent = {
642 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
643 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
644 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
645 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
646 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
647 }
648 },
649
650 /** WAYPOINT mode ************************************************/
651 [NAV_STATE_WAYPOINT_INITIALIZE] = {
652 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE,
653 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
654 .timeoutMs = 0,
655 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
656 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
657 .mwState = MW_NAV_STATE_PROCESS_NEXT,
658 .mwError = MW_NAV_ERROR_NONE,
659 .onEvent = {
660 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
661 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
662 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
663 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
664 }
665 },
666
667 [NAV_STATE_WAYPOINT_PRE_ACTION] = {
668 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
669 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
670 .timeoutMs = 10,
671 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
672 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
673 .mwState = MW_NAV_STATE_PROCESS_NEXT,
674 .mwError = MW_NAV_ERROR_NONE,
675 .onEvent = {
676 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
677 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
678 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
679 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
680 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
681 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
682 }
683 },
684
685 [NAV_STATE_WAYPOINT_IN_PROGRESS] = {
686 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS,
687 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
688 .timeoutMs = 10,
689 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
690 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
691 .mwState = MW_NAV_STATE_WP_ENROUTE,
692 .mwError = MW_NAV_ERROR_NONE,
693 .onEvent = {
694 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state
695 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
696 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
697 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
698 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
699 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
700 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
701 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
702 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
703 }
704 },
705
706 [NAV_STATE_WAYPOINT_REACHED] = {
707 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED,
708 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
709 .timeoutMs = 10,
710 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
711 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
712 .mwState = MW_NAV_STATE_PROCESS_NEXT,
713 .mwError = MW_NAV_ERROR_NONE,
714 .onEvent = {
715 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
716 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
717 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
718 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
719 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
720 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
721 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
722 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
723 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
724 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
725 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
726 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
727 }
728 },
729
730 [NAV_STATE_WAYPOINT_HOLD_TIME] = {
731 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
732 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
733 .timeoutMs = 10,
734 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
735 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
736 .mwState = MW_NAV_STATE_HOLD_TIMED,
737 .mwError = MW_NAV_ERROR_NONE,
738 .onEvent = {
739 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state
740 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint
741 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
742 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
743 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
744 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
745 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
746 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
747 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
748 }
749 },
750
751 [NAV_STATE_WAYPOINT_RTH_LAND] = {
752 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
753 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
754 .timeoutMs = 10,
755 .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,
756 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
757 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
758 .mwError = MW_NAV_ERROR_LANDING,
759 .onEvent = {
760 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state
761 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
762 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
763 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
764 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
765 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
766 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
767 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
768 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
769 }
770 },
771
772 [NAV_STATE_WAYPOINT_NEXT] = {
773 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT,
774 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT,
775 .timeoutMs = 0,
776 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
777 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
778 .mwState = MW_NAV_STATE_PROCESS_NEXT,
779 .mwError = MW_NAV_ERROR_NONE,
780 .onEvent = {
781 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
782 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
783 }
784 },
785
786 [NAV_STATE_WAYPOINT_FINISHED] = {
787 .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
788 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
789 .timeoutMs = 10,
790 .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,
791 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
792 .mwState = MW_NAV_STATE_WP_ENROUTE,
793 .mwError = MW_NAV_ERROR_FINISH,
794 .onEvent = {
795 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, // re-process state
796 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
797 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
798 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
799 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
800 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
801 [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
802 [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
803 }
804 },
805
806 /** EMERGENCY LANDING ************************************************/
807 [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
808 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
809 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
810 .timeoutMs = 0,
811 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
812 .mapToFlightModes = 0,
813 .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
814 .mwError = MW_NAV_ERROR_LANDING,
815 .onEvent = {
816 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
817 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
818 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
819 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
820 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
821 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
822 }
823 },
824
825 [NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
826 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
827 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
828 .timeoutMs = 10,
829 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
830 .mapToFlightModes = 0,
831 .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
832 .mwError = MW_NAV_ERROR_LANDING,
833 .onEvent = {
834 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state
835 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
836 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
837 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
838 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
839 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
840 }
841 },
842
843 [NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
844 .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
845 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
846 .timeoutMs = 10,
847 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
848 .mapToFlightModes = 0,
849 .mwState = MW_NAV_STATE_LANDED,
850 .mwError = MW_NAV_ERROR_LANDING,
851 .onEvent = {
852 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
853 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
854 }
855 },
856
857 [NAV_STATE_LAUNCH_INITIALIZE] = {
858 .persistentId = NAV_PERSISTENT_ID_LAUNCH_INITIALIZE,
859 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
860 .timeoutMs = 0,
861 .stateFlags = NAV_REQUIRE_ANGLE,
862 .mapToFlightModes = NAV_LAUNCH_MODE,
863 .mwState = MW_NAV_STATE_NONE,
864 .mwError = MW_NAV_ERROR_NONE,
865 .onEvent = {
866 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT,
867 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
868 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
869 }
870 },
871
872 [NAV_STATE_LAUNCH_WAIT] = {
873 .persistentId = NAV_PERSISTENT_ID_LAUNCH_WAIT,
874 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
875 .timeoutMs = 10,
876 .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
877 .mapToFlightModes = NAV_LAUNCH_MODE,
878 .mwState = MW_NAV_STATE_NONE,
879 .mwError = MW_NAV_ERROR_NONE,
880 .onEvent = {
881 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state
882 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS,
883 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
884 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
885 }
886 },
887
888 [NAV_STATE_LAUNCH_IN_PROGRESS] = {
889 .persistentId = NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS,
890 .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
891 .timeoutMs = 10,
892 .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
893 .mapToFlightModes = NAV_LAUNCH_MODE,
894 .mwState = MW_NAV_STATE_NONE,
895 .mwError = MW_NAV_ERROR_NONE,
896 .onEvent = {
897 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state
898 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
899 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
900 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
901 }
902 },
903 };
904
905 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
906 {
907 return navFSM[state].stateFlags;
908 }
909
910 static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
911 {
912 return navFSM[state].mapToFlightModes;
913 }
914
915 navigationFSMStateFlags_t navGetCurrentStateFlags(void)
916 {
917 return navGetStateFlags(posControl.navState);
918 }
919
920 static bool navTerrainFollowingRequested(void)
921 {
922 // Terrain following not supported on FIXED WING aircraft yet
923 return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
924 }
925
926 /*************************************************************************************************/
927 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
928 {
929 UNUSED(previousState);
930
931 resetAltitudeController(false);
932 resetHeadingController();
933 resetPositionController();
934
935 return NAV_FSM_EVENT_NONE;
936 }
937
938 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
939 {
940 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
941 const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
942
943 resetGCSFlags();
944
945 // If surface tracking mode changed value - reset altitude controller
946 if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
947 resetAltitudeController(navTerrainFollowingRequested());
948 }
949
950 if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
951 setupAltitudeController();
952 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
953 }
954
955 return NAV_FSM_EVENT_SUCCESS;
956 }
957
958 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState)
959 {
960 UNUSED(previousState);
961
962 // If GCS was disabled - reset altitude setpoint
963 if (posControl.flags.isGCSAssistedNavigationReset) {
964 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
965 resetGCSFlags();
966 }
967
968 return NAV_FSM_EVENT_NONE;
969 }
970
971 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
972 {
973 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
974 const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
975
976 resetGCSFlags();
977
978 if ((prevFlags & NAV_CTL_POS) == 0) {
979 resetPositionController();
980 }
981
982 if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
983 resetAltitudeController(navTerrainFollowingRequested());
984 setupAltitudeController();
985 }
986
987 if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
988 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
989 }
990
991 if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
992 fpVector3_t targetHoldPos;
993 calculateInitialHoldPosition(&targetHoldPos);
994 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
995 }
996
997 return NAV_FSM_EVENT_SUCCESS;
998 }
999
1000 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState)
1001 {
1002 UNUSED(previousState);
1003
1004 // If GCS was disabled - reset 2D pos setpoint
1005 if (posControl.flags.isGCSAssistedNavigationReset) {
1006 fpVector3_t targetHoldPos;
1007 calculateInitialHoldPosition(&targetHoldPos);
1008 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
1009 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1010 resetGCSFlags();
1011 }
1012
1013 return NAV_FSM_EVENT_NONE;
1014 }
1015 /////////////////
1016
1017 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
1018 {
1019 const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1020
1021 if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
1022
1023 DEBUG_SET(DEBUG_CRUISE, 0, 1);
1024 if (checkForPositionSensorTimeout()) {
1025 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1026 } // Switch to IDLE if we do not have an healty position. Try the next iteration.
1027
1028 if (!(prevFlags & NAV_CTL_POS)) {
1029 resetPositionController();
1030 }
1031
1032 posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
1033 posControl.cruise.previousYaw = posControl.cruise.yaw;
1034 posControl.cruise.lastYawAdjustmentTime = 0;
1035
1036 return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
1037 }
1038
1039 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
1040 {
1041 const timeMs_t currentTimeMs = millis();
1042
1043 if (checkForPositionSensorTimeout()) {
1044 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1045 } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
1046
1047 DEBUG_SET(DEBUG_CRUISE, 0, 2);
1048 DEBUG_SET(DEBUG_CRUISE, 2, 0);
1049
1050 if (posControl.flags.isAdjustingPosition) {
1051 return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
1052 }
1053
1054 // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
1055 if (posControl.flags.isAdjustingHeading) {
1056 timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
1057 if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
1058 float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
1059 float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
1060 posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
1061 DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
1062 posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
1063 }
1064
1065 if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
1066 posControl.cruise.previousYaw = posControl.cruise.yaw;
1067
1068 uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
1069
1070 if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
1071 || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
1072 || posControl.flags.isAdjustingHeading) {
1073 calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
1074 DEBUG_SET(DEBUG_CRUISE, 2, 1);
1075 } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
1076 calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
1077 DEBUG_SET(DEBUG_CRUISE, 2, 2);
1078 }
1079
1080 setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
1081
1082 return NAV_FSM_EVENT_NONE;
1083 }
1084
1085 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState)
1086 {
1087 UNUSED(previousState);
1088 DEBUG_SET(DEBUG_CRUISE, 0, 3);
1089
1090 // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
1091 if (posControl.flags.isAdjustingPosition) {
1092 posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
1093 posControl.cruise.lastYawAdjustmentTime = millis();
1094 return NAV_FSM_EVENT_NONE; // reprocess the state
1095 }
1096
1097 resetPositionController();
1098
1099 return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
1100 }
1101
1102 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
1103 {
1104 if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
1105
1106 navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
1107
1108 return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);
1109 }
1110
1111 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState)
1112 {
1113 navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1114
1115 return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState);
1116 }
1117
1118 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState)
1119 {
1120 navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
1121
1122 return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState);
1123 }
1124
1125 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
1126 {
1127 navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
1128
1129 if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
1130 // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
1131 // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
1132 // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
1133 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1134 }
1135
1136 if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
1137 // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
1138 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1139 }
1140
1141 // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
1142 if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
1143 // Reset altitude and position controllers if necessary
1144 if ((prevFlags & NAV_CTL_POS) == 0) {
1145 resetPositionController();
1146 }
1147
1148 // Reset altitude controller if it was not enabled or if we are in terrain follow mode
1149 if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
1150 // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
1151 resetAltitudeController(false);
1152 setupAltitudeController();
1153 }
1154
1155 // If close to home - reset home position and land
1156 if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
1157 setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
1158 setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1159
1160 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1161 }
1162 else {
1163 fpVector3_t targetHoldPos;
1164
1165 if (STATE(FIXED_WING_LEGACY)) {
1166 // Airplane - climbout before heading home
1167 if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
1168 // Spiral climb centered at xy of RTH activation
1169 calculateInitialHoldPosition(&targetHoldPos);
1170 } else {
1171 calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb
1172 }
1173 } else {
1174 // Multicopter, hover and climb
1175 calculateInitialHoldPosition(&targetHoldPos);
1176
1177 // Initialize RTH sanity check to prevent fly-aways on RTH
1178 // For airplanes this is delayed until climb-out is finished
1179 initializeRTHSanityChecker();
1180 }
1181
1182 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
1183
1184 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
1185 }
1186 }
1187 /* Position sensor failure timeout - land. Land immediately if failsafe RTH and timeout disabled (set to 0) */
1188 else if (checkForPositionSensorTimeout() || (!navConfig()->general.pos_failure_timeout && posControl.flags.forcedRTHActivated)) {
1189 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1190 }
1191 /* No valid POS sensor but still within valid timeout - wait */
1192 return NAV_FSM_EVENT_NONE;
1193 }
1194
1195 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
1196 {
1197 UNUSED(previousState);
1198
1199 if (!STATE(ALTITUDE_CONTROL)) {
1200 //If altitude control is not a thing, switch to RTH in progress instead
1201 return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
1202 }
1203
1204 rthAltControlStickOverrideCheck(PITCH);
1205
1206 /* Position sensor failure timeout and not configured to ignore GPS loss - land */
1207 if ((posControl.flags.estHeadingStatus == EST_NONE) ||
1208 (checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) {
1209 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1210 }
1211
1212 const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
1213 const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
1214
1215 // If we reached desired initial RTH altitude or we don't want to climb first
1216 if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
1217
1218 // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
1219 if (STATE(FIXED_WING_LEGACY)) {
1220 initializeRTHSanityChecker();
1221 }
1222
1223 // Save initial home distance for future use
1224 posControl.rthState.rthInitialDistance = posControl.homeDistance;
1225 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1226
1227 if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
1228 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1229 }
1230 else {
1231 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1232 }
1233
1234 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
1235
1236 } else {
1237
1238 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
1239
1240 /* For multi-rotors execute sanity check during initial ascent as well */
1241 if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
1242 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1243 }
1244
1245 // Climb to safe altitude and turn to correct direction
1246 if (STATE(FIXED_WING_LEGACY)) {
1247 if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
1248 float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
1249 updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
1250 } else {
1251 tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
1252 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1253 }
1254 } else {
1255 // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
1256 // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
1257 tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
1258
1259 if (navConfig()->general.flags.rth_tail_first) {
1260 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
1261 } else {
1262 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1263 }
1264 }
1265
1266 return NAV_FSM_EVENT_NONE;
1267 }
1268 }
1269
1270 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
1271 {
1272 UNUSED(previousState);
1273
1274 rthAltControlStickOverrideCheck(PITCH);
1275
1276 /* If position sensors unavailable - land immediately */
1277 if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) {
1278 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1279 }
1280
1281 // If we have position sensor - continue home
1282 if ((posControl.flags.estPosStatus >= EST_USABLE)) {
1283 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
1284
1285 if (isWaypointPositionReached(tmpHomePos, true)) {
1286 // Successfully reached position target - update XYZ-position
1287 setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1288 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
1289 } else {
1290 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
1291 return NAV_FSM_EVENT_NONE;
1292 }
1293 }
1294 /* Position sensor failure timeout - land */
1295 else if (checkForPositionSensorTimeout()) {
1296 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1297 }
1298 /* No valid POS sensor but still within valid timeout - wait */
1299 return NAV_FSM_EVENT_NONE;
1300 }
1301
1302 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
1303 {
1304 UNUSED(previousState);
1305
1306 //On ROVER and BOAT we immediately switch to the next event
1307 if (!STATE(ALTITUDE_CONTROL)) {
1308 return NAV_FSM_EVENT_SUCCESS;
1309 }
1310
1311 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1312 if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1313 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1314 }
1315
1316 // If position ok OR within valid timeout - continue
1317 // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
1318 if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
1319 resetLandingDetector();
1320 updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
1321 return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
1322 } else {
1323 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
1324 setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1325 return NAV_FSM_EVENT_NONE;
1326 }
1327 }
1328
1329 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
1330 {
1331 UNUSED(previousState);
1332
1333 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1334 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1335 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1336 }
1337
1338 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
1339
1340 if (navConfig()->general.rth_home_altitude) {
1341 float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
1342
1343 if (timeToReachHomeAltitude < 1) {
1344 // we almost reached the target home altitude so set the desired altitude to the desired home altitude
1345 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1346 } else {
1347 float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
1348 updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
1349 }
1350 }
1351 else {
1352 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
1353 }
1354
1355 return NAV_FSM_EVENT_NONE;
1356 }
1357
1358 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
1359 {
1360 UNUSED(previousState);
1361
1362 //On ROVER and BOAT we immediately switch to the next event
1363 if (!STATE(ALTITUDE_CONTROL)) {
1364 return NAV_FSM_EVENT_SUCCESS;
1365 }
1366
1367 if (!ARMING_FLAG(ARMED) || isLandingDetected()) {
1368 return NAV_FSM_EVENT_SUCCESS;
1369 }
1370
1371 /* If position sensors unavailable - land immediately (wait for timeout on GPS)
1372 * Continue to check for RTH sanity during landing */
1373 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
1374 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1375 }
1376
1377 float descentVelLimited = 0;
1378
1379 // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
1380 if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
1381 // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
1382 // Do not allow descent velocity slower than -30cm/s so the landing detector works.
1383 descentVelLimited = navConfig()->general.land_minalt_vspd;
1384 } else {
1385 // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
1386 float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
1387 navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
1388 navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1389
1390 descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
1391 }
1392
1393 updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
1394
1395 return NAV_FSM_EVENT_NONE;
1396 }
1397
1398 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState)
1399 {
1400 UNUSED(previousState);
1401
1402 //On ROVER and BOAT disarm immediately
1403 if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
1404 disarm(DISARM_NAVIGATION);
1405 }
1406
1407 return NAV_FSM_EVENT_SUCCESS;
1408 }
1409
1410 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState)
1411 {
1412 // Stay in this state
1413 UNUSED(previousState);
1414
1415 if (STATE(ALTITUDE_CONTROL)) {
1416 updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME
1417 }
1418
1419 // Prevent I-terms growing when already landed
1420 pidResetErrorAccumulators();
1421 return NAV_FSM_EVENT_NONE;
1422 }
1423
1424 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState)
1425 {
1426 UNUSED(previousState);
1427
1428 if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
1429 return NAV_FSM_EVENT_ERROR;
1430 }
1431 else {
1432 // Prepare controllers
1433 resetPositionController();
1434
1435 // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
1436 resetAltitudeController(false);
1437 setupAltitudeController();
1438 /*
1439 Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
1440 Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
1441 */
1442 if (posControl.activeWaypointIndex == 0 || posControl.wpMissionRestart) {
1443 setupJumpCounters();
1444 posControl.activeWaypointIndex = 0;
1445 wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1446 }
1447
1448 if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
1449 posControl.wpMissionRestart = posControl.activeWaypointIndex ? !posControl.wpMissionRestart : false;
1450 } else {
1451 posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
1452 }
1453
1454 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1455 }
1456 }
1457
1458 static navigationFSMEvent_t nextForNonGeoStates(void)
1459 {
1460 /* simple helper for non-geographical states that just set other data */
1461 const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
1462
1463 if (isLastWaypoint) {
1464 // non-geo state is the last waypoint, switch to finish.
1465 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1466 } else {
1467 // Finished non-geo, move to next WP
1468 posControl.activeWaypointIndex++;
1469 return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1470 }
1471 }
1472
1473 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
1474 {
1475 /* A helper function to do waypoint-specific action */
1476 UNUSED(previousState);
1477
1478 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1479 case NAV_WP_ACTION_HOLD_TIME:
1480 case NAV_WP_ACTION_WAYPOINT:
1481 case NAV_WP_ACTION_LAND:
1482 calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
1483 posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
1484 posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
1485 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1486
1487 // We use p3 as the volatile jump counter (p2 is the static value)
1488 case NAV_WP_ACTION_JUMP:
1489 if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
1490 if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
1491 resetJumpCounter();
1492 return nextForNonGeoStates();
1493 }
1494 else
1495 {
1496 posControl.waypointList[posControl.activeWaypointIndex].p3--;
1497 }
1498 }
1499 posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
1500 return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
1501
1502 case NAV_WP_ACTION_SET_POI:
1503 if (STATE(MULTIROTOR)) {
1504 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
1505 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
1506 &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
1507 }
1508 return nextForNonGeoStates();
1509
1510 case NAV_WP_ACTION_SET_HEAD:
1511 if (STATE(MULTIROTOR)) {
1512 if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
1513 posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
1514 wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
1515 } else {
1516 wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
1517 wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
1518 }
1519 }
1520 return nextForNonGeoStates();
1521
1522 case NAV_WP_ACTION_RTH:
1523 posControl.wpMissionRestart = true;
1524 return NAV_FSM_EVENT_SWITCH_TO_RTH;
1525 };
1526
1527 UNREACHABLE();
1528 }
1529
1530 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
1531 {
1532 UNUSED(previousState);
1533
1534 // If no position sensor available - land immediately
1535 if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
1536 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1537 case NAV_WP_ACTION_HOLD_TIME:
1538 case NAV_WP_ACTION_WAYPOINT:
1539 case NAV_WP_ACTION_LAND:
1540 if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
1541 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
1542 }
1543 else {
1544 fpVector3_t tmpWaypoint;
1545 tmpWaypoint.x = posControl.activeWaypoint.pos.x;
1546 tmpWaypoint.y = posControl.activeWaypoint.pos.y;
1547 tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
1548 posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
1549 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
1550 setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
1551 if(STATE(MULTIROTOR)) {
1552 switch (wpHeadingControl.mode) {
1553 case NAV_WP_HEAD_MODE_NONE:
1554 break;
1555 case NAV_WP_HEAD_MODE_FIXED:
1556 setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
1557 break;
1558 case NAV_WP_HEAD_MODE_POI:
1559 setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
1560 break;
1561 }
1562 }
1563 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1564 }
1565 break;
1566
1567 case NAV_WP_ACTION_JUMP:
1568 case NAV_WP_ACTION_SET_HEAD:
1569 case NAV_WP_ACTION_SET_POI:
1570 case NAV_WP_ACTION_RTH:
1571 UNREACHABLE();
1572 }
1573 }
1574 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1575 else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
1576 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1577 }
1578
1579 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1580 }
1581
1582 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
1583 {
1584 UNUSED(previousState);
1585
1586 switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
1587 case NAV_WP_ACTION_WAYPOINT:
1588 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
1589
1590 case NAV_WP_ACTION_JUMP:
1591 case NAV_WP_ACTION_SET_HEAD:
1592 case NAV_WP_ACTION_SET_POI:
1593 case NAV_WP_ACTION_RTH:
1594 UNREACHABLE();
1595
1596 case NAV_WP_ACTION_LAND:
1597 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
1598
1599 case NAV_WP_ACTION_HOLD_TIME:
1600 // Save the current time for the time the waypoint was reached
1601 posControl.wpReachedTime = millis();
1602 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
1603 }
1604
1605 UNREACHABLE();
1606 }
1607
1608 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
1609 {
1610 UNUSED(previousState);
1611
1612 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1613 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
1614 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1615 }
1616
1617 timeMs_t currentTime = millis();
1618
1619 if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
1620 (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
1621 return NAV_FSM_EVENT_SUCCESS;
1622 }
1623
1624
1625 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1626 }
1627
1628 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
1629 {
1630 UNUSED(previousState);
1631
1632 const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
1633 if (landEvent == NAV_FSM_EVENT_SUCCESS) {
1634 // Landing controller returned success - invoke RTH finishing state and finish the waypoint
1635 navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
1636 return NAV_FSM_EVENT_SUCCESS;
1637 }
1638 else {
1639 return NAV_FSM_EVENT_NONE;
1640 }
1641 }
1642
1643 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
1644 {
1645 UNUSED(previousState);
1646
1647 if (isLastMissionWaypoint()) { // Last waypoint reached
1648 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1649 }
1650 else {
1651 // Waypoint reached, do something and move on to next waypoint
1652 posControl.activeWaypointIndex++;
1653 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1654 }
1655 }
1656
1657 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState)
1658 {
1659 UNUSED(previousState);
1660
1661 clearJumpCounters();
1662 posControl.wpMissionRestart = true;
1663
1664 /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
1665 if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
1666 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1667 }
1668
1669 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1670 }
1671
1672 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
1673 {
1674 // TODO:
1675 UNUSED(previousState);
1676
1677 // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
1678 // Make sure terrain following is not enabled
1679 resetAltitudeController(false);
1680
1681 return NAV_FSM_EVENT_SUCCESS;
1682 }
1683
1684 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
1685 {
1686 // TODO:
1687 UNUSED(previousState);
1688 return NAV_FSM_EVENT_NONE;
1689 }
1690
1691 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
1692 {
1693 // TODO:
1694 UNUSED(previousState);
1695
1696 // Prevent I-terms growing when already landed
1697 pidResetErrorAccumulators();
1698
1699 return NAV_FSM_EVENT_SUCCESS;
1700 }
1701
1702 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
1703 {
1704 const timeUs_t currentTimeUs = micros();
1705 UNUSED(previousState);
1706
1707 resetFixedWingLaunchController(currentTimeUs);
1708
1709 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT
1710 }
1711
1712 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState)
1713 {
1714 const timeUs_t currentTimeUs = micros();
1715 UNUSED(previousState);
1716
1717 if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
1718 enableFixedWingLaunchController(currentTimeUs);
1719 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
1720 }
1721
1722 //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
1723 if (feature(FEATURE_FW_LAUNCH)) {
1724 throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
1725 if ((throttleStatus == THROTTLE_LOW) && (isRollPitchStickDeflected())) {
1726 abortFixedWingLaunch();
1727 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
1728 }
1729 }
1730
1731 return NAV_FSM_EVENT_NONE;
1732 }
1733
1734 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState)
1735 {
1736 UNUSED(previousState);
1737
1738 if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) {
1739 return NAV_FSM_EVENT_SUCCESS;
1740 }
1741
1742 return NAV_FSM_EVENT_NONE;
1743 }
1744
1745 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
1746 {
1747 navigationFSMState_t previousState;
1748
1749 previousState = posControl.navState;
1750 if (posControl.navState != newState) {
1751 posControl.navState = newState;
1752 posControl.navPersistentId = navFSM[newState].persistentId;
1753 }
1754 return previousState;
1755 }
1756
1757 static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
1758 {
1759 const timeMs_t currentMillis = millis();
1760 navigationFSMState_t previousState;
1761 static timeMs_t lastStateProcessTime = 0;
1762
1763 /* Process new injected event if event defined,
1764 * otherwise process timeout event if defined */
1765 if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
1766 /* Update state */
1767 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
1768 } else if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
1769 ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
1770 /* Update state */
1771 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
1772 }
1773
1774 if (previousState) { /* If state updated call new state's entry function */
1775 while (navFSM[posControl.navState].onEntry) {
1776 navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
1777
1778 if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
1779 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
1780 }
1781 else {
1782 break;
1783 }
1784 }
1785
1786 lastStateProcessTime = currentMillis;
1787 }
1788
1789 /* Update public system state information */
1790 NAV_Status.mode = MW_GPS_MODE_NONE;
1791
1792 if (ARMING_FLAG(ARMED)) {
1793 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
1794
1795 if (navStateFlags & NAV_AUTO_RTH) {
1796 NAV_Status.mode = MW_GPS_MODE_RTH;
1797 }
1798 else if (navStateFlags & NAV_AUTO_WP) {
1799 NAV_Status.mode = MW_GPS_MODE_NAV;
1800 }
1801 else if (navStateFlags & NAV_CTL_EMERG) {
1802 NAV_Status.mode = MW_GPS_MODE_EMERG;
1803 }
1804 else if (navStateFlags & NAV_CTL_POS) {
1805 NAV_Status.mode = MW_GPS_MODE_HOLD;
1806 }
1807 }
1808
1809 NAV_Status.state = navFSM[posControl.navState].mwState;
1810 NAV_Status.error = navFSM[posControl.navState].mwError;
1811
1812 NAV_Status.flags = 0;
1813 if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
1814 if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
1815
1816 NAV_Status.activeWpNumber = posControl.activeWaypointIndex + 1;
1817 NAV_Status.activeWpAction = 0;
1818 if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
1819 NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
1820 }
1821 }
1822
1823 static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
1824 {
1825 posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos;
1826
1827 switch (mode) {
1828 case RTH_HOME_ENROUTE_INITIAL:
1829 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
1830 break;
1831
1832 case RTH_HOME_ENROUTE_PROPORTIONAL:
1833 {
1834 float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
1835 if (rthTotalDistanceToTravel >= 100) {
1836 float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
1837 posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
1838 }
1839 else {
1840 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1841 }
1842 }
1843 break;
1844
1845 case RTH_HOME_ENROUTE_FINAL:
1846 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1847 break;
1848
1849 case RTH_HOME_FINAL_HOVER:
1850 if (navConfig()->general.rth_home_altitude) {
1851 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
1852 }
1853 else {
1854 // If home altitude not defined - fall back to final ENROUTE altitude
1855 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
1856 }
1857 break;
1858
1859 case RTH_HOME_FINAL_LAND:
1860 // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
1861 if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
1862 posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm
1863 if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
1864 posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used
1865 }
1866 }
1867 break;
1868 }
1869
1870 return &posControl.rthState.homeTmpWaypoint;
1871 }
1872
1873 /*-----------------------------------------------------------
1874 * Detects if thrust vector is facing downwards
1875 *-----------------------------------------------------------*/
1876 bool isThrustFacingDownwards(void)
1877 {
1878 // Tilt angle <= 80 deg; cos(80) = 0.17364817766693034885171662676931
1879 return (calculateCosTiltAngle() >= 0.173648178f);
1880 }
1881
1882 /*-----------------------------------------------------------
1883 * Checks if position sensor (GPS) is failing for a specified timeout (if enabled)
1884 *-----------------------------------------------------------*/
1885 bool checkForPositionSensorTimeout(void)
1886 {
1887 if (navConfig()->general.pos_failure_timeout) {
1888 if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
1889 return true;
1890 }
1891 else {
1892 return false;
1893 }
1894 }
1895 else {
1896 // Timeout not defined, never fail
1897 return false;
1898 }
1899 }
1900
1901 /*-----------------------------------------------------------
1902 * Processes an update to XY-position and velocity
1903 *-----------------------------------------------------------*/
1904 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
1905 {
1906 posControl.actualState.abs.pos.x = newX;
1907 posControl.actualState.abs.pos.y = newY;
1908 posControl.actualState.abs.vel.x = newVelX;
1909 posControl.actualState.abs.vel.y = newVelY;
1910
1911 posControl.actualState.agl.pos.x = newX;
1912 posControl.actualState.agl.pos.y = newY;
1913 posControl.actualState.agl.vel.x = newVelX;
1914 posControl.actualState.agl.vel.y = newVelY;
1915
1916 posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
1917
1918 // CASE 1: POS & VEL valid
1919 if (estPosValid && estVelValid) {
1920 posControl.flags.estPosStatus = EST_TRUSTED;
1921 posControl.flags.estVelStatus = EST_TRUSTED;
1922 posControl.flags.horizontalPositionDataNew = true;
1923 posControl.lastValidPositionTimeMs = millis();
1924 }
1925 // CASE 1: POS invalid, VEL valid
1926 else if (!estPosValid && estVelValid) {
1927 posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
1928 posControl.flags.estVelStatus = EST_TRUSTED;
1929 posControl.flags.horizontalPositionDataNew = true;
1930 posControl.lastValidPositionTimeMs = millis();
1931 }
1932 // CASE 3: can't use pos/vel data
1933 else {
1934 posControl.flags.estPosStatus = EST_NONE;
1935 posControl.flags.estVelStatus = EST_NONE;
1936 posControl.flags.horizontalPositionDataNew = false;
1937 }
1938
1939 //Update blackbox data
1940 navLatestActualPosition[X] = newX;
1941 navLatestActualPosition[Y] = newY;
1942 navActualVelocity[X] = constrain(newVelX, -32678, 32767);
1943 navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
1944 }
1945
1946 /*-----------------------------------------------------------
1947 * Processes an update to Z-position and velocity
1948 *-----------------------------------------------------------*/
1949 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
1950 {
1951 posControl.actualState.abs.pos.z = newAltitude;
1952 posControl.actualState.abs.vel.z = newVelocity;
1953
1954 posControl.actualState.agl.pos.z = surfaceDistance;
1955 posControl.actualState.agl.vel.z = surfaceVelocity;
1956
1957 // Update altitude that would be used when executing RTH
1958 if (estimateValid) {
1959 updateDesiredRTHAltitude();
1960
1961 // If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
1962 if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
1963 // If we are in terrain-following modes - signal that we should update the surface tracking setpoint
1964 // NONE/USABLE means that we were flying blind, now we should lock to surface
1965 //updateSurfaceTrackingSetpoint();
1966 }
1967
1968 posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
1969 posControl.flags.estAltStatus = EST_TRUSTED;
1970 posControl.flags.verticalPositionDataNew = true;
1971 posControl.lastValidAltitudeTimeMs = millis();
1972 }
1973 else {
1974 posControl.flags.estAltStatus = EST_NONE;
1975 posControl.flags.estAglStatus = EST_NONE;
1976 posControl.flags.verticalPositionDataNew = false;
1977 }
1978
1979 if (ARMING_FLAG(ARMED)) {
1980 if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
1981 if (posControl.actualState.surfaceMin > 0) {
1982 posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
1983 }
1984 else {
1985 posControl.actualState.surfaceMin = surfaceDistance;
1986 }
1987 }
1988 }
1989 else {
1990 posControl.actualState.surfaceMin = -1;
1991 }
1992
1993 //Update blackbox data
1994 navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
1995 navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
1996 }
1997
1998 /*-----------------------------------------------------------
1999 * Processes an update to estimated heading
2000 *-----------------------------------------------------------*/
2001 void updateActualHeading(bool headingValid, int32_t newHeading)
2002 {
2003 /* Update heading. Check if we're acquiring a valid heading for the
2004 * first time and update home heading accordingly.
2005 */
2006 navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
2007 if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
2008 (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
2009 (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
2010
2011 // Home was stored using the fake heading (assuming boot as 0deg). Calculate
2012 // the offset from the fake to the actual yaw and apply the same rotation
2013 // to the home point.
2014 int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
2015
2016 posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
2017 if (posControl.rthState.homePosition.yaw < 0) {
2018 posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
2019 }
2020 if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
2021 posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
2022 }
2023 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2024 }
2025 posControl.actualState.yaw = newHeading;
2026 posControl.flags.estHeadingStatus = newEstHeading;
2027
2028 /* Precompute sin/cos of yaw angle */
2029 posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2030 posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
2031 }
2032
2033 /*-----------------------------------------------------------
2034 * Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
2035 *-----------------------------------------------------------*/
2036 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
2037 {
2038 return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
2039 }
2040
2041 /*-----------------------------------------------------------
2042 * Calculates distance and bearing to destination point
2043 *-----------------------------------------------------------*/
2044 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
2045 {
2046 return fast_fsqrtf(sq(deltaX) + sq(deltaY));
2047 }
2048
2049 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
2050 {
2051 return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
2052 }
2053
2054 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
2055 {
2056 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2057 const float deltaX = destinationPos->x - posvel->pos.x;
2058 const float deltaY = destinationPos->y - posvel->pos.y;
2059
2060 return calculateDistanceFromDelta(deltaX, deltaY);
2061 }
2062
2063 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
2064 {
2065 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2066 const float deltaX = destinationPos->x - posvel->pos.x;
2067 const float deltaY = destinationPos->y - posvel->pos.y;
2068
2069 return calculateBearingFromDelta(deltaX, deltaY);
2070 }
2071
2072 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
2073 {
2074 if (posControl.flags.estPosStatus == EST_NONE ||
2075 posControl.flags.estHeadingStatus == EST_NONE) {
2076
2077 return false;
2078 }
2079
2080 const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
2081 const float deltaX = destinationPos->x - posvel->pos.x;
2082 const float deltaY = destinationPos->y - posvel->pos.y;
2083
2084 result->distance = calculateDistanceFromDelta(deltaX, deltaY);
2085 result->bearing = calculateBearingFromDelta(deltaX, deltaY);
2086 return true;
2087 }
2088
2089 /*-----------------------------------------------------------
2090 * Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing
2091 *-----------------------------------------------------------*/
2092 bool isWaypointMissed(const navWaypointPosition_t * waypoint)
2093 {
2094 int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
2095 bearingError = wrap_18000(bearingError);
2096
2097 return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
2098 }
2099
2100 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
2101 {
2102 // We consider waypoint reached if within specified radius
2103 posControl.wpDistance = calculateDistanceToDestination(pos);
2104
2105 if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
2106 // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
2107 return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
2108 || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
2109 }
2110 else {
2111 return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
2112 }
2113 }
2114
2115 bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome)
2116 {
2117 return isWaypointPositionReached(&waypoint->pos, isWaypointHome);
2118 }
2119
2120 static void updateHomePositionCompatibility(void)
2121 {
2122 geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
2123 GPS_distanceToHome = posControl.homeDistance * 0.01f;
2124 GPS_directionToHome = posControl.homeDirection * 0.01f;
2125 }
2126
2127 // Backdoor for RTH estimator
2128 float getFinalRTHAltitude(void)
2129 {
2130 return posControl.rthState.rthFinalAltitude;
2131 }
2132
2133 /*-----------------------------------------------------------
2134 * Update the RTH Altitudes
2135 *-----------------------------------------------------------*/
2136 static void updateDesiredRTHAltitude(void)
2137 {
2138 if (ARMING_FLAG(ARMED)) {
2139 if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
2140 || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
2141 switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
2142 case NAV_RTH_CLIMB_STAGE_AT_LEAST:
2143 posControl.rthState.rthClimbStageAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2144 break;
2145 case NAV_RTH_CLIMB_STAGE_EXTRA:
2146 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
2147 break;
2148 }
2149
2150 switch (navConfig()->general.flags.rth_alt_control_mode) {
2151 case NAV_RTH_NO_ALT:
2152 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2153 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2154 break;
2155
2156 case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
2157 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
2158 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2159 break;
2160
2161 case NAV_RTH_MAX_ALT:
2162 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z);
2163 if (navConfig()->general.rth_altitude > 0) {
2164 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude);
2165 }
2166 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2167 break;
2168
2169 case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
2170 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2171 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2172 break;
2173
2174 case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
2175 posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
2176 posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2177 break;
2178
2179 case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
2180 default:
2181 posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
2182 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2183 }
2184 }
2185 } else {
2186 posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
2187 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2188 posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
2189 }
2190 }
2191
2192 /*-----------------------------------------------------------
2193 * RTH sanity test logic
2194 *-----------------------------------------------------------*/
2195 void initializeRTHSanityChecker(void)
2196 {
2197 const timeMs_t currentTimeMs = millis();
2198
2199 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2200 posControl.rthSanityChecker.rthSanityOK = true;
2201 posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2202 }
2203
2204 bool validateRTHSanityChecker(void)
2205 {
2206 const timeMs_t currentTimeMs = millis();
2207
2208 // Ability to disable sanity checker
2209 if (navConfig()->general.rth_abort_threshold == 0) {
2210 return true;
2211 }
2212
2213 // Check at 10Hz rate
2214 if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
2215 const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
2216 posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
2217
2218 if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) {
2219 posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
2220 } else {
2221 // If while doing RTH we got even farther away from home - RTH is doing something crazy
2222 posControl.rthSanityChecker.rthSanityOK = (currentDistanceToHome - posControl.rthSanityChecker.minimalDistanceToHome) < navConfig()->general.rth_abort_threshold;
2223 }
2224 }
2225
2226 return posControl.rthSanityChecker.rthSanityOK;
2227 }
2228
2229 /*-----------------------------------------------------------
2230 * Reset home position to current position
2231 *-----------------------------------------------------------*/
2232 void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
2233 {
2234 // XY-position
2235 if ((useMask & NAV_POS_UPDATE_XY) != 0) {
2236 posControl.rthState.homePosition.pos.x = pos->x;
2237 posControl.rthState.homePosition.pos.y = pos->y;
2238 if (homeFlags & NAV_HOME_VALID_XY) {
2239 posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
2240 } else {
2241 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
2242 }
2243 }
2244
2245 // Z-position
2246 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
2247 posControl.rthState.homePosition.pos.z = pos->z;
2248 if (homeFlags & NAV_HOME_VALID_Z) {
2249 posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
2250 } else {
2251 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
2252 }
2253 }
2254
2255 // Heading
2256 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
2257 // Heading
2258 posControl.rthState.homePosition.yaw = yaw;
2259 if (homeFlags & NAV_HOME_VALID_HEADING) {
2260 posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
2261 } else {
2262 posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING;
2263 }
2264 }
2265
2266 posControl.homeDistance = 0;
2267 posControl.homeDirection = 0;
2268
2269 // Update target RTH altitude as a waypoint above home
2270 updateDesiredRTHAltitude();
2271
2272 updateHomePositionCompatibility();
2273 ENABLE_STATE(GPS_FIX_HOME);
2274 }
2275
2276 static navigationHomeFlags_t navigationActualStateHomeValidity(void)
2277 {
2278 navigationHomeFlags_t flags = 0;
2279
2280 if (posControl.flags.estPosStatus >= EST_USABLE) {
2281 flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
2282 }
2283
2284 if (posControl.flags.estHeadingStatus >= EST_USABLE) {
2285 flags |= NAV_HOME_VALID_HEADING;
2286 }
2287
2288 return flags;
2289 }
2290
2291 #if defined(USE_SAFE_HOME)
2292
2293 void checkSafeHomeState(bool shouldBeEnabled)
2294 {
2295 if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
2296 shouldBeEnabled = false;
2297 } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
2298 // if safehomes are only used with failsafe and we're trying to enable safehome
2299 // then enable the safehome only with failsafe
2300 shouldBeEnabled = posControl.flags.forcedRTHActivated;
2301 }
2302 // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
2303 if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
2304 return;
2305 }
2306 if (shouldBeEnabled) {
2307 // set home to safehome
2308 setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2309 safehome_applied = true;
2310 } else {
2311 // set home to original arming point
2312 setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2313 safehome_applied = false;
2314 }
2315 // if we've changed the home position, update the distance and direction
2316 updateHomePosition();
2317 }
2318
2319 /***********************************************************
2320 * See if there are any safehomes near where we are arming.
2321 * If so, save the nearest one in case we need it later for RTH.
2322 **********************************************************/
2323 bool findNearestSafeHome(void)
2324 {
2325 safehome_index = -1;
2326 uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
2327 uint32_t distance_to_current;
2328 fpVector3_t currentSafeHome;
2329 gpsLocation_t shLLH;
2330 shLLH.alt = 0;
2331 for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
2332 if (!safeHomeConfig(i)->enabled)
2333 continue;
2334
2335 shLLH.lat = safeHomeConfig(i)->lat;
2336 shLLH.lon = safeHomeConfig(i)->lon;
2337 geoConvertGeodeticToLocal(&currentSafeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
2338 distance_to_current = calculateDistanceToDestination(&currentSafeHome);
2339 if (distance_to_current < nearest_safehome_distance) {
2340 // this safehome is the nearest so far - keep track of it.
2341 safehome_index = i;
2342 nearest_safehome_distance = distance_to_current;
2343 nearestSafeHome.x = currentSafeHome.x;
2344 nearestSafeHome.y = currentSafeHome.y;
2345 nearestSafeHome.z = currentSafeHome.z;
2346 }
2347 }
2348 if (safehome_index >= 0) {
2349 safehome_distance = nearest_safehome_distance;
2350 } else {
2351 safehome_distance = 0;
2352 }
2353 return safehome_distance > 0;
2354 }
2355 #endif
2356
2357 /*-----------------------------------------------------------
2358 * Update home position, calculate distance and bearing to home
2359 *-----------------------------------------------------------*/
2360 void updateHomePosition(void)
2361 {
2362 // Disarmed and have a valid position, constantly update home
2363 if (!ARMING_FLAG(ARMED)) {
2364 if (posControl.flags.estPosStatus >= EST_USABLE) {
2365 const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
2366 bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
2367 switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
2368 case NAV_RESET_NEVER:
2369 break;
2370 case NAV_RESET_ON_FIRST_ARM:
2371 setHome |= !ARMING_FLAG(WAS_EVER_ARMED);
2372 break;
2373 case NAV_RESET_ON_EACH_ARM:
2374 setHome = true;
2375 break;
2376 }
2377 if (setHome) {
2378 #if defined(USE_SAFE_HOME)
2379 findNearestSafeHome();
2380 #endif
2381 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
2382 // save the current location in case it is replaced by a safehome or HOME_RESET
2383 original_rth_home.x = posControl.rthState.homePosition.pos.x;
2384 original_rth_home.y = posControl.rthState.homePosition.pos.y;
2385 original_rth_home.z = posControl.rthState.homePosition.pos.z;
2386 }
2387 }
2388 }
2389 else {
2390 static bool isHomeResetAllowed = false;
2391
2392 // If pilot so desires he may reset home position to current position
2393 if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
2394 if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
2395 const navSetWaypointFlags_t 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);
2396 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
2397 isHomeResetAllowed = false;
2398 }
2399 }
2400 else {
2401 isHomeResetAllowed = true;
2402 }
2403
2404 // Update distance and direction to home if armed (home is not updated when armed)
2405 if (STATE(GPS_FIX_HOME)) {
2406 fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
2407 posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
2408 posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
2409 updateHomePositionCompatibility();
2410 }
2411 }
2412 }
2413
2414 /* -----------------------------------------------------------
2415 * Override RTH preset altitude and Climb First option
2416 * using Pitch/Roll stick held for > 1 seconds
2417 * Climb First override limited to Fixed Wing only
2418 *-----------------------------------------------------------*/
2419 static bool rthAltControlStickOverrideCheck(unsigned axis)
2420 {
2421 if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) {
2422 return false;
2423 }
2424 static timeMs_t rthOverrideStickHoldStartTime[2];
2425
2426 if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
2427 timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis];
2428
2429 if (!rthOverrideStickHoldStartTime[axis]) {
2430 rthOverrideStickHoldStartTime[axis] = millis();
2431 } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec
2432 if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude
2433 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
2434 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
2435 return true;
2436 } else if (axis == ROLL) { // ROLL right to override climb first
2437 return true;
2438 }
2439 }
2440 } else {
2441 rthOverrideStickHoldStartTime[axis] = 0;
2442 }
2443
2444 return false;
2445 }
2446
2447 /* ---------------------------------------------------
2448 * If climb stage is being used, see if it is time to
2449 * transiton in to turn.
2450 * Limited to fixed wing only.
2451 * --------------------------------------------------- */
2452 bool rthClimbStageActiveAndComplete() {
2453 if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
2454 if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
2455 return true;
2456 }
2457 }
2458
2459 return false;
2460 }
2461
2462 /*-----------------------------------------------------------
2463 * Update flight statistics
2464 *-----------------------------------------------------------*/
2465 static void updateNavigationFlightStatistics(void)
2466 {
2467 static timeMs_t previousTimeMs = 0;
2468 const timeMs_t currentTimeMs = millis();
2469 const timeDelta_t timeDeltaMs = currentTimeMs - previousTimeMs;
2470 previousTimeMs = currentTimeMs;
2471
2472 if (ARMING_FLAG(ARMED)) {
2473 posControl.totalTripDistance += posControl.actualState.velXY * MS2S(timeDeltaMs);
2474 }
2475 }
2476
2477 uint32_t getTotalTravelDistance(void)
2478 {
2479 return lrintf(posControl.totalTripDistance);
2480 }
2481
2482 /*-----------------------------------------------------------
2483 * Calculate platform-specific hold position (account for deceleration)
2484 *-----------------------------------------------------------*/
2485 void calculateInitialHoldPosition(fpVector3_t * pos)
2486 {
2487 if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2488 calculateFixedWingInitialHoldPosition(pos);
2489 }
2490 else {
2491 calculateMulticopterInitialHoldPosition(pos);
2492 }
2493 }
2494
2495 /*-----------------------------------------------------------
2496 * Set active XYZ-target and desired heading
2497 *-----------------------------------------------------------*/
2498 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
2499 {
2500 // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
2501 if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
2502 posControl.desiredState.pos.x = pos->x;
2503 posControl.desiredState.pos.y = pos->y;
2504 }
2505
2506 // Z-position
2507 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
2508 updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
2509 posControl.desiredState.pos.z = pos->z;
2510 }
2511
2512 // Heading
2513 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
2514 // Heading
2515 posControl.desiredState.yaw = yaw;
2516 }
2517 else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) {
2518 posControl.desiredState.yaw = calculateBearingToDestination(pos);
2519 }
2520 else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
2521 posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
2522 }
2523 }
2524
2525 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
2526 {
2527 farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
2528 farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
2529 farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
2530 }
2531
2532 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance)
2533 {
2534 origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
2535 origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
2536 origin->z = origin->z;
2537 }
2538
2539 /*-----------------------------------------------------------
2540 * NAV land detector
2541 *-----------------------------------------------------------*/
2542 void resetLandingDetector(void)
2543 {
2544 if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2545 resetFixedWingLandingDetector();
2546 }
2547 else {
2548 resetMulticopterLandingDetector();
2549 }
2550 }
2551
2552 bool isLandingDetected(void)
2553 {
2554 bool landingDetected;
2555
2556 if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
2557 landingDetected = isFixedWingLandingDetected();
2558 }
2559 else {
2560 landingDetected = isMulticopterLandingDetected();
2561 }
2562
2563 return landingDetected;
2564 }
2565
2566 /*-----------------------------------------------------------
2567 * Z-position controller
2568 *-----------------------------------------------------------*/
2569 void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
2570 {
2571 static timeUs_t lastUpdateTimeUs;
2572 timeUs_t currentTimeUs = micros();
2573
2574 // Terrain following uses different altitude measurement
2575 const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
2576
2577 if (mode == ROC_TO_ALT_RESET) {
2578 lastUpdateTimeUs = currentTimeUs;
2579 posControl.desiredState.pos.z = altitudeToUse;
2580 }
2581 else {
2582
2583 /*
2584 * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
2585 * In other words, when altitude is reached, allow it only to shrink
2586 */
2587 if (navConfig()->general.max_altitude > 0 &&
2588 altitudeToUse >= navConfig()->general.max_altitude &&
2589 desiredClimbRate > 0
2590 ) {
2591 desiredClimbRate = 0;
2592 }
2593
2594 if (STATE(FIXED_WING_LEGACY)) {
2595 // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
2596 float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
2597
2598 if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
2599 posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
2600 posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
2601 }
2602 }
2603 else {
2604 // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
2605 posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
2606 }
2607
2608 lastUpdateTimeUs = currentTimeUs;
2609 }
2610 }
2611
2612 static void resetAltitudeController(bool useTerrainFollowing)
2613 {
2614 // Set terrain following flag
2615 posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
2616
2617 if (STATE(FIXED_WING_LEGACY)) {
2618 resetFixedWingAltitudeController();
2619 }
2620 else {
2621 resetMulticopterAltitudeController();
2622 }
2623 }
2624
2625 static void setupAltitudeController(void)
2626 {
2627 if (STATE(FIXED_WING_LEGACY)) {
2628 setupFixedWingAltitudeController();
2629 }
2630 else {
2631 setupMulticopterAltitudeController();
2632 }
2633 }
2634
2635 static bool adjustAltitudeFromRCInput(void)
2636 {
2637 if (STATE(FIXED_WING_LEGACY)) {
2638 return adjustFixedWingAltitudeFromRCInput();
2639 }
2640 else {
2641 return adjustMulticopterAltitudeFromRCInput();
2642 }
2643 }
2644
2645 /*-----------------------------------------------------------
2646 * Jump Counter support functions
2647 *-----------------------------------------------------------*/
2648 static void setupJumpCounters(void)
2649 {
2650 for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
2651 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
2652 posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
2653 }
2654 }
2655 }
2656
2657 static void resetJumpCounter(void)
2658 {
2659 // reset the volatile counter from the set / static value
2660 posControl.waypointList[posControl.activeWaypointIndex].p3 =
2661 posControl.waypointList[posControl.activeWaypointIndex].p2;
2662 }
2663
2664 static void clearJumpCounters(void)
2665 {
2666 for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
2667 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
2668 posControl.waypointList[wp].p3 = 0;
2669 }
2670 }
2671 }
2672
2673
2674
2675 /*-----------------------------------------------------------
2676 * Heading controller (pass-through to MAG mode)
2677 *-----------------------------------------------------------*/
2678 static void resetHeadingController(void)
2679 {
2680 if (STATE(FIXED_WING_LEGACY)) {
2681 resetFixedWingHeadingController();
2682 }
2683 else {
2684 resetMulticopterHeadingController();
2685 }
2686 }
2687
2688 static bool adjustHeadingFromRCInput(void)
2689 {
2690 if (STATE(FIXED_WING_LEGACY)) {
2691 return adjustFixedWingHeadingFromRCInput();
2692 }
2693 else {
2694 return adjustMulticopterHeadingFromRCInput();
2695 }
2696 }
2697
2698 /*-----------------------------------------------------------
2699 * XY Position controller
2700 *-----------------------------------------------------------*/
2701 static void resetPositionController(void)
2702 {
2703 if (STATE(FIXED_WING_LEGACY)) {
2704 resetFixedWingPositionController();
2705 }
2706 else {
2707 resetMulticopterPositionController();
2708 resetMulticopterBrakingMode();
2709 }
2710 }
2711
2712 static bool adjustPositionFromRCInput(void)
2713 {
2714 bool retValue;
2715
2716 if (STATE(FIXED_WING_LEGACY)) {
2717 retValue = adjustFixedWingPositionFromRCInput();
2718 }
2719 else {
2720
2721 const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
2722 const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
2723
2724 retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
2725 }
2726
2727 return retValue;
2728 }
2729
2730 /*-----------------------------------------------------------
2731 * WP controller
2732 *-----------------------------------------------------------*/
2733 void resetGCSFlags(void)
2734 {
2735 posControl.flags.isGCSAssistedNavigationReset = false;
2736 posControl.flags.isGCSAssistedNavigationEnabled = false;
2737 }
2738
2739 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
2740 {
2741 /* Default waypoint to send */
2742 wpData->action = NAV_WP_ACTION_RTH;
2743 wpData->lat = 0;
2744 wpData->lon = 0;
2745 wpData->alt = 0;
2746 wpData->p1 = 0;
2747 wpData->p2 = 0;
2748 wpData->p3 = 0;
2749 wpData->flag = NAV_WP_FLAG_LAST;
2750
2751 // WP #0 - special waypoint - HOME
2752 if (wpNumber == 0) {
2753 if (STATE(GPS_FIX_HOME)) {
2754 wpData->lat = GPS_home.lat;
2755 wpData->lon = GPS_home.lon;
2756 wpData->alt = GPS_home.alt;
2757 }
2758 }
2759 // WP #255 - special waypoint - directly get actualPosition
2760 else if (wpNumber == 255) {
2761 gpsLocation_t wpLLH;
2762
2763 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
2764
2765 wpData->lat = wpLLH.lat;
2766 wpData->lon = wpLLH.lon;
2767 wpData->alt = wpLLH.alt;
2768 }
2769 // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode
2770 else if (wpNumber == 254) {
2771 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
2772
2773 if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
2774 gpsLocation_t wpLLH;
2775
2776 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
2777
2778 wpData->lat = wpLLH.lat;
2779 wpData->lon = wpLLH.lon;
2780 wpData->alt = wpLLH.alt;
2781 }
2782 }
2783 // WP #1 - #60 - common waypoints - pre-programmed mission
2784 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
2785 if (wpNumber <= posControl.waypointCount) {
2786 *wpData = posControl.waypointList[wpNumber - 1];
2787 if(wpData->action == NAV_WP_ACTION_JUMP) {
2788 wpData->p1 += 1; // make WP # (vice index)
2789 }
2790
2791 }
2792 }
2793 }
2794
2795 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
2796 {
2797 gpsLocation_t wpLLH;
2798 navWaypointPosition_t wpPos;
2799
2800 // Pre-fill structure to convert to local coordinates
2801 wpLLH.lat = wpData->lat;
2802 wpLLH.lon = wpData->lon;
2803 wpLLH.alt = wpData->alt;
2804
2805 // WP #0 - special waypoint - HOME
2806 if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
2807 // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
2808 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
2809 setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
2810 }
2811 // WP #255 - special waypoint - directly set desiredPosition
2812 // Only valid when armed and in poshold mode
2813 else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
2814 ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
2815 (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
2816 // Convert to local coordinates
2817 geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
2818
2819 navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
2820
2821 // If we received global altitude == 0, use current altitude
2822 if (wpData->alt != 0) {
2823 waypointUpdateFlags |= NAV_POS_UPDATE_Z;
2824 }
2825
2826 if (wpData->p1 > 0 && wpData->p1 < 360) {
2827 waypointUpdateFlags |= NAV_POS_UPDATE_HEADING;
2828 }
2829
2830 setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
2831 }
2832 // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
2833 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
2834 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 ) {
2835 // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
2836 static int8_t nonGeoWaypointCount = 0;
2837
2838 if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
2839 if (wpNumber == 1) {
2840 resetWaypointList();
2841 }
2842 posControl.waypointList[wpNumber - 1] = *wpData;
2843 if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
2844 nonGeoWaypointCount += 1;
2845 if(wpData->action == NAV_WP_ACTION_JUMP) {
2846 posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
2847 }
2848 }
2849
2850 posControl.waypointCount = wpNumber;
2851 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
2852 posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
2853 if (posControl.waypointListValid) {
2854 nonGeoWaypointCount = 0;
2855 }
2856 }
2857 }
2858 }
2859 }
2860
2861 void resetWaypointList(void)
2862 {
2863 /* Can only reset waypoint list if not armed */
2864 if (!ARMING_FLAG(ARMED)) {
2865 posControl.waypointCount = 0;
2866 posControl.waypointListValid = false;
2867 posControl.geoWaypointCount = 0;
2868 #ifdef USE_MULTI_MISSION
2869 posControl.loadedMultiMissionIndex = 0;
2870 posControl.loadedMultiMissionStartWP = 0;
2871 posControl.loadedMultiMissionWPCount = 0;
2872 #endif
2873 }
2874 }
2875
2876 bool isWaypointListValid(void)
2877 {
2878 return posControl.waypointListValid;
2879 }
2880
2881 int getWaypointCount(void)
2882 {
2883 return posControl.waypointCount;
2884 }
2885 #ifdef USE_MULTI_MISSION
2886 void selectMultiMissionIndex(int8_t increment)
2887 {
2888 if (posControl.multiMissionCount > 1) { // stick selection only active when multi mission loaded
2889 navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 0, posControl.multiMissionCount);
2890 }
2891 }
2892
2893 void setMultiMissionOnArm(void)
2894 {
2895 if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) {
2896 posControl.waypointCount = posControl.loadedMultiMissionWPCount;
2897
2898 for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
2899 posControl.waypointList[i] = posControl.waypointList[i + posControl.loadedMultiMissionStartWP];
2900 if (i == posControl.waypointCount - 1) {
2901 break;
2902 }
2903 }
2904
2905 posControl.loadedMultiMissionStartWP = 0;
2906 posControl.loadedMultiMissionWPCount = 0;
2907 }
2908 }
2909
2910 bool checkMissionCount(int8_t waypoint)
2911 {
2912 if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
2913 posControl.multiMissionCount += 1; // count up no missions in multi mission WP file
2914 if (waypoint != NAV_MAX_WAYPOINTS - 1) {
2915 return (nonVolatileWaypointList(waypoint + 1)->flag == NAV_WP_FLAG_LAST &&
2916 nonVolatileWaypointList(waypoint + 1)->action ==NAV_WP_ACTION_RTH);
2917 // end of multi mission file if successive NAV_WP_FLAG_LAST and default action (RTH)
2918 }
2919 }
2920 return false;
2921 }
2922 #endif // multi mission
2923 #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
2924 bool loadNonVolatileWaypointList(bool clearIfLoaded)
2925 {
2926 #ifdef USE_MULTI_MISSION
2927 /* multi_mission_index 0 only used for non NVM missions - don't load.
2928 * Don't load if mission planner WP count > 0 */
2929 if (ARMING_FLAG(ARMED) || !navConfig()->general.waypoint_multi_mission_index || posControl.wpPlannerActiveWPIndex) {
2930 #else
2931 if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
2932 #endif
2933 return false;
2934 }
2935
2936 // if forced and waypoints are already loaded, just unload them.
2937 if (clearIfLoaded && posControl.waypointCount > 0) {
2938 resetWaypointList();
2939 return false;
2940 }
2941 #ifdef USE_MULTI_MISSION
2942 /* Reset multi mission index to 1 if exceeds number of available missions */
2943 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
2944 navConfigMutable()->general.waypoint_multi_mission_index = 1;
2945 }
2946 posControl.multiMissionCount = 0;
2947 posControl.loadedMultiMissionWPCount = 0;
2948 int8_t loadedMultiMissionGeoWPCount;
2949 #endif
2950 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
2951 setWaypoint(i + 1, nonVolatileWaypointList(i));
2952 #ifdef USE_MULTI_MISSION
2953 /* store details of selected mission */
2954 if ((posControl.multiMissionCount + 1 == navConfig()->general.waypoint_multi_mission_index)) {
2955 // mission start WP
2956 if (posControl.loadedMultiMissionWPCount == 0) {
2957 posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
2958 posControl.loadedMultiMissionStartWP = i;
2959 loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
2960 }
2961 // mission end WP
2962 if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
2963 posControl.loadedMultiMissionWPCount = i - posControl.loadedMultiMissionStartWP + 1;
2964 loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1;
2965 }
2966 }
2967
2968 /* count up number of missions */
2969 if (checkMissionCount(i)) {
2970 break;
2971 }
2972 }
2973
2974 posControl.geoWaypointCount = loadedMultiMissionGeoWPCount;
2975 posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0;
2976
2977 /* Mission sanity check failed - reset the list
2978 * Also reset if no selected mission loaded (shouldn't happen) */
2979 if (!posControl.waypointListValid || !posControl.loadedMultiMissionWPCount) {
2980 #else
2981 // check this is the last waypoint
2982 if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
2983 break;
2984 }
2985 }
2986 // Mission sanity check failed - reset the list
2987 if (!posControl.waypointListValid) {
2988 #endif
2989 resetWaypointList();
2990 }
2991
2992 return posControl.waypointListValid;
2993 }
2994
2995 bool saveNonVolatileWaypointList(void)
2996 {
2997 if (ARMING_FLAG(ARMED) || !posControl.waypointListValid)
2998 return false;
2999
3000 for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
3001 getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
3002 }
3003 #ifdef USE_MULTI_MISSION
3004 navConfigMutable()->general.waypoint_multi_mission_index = 1; // reset selected mission to 1 when new entries saved
3005 #endif
3006 saveConfigAndNotify();
3007
3008 return true;
3009 }
3010 #endif
3011
3012 #if defined(USE_SAFE_HOME)
3013
3014 void resetSafeHomes(void)
3015 {
3016 memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
3017 }
3018 #endif
3019
3020 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
3021 {
3022 gpsLocation_t wpLLH;
3023
3024 /* Default to home position if lat & lon = 0 or HOME flag set
3025 * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
3026 if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
3027 wpLLH.lat = GPS_home.lat;
3028 wpLLH.lon = GPS_home.lon;
3029 } else {
3030 wpLLH.lat = waypoint->lat;
3031 wpLLH.lon = waypoint->lon;
3032 }
3033 wpLLH.alt = waypoint->alt;
3034
3035 geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
3036 }
3037
3038 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
3039 {
3040 posControl.activeWaypoint.pos = *pos;
3041
3042 // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
3043 posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
3044
3045 // Set desired position to next waypoint (XYZ-controller)
3046 setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
3047 }
3048
3049 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
3050 {
3051 return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
3052 }
3053
3054 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
3055 {
3056 fpVector3_t localPos;
3057 mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
3058 calculateAndSetActiveWaypointToLocalPosition(&localPos);
3059 }
3060
3061 /* Checks if active waypoint is last in mission */
3062 bool isLastMissionWaypoint(void)
3063 {
3064 return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
3065 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
3066 }
3067
3068 /* Checks if Nav hold position is active */
3069 bool isNavHoldPositionActive(void)
3070 {
3071 if (FLIGHT_MODE(NAV_WP_MODE)) { // WP mode last WP hold and Timed hold positions
3072 return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
3073 }
3074 // RTH spiral climb and Home positions and POSHOLD position
3075 return FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_POSHOLD_MODE);
3076 }
3077
3078 float getActiveWaypointSpeed(void)
3079 {
3080 if (posControl.flags.isAdjustingPosition) {
3081 // In manual control mode use different cap for speed
3082 return navConfig()->general.max_manual_speed;
3083 }
3084 else {
3085 uint16_t waypointSpeed = navConfig()->general.auto_speed;
3086
3087 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
3088 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)) {
3089 float wpSpecificSpeed = 0.0f;
3090 if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
3091 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
3092 else
3093 wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
3094
3095 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
3096 waypointSpeed = wpSpecificSpeed;
3097 } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
3098 waypointSpeed = navConfig()->general.max_auto_speed;
3099 }
3100 }
3101 }
3102
3103 return waypointSpeed;
3104 }
3105 }
3106
3107 /*-----------------------------------------------------------
3108 * Process adjustments to alt, pos and yaw controllers
3109 *-----------------------------------------------------------*/
3110 static void processNavigationRCAdjustments(void)
3111 {
3112 /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
3113 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3114 if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
3115 posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
3116 }
3117 else {
3118 posControl.flags.isAdjustingAltitude = false;
3119 }
3120
3121 if (navStateFlags & NAV_RC_POS) {
3122 if (!FLIGHT_MODE(FAILSAFE_MODE)) {
3123 posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
3124 }
3125 else {
3126 if (!STATE(FIXED_WING_LEGACY)) {
3127 resetMulticopterBrakingMode();
3128 }
3129 }
3130 }
3131 else {
3132 posControl.flags.isAdjustingPosition = false;
3133 }
3134
3135 if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
3136 posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
3137 }
3138 else {
3139 posControl.flags.isAdjustingHeading = false;
3140 }
3141 }
3142
3143 /*-----------------------------------------------------------
3144 * A main function to call position controllers at loop rate
3145 *-----------------------------------------------------------*/
3146 void applyWaypointNavigationAndAltitudeHold(void)
3147 {
3148 const timeUs_t currentTimeUs = micros();
3149
3150 //Updata blackbox data
3151 navFlags = 0;
3152 if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
3153 if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
3154 if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
3155 if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
3156 #if defined(NAV_GPS_GLITCH_DETECTION)
3157 if (isGPSGlitchDetected()) navFlags |= (1 << 4);
3158 #endif
3159 if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
3160
3161 // Reset all navigation requests - NAV controllers will set them if necessary
3162 DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
3163
3164 // No navigation when disarmed
3165 if (!ARMING_FLAG(ARMED)) {
3166 // If we are disarmed, abort forced RTH or Emergency Landing
3167 posControl.flags.forcedRTHActivated = false;
3168 posControl.flags.forcedEmergLandingActivated = false;
3169 // ensure WP missions always restart from first waypoint after disarm
3170 posControl.activeWaypointIndex = 0;
3171 return;
3172 }
3173
3174 /* Reset flags */
3175 posControl.flags.horizontalPositionDataConsumed = false;
3176 posControl.flags.verticalPositionDataConsumed = false;
3177
3178 /* Process controllers */
3179 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
3180 if (STATE(ROVER) || STATE(BOAT)) {
3181 applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
3182 } else if (STATE(FIXED_WING_LEGACY)) {
3183 applyFixedWingNavigationController(navStateFlags, currentTimeUs);
3184 }
3185 else {
3186 applyMulticopterNavigationController(navStateFlags, currentTimeUs);
3187 }
3188
3189 /* Consume position data */
3190 if (posControl.flags.horizontalPositionDataConsumed)
3191 posControl.flags.horizontalPositionDataNew = false;
3192
3193 if (posControl.flags.verticalPositionDataConsumed)
3194 posControl.flags.verticalPositionDataNew = false;
3195
3196 //Update blackbox data
3197 if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
3198 if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
3199 if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
3200
3201 navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
3202 navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
3203 navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
3204 }
3205
3206 /*-----------------------------------------------------------
3207 * Set CF's FLIGHT_MODE from current NAV_MODE
3208 *-----------------------------------------------------------*/
3209 void switchNavigationFlightModes(void)
3210 {
3211 const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
3212 const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
3213 DISABLE_FLIGHT_MODE(disabledFlightModes);
3214 ENABLE_FLIGHT_MODE(enabledNavFlightModes);
3215 }
3216
3217 /*-----------------------------------------------------------
3218 * desired NAV_MODE from combination of FLIGHT_MODE flags
3219 *-----------------------------------------------------------*/
3220 static bool canActivateAltHoldMode(void)
3221 {
3222 return (posControl.flags.estAltStatus >= EST_USABLE);
3223 }
3224
3225 static bool canActivatePosHoldMode(void)
3226 {
3227 return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
3228 }
3229
3230 static bool canActivateNavigationModes(void)
3231 {
3232 return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
3233 }
3234
3235 static bool isWaypointMissionValid(void)
3236 {
3237 return posControl.waypointListValid && (posControl.waypointCount > 0);
3238 }
3239
3240 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
3241 {
3242 static bool canActivateWaypoint = false;
3243 static bool canActivateLaunchMode = false;
3244
3245 //We can switch modes only when ARMED
3246 if (ARMING_FLAG(ARMED)) {
3247 // Ask failsafe system if we can use navigation system
3248 if (failsafeBypassNavigation()) {
3249 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3250 }
3251
3252 // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
3253 const bool canActivateAltHold = canActivateAltHoldMode();
3254 const bool canActivatePosHold = canActivatePosHoldMode();
3255 const bool canActivateNavigation = canActivateNavigationModes();
3256 const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
3257 checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
3258
3259 /* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */
3260 if (posControl.flags.forcedEmergLandingActivated) {
3261 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
3262 }
3263
3264 /* Keep Emergency landing mode active once triggered.
3265 * If caused by sensor failure - landing auto cancelled if sensors working again or when WP and RTH deselected or if Althold selected.
3266 * If caused by RTH Sanity Checking - landing cancelled if RTH deselected.
3267 * Remains active if failsafe active regardless of mode selections */
3268 // CR61
3269 if (navigationIsExecutingAnEmergencyLanding()) {
3270 bool autonomousNavIsPossible = canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME);
3271 bool emergLandingCancel = (!autonomousNavIsPossible &&
3272 ((IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) || !(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH)))) ||
3273 (autonomousNavIsPossible && !IS_RC_MODE_ACTIVE(BOXNAVRTH));
3274
3275 if ((!posControl.rthSanityChecker.rthSanityOK || !autonomousNavIsPossible) && (!emergLandingCancel || FLIGHT_MODE(FAILSAFE_MODE))) {
3276 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
3277 }
3278 }
3279 posControl.rthSanityChecker.rthSanityOK = true;
3280
3281 // Keep canActivateWaypoint flag at FALSE if there is no mission loaded
3282 // Also block WP mission if we are executing RTH
3283 if (!isWaypointMissionValid() || isExecutingRTH) {
3284 canActivateWaypoint = false;
3285 }
3286
3287 /* Airplane specific modes */
3288 if (STATE(AIRPLANE)) {
3289 // LAUNCH mode has priority over any other NAV mode
3290 if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
3291 if (canActivateLaunchMode) {
3292 canActivateLaunchMode = false;
3293 return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
3294 }
3295 else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
3296 // Make sure we don't bail out to IDLE
3297 return NAV_FSM_EVENT_NONE;
3298 }
3299 }
3300 else {
3301 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
3302 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
3303 throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
3304 if (throttleStatus != THROTTLE_LOW)
3305 return NAV_FSM_EVENT_NONE;
3306 else
3307 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3308 }
3309 }
3310
3311 /* Soaring mode, disables altitude control in Position hold and Course hold modes.
3312 * Pitch allowed to freefloat within defined Angle mode deadband */
3313 if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
3314 if (!FLIGHT_MODE(SOARING_MODE)) {
3315 ENABLE_FLIGHT_MODE(SOARING_MODE);
3316 }
3317 } else {
3318 DISABLE_FLIGHT_MODE(SOARING_MODE);
3319 }
3320 }
3321
3322 // Failsafe_RTH (can override MANUAL)
3323 if (posControl.flags.forcedRTHActivated) {
3324 // If we request forced RTH - attempt to activate it no matter what
3325 // This might switch to emergency landing controller if GPS is unavailable
3326 return NAV_FSM_EVENT_SWITCH_TO_RTH;
3327 }
3328
3329 /* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
3330 * Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
3331 * Also prevent WP falling back to RTH if WP mission planner is active */
3332 const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive;
3333 if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) {
3334 // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
3335 // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold
3336 // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
3337 // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
3338 if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
3339 return NAV_FSM_EVENT_SWITCH_TO_RTH;
3340 }
3341 }
3342
3343 // MANUAL mode has priority over WP/PH/AH
3344 if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
3345 canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
3346 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3347 }
3348
3349 // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
3350 // Block activation if using WP Mission Planner
3351 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
3352 if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
3353 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
3354 }
3355 else {
3356 // Arm the state variable if the WP BOX mode is not enabled
3357 canActivateWaypoint = true;
3358 }
3359
3360 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
3361 if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
3362 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
3363 }
3364
3365 // CRUISE has priority over COURSE_HOLD and AH
3366 if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
3367 if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
3368 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
3369 }
3370
3371 // PH has priority over COURSE_HOLD
3372 // CRUISE has priority on AH
3373 if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
3374
3375 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
3376 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
3377
3378 if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
3379 return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
3380
3381 }
3382
3383 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
3384 if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
3385 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
3386 }
3387 }
3388 else {
3389 canActivateWaypoint = false;
3390
3391 // 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)
3392 canActivateLaunchMode = isNavLaunchEnabled();
3393 }
3394
3395 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
3396 }
3397
3398 /*-----------------------------------------------------------
3399 * An indicator that throttle tilt compensation is forced
3400 *-----------------------------------------------------------*/
3401 bool navigationRequiresThrottleTiltCompensation(void)
3402 {
3403 return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
3404 }
3405
3406 /*-----------------------------------------------------------
3407 * An indicator that ANGLE mode must be forced per NAV requirement
3408 *-----------------------------------------------------------*/
3409 bool navigationRequiresAngleMode(void)
3410 {
3411 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
3412 return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
3413 }
3414
3415 /*-----------------------------------------------------------
3416 * An indicator that TURN ASSISTANCE is required for navigation
3417 *-----------------------------------------------------------*/
3418 bool navigationRequiresTurnAssistance(void)
3419 {
3420 const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
3421 if (STATE(FIXED_WING_LEGACY)) {
3422 // For airplanes turn assistant is always required when controlling position
3423 return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
3424 }
3425 else {
3426 return false;
3427 }
3428 }
3429
3430 /**
3431 * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
3432 */
3433 int8_t navigationGetHeadingControlState(void)
3434 {
3435 // For airplanes report as manual heading control
3436 if (STATE(FIXED_WING_LEGACY)) {
3437 return NAV_HEADING_CONTROL_MANUAL;
3438 }
3439
3440 // For multirotors it depends on navigation system mode
3441 if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
3442 if (posControl.flags.isAdjustingHeading) {
3443 return NAV_HEADING_CONTROL_MANUAL;
3444 }
3445 else {
3446 return NAV_HEADING_CONTROL_AUTO;
3447 }
3448 }
3449 else {
3450 return NAV_HEADING_CONTROL_NONE;
3451 }
3452 }
3453
3454 bool navigationTerrainFollowingEnabled(void)
3455 {
3456 return posControl.flags.isTerrainFollowEnabled;
3457 }
3458
3459 uint32_t distanceToFirstWP(void)
3460 {
3461 fpVector3_t startingWaypointPos;
3462 #ifdef USE_MULTI_MISSION
3463 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE);
3464 #else
3465 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
3466 #endif
3467 return calculateDistanceToDestination(&startingWaypointPos);
3468 }
3469
3470 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
3471 {
3472 const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
3473 const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
3474
3475 if (usedBypass) {
3476 *usedBypass = false;
3477 }
3478
3479 if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_OFF) {
3480 return NAV_ARMING_BLOCKER_NONE;
3481 }
3482
3483 // Apply extra arming safety only if pilot has any of GPS modes configured
3484 if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
3485 if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
3486 (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
3487 if (usedBypass) {
3488 *usedBypass = true;
3489 }
3490 return NAV_ARMING_BLOCKER_NONE;
3491 }
3492 return NAV_ARMING_BLOCKER_MISSING_GPS_FIX;
3493 }
3494
3495 // Don't allow arming if any of NAV modes is active
3496 if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
3497 return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
3498 }
3499
3500 // Don't allow arming if first waypoint is farther than configured safe distance
3501 if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
3502 if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
3503 return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
3504 }
3505 }
3506
3507 /*
3508 * Don't allow arming if any of JUMP waypoint has invalid settings
3509 * First WP can't be JUMP
3510 * Can't jump to immediately adjacent WPs (pointless)
3511 * Can't jump beyond WP list
3512 * Only jump to geo-referenced WP types
3513 */
3514 #ifdef USE_MULTI_MISSION
3515 // Only perform check when mission loaded at start of posControl.waypointList
3516 if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
3517 #else
3518 if (posControl.waypointCount) {
3519 #endif
3520 for (uint8_t wp = 0; wp < posControl.waypointCount; wp++){
3521 if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
3522 if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >= posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
3523 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
3524 }
3525 /* check for target geo-ref sanity */
3526 uint16_t target = posControl.waypointList[wp].p1;
3527 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)) {
3528 return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
3529 }
3530 }
3531 }
3532 }
3533
3534 return NAV_ARMING_BLOCKER_NONE;
3535 }
3536
3537 bool navigationPositionEstimateIsHealthy(void)
3538 {
3539 return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
3540 }
3541
3542 /**
3543 * Indicate ready/not ready status
3544 */
3545 static void updateReadyStatus(void)
3546 {
3547 static bool posReadyBeepDone = false;
3548
3549 /* Beep out READY_BEEP once when position lock is firstly acquired and HOME set */
3550 if (navigationPositionEstimateIsHealthy() && !posReadyBeepDone) {
3551 beeper(BEEPER_READY_BEEP);
3552 posReadyBeepDone = true;
3553 }
3554 }
3555
3556 void updateFlightBehaviorModifiers(void)
3557 {
3558 if (posControl.flags.isGCSAssistedNavigationEnabled && !IS_RC_MODE_ACTIVE(BOXGCSNAV)) {
3559 posControl.flags.isGCSAssistedNavigationReset = true;
3560 }
3561
3562 posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
3563 }
3564
3565 /* On the fly WP mission planner mode allows WP missions to be setup during navigation.
3566 * Uses the WP mode switch to save WP at current location (WP mode disabled when active)
3567 * Mission can be flown after mission planner mode switched off and saved after disarm. */
3568
3569 void updateWpMissionPlanner(void)
3570 {
3571 static timeMs_t resetTimerStart = 0;
3572 if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
3573 const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
3574
3575 posControl.flags.wpMissionPlannerActive = true;
3576 if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
3577 posControl.waypointCount = posControl.wpPlannerActiveWPIndex = 0;
3578 posControl.waypointListValid = false;
3579 posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
3580 }
3581 if (positionTrusted && posControl.wpMissionPlannerStatus != WP_PLAN_FULL) {
3582 missionPlannerSetWaypoint();
3583 } else {
3584 posControl.wpMissionPlannerStatus = posControl.wpMissionPlannerStatus == WP_PLAN_FULL ? WP_PLAN_FULL : WP_PLAN_WAIT;
3585 }
3586 } else if (posControl.flags.wpMissionPlannerActive) {
3587 posControl.flags.wpMissionPlannerActive = false;
3588 posControl.activeWaypointIndex = 0;
3589 resetTimerStart = millis();
3590 }
3591 }
3592
3593 void missionPlannerSetWaypoint(void)
3594 {
3595 static bool boxWPModeIsReset = true;
3596
3597 boxWPModeIsReset = !boxWPModeIsReset ? !IS_RC_MODE_ACTIVE(BOXNAVWP) : boxWPModeIsReset; // only able to save new WP when WP mode reset
3598 posControl.wpMissionPlannerStatus = boxWPModeIsReset ? boxWPModeIsReset : posControl.wpMissionPlannerStatus; // hold save status until WP mode reset
3599
3600 if (!boxWPModeIsReset || !IS_RC_MODE_ACTIVE(BOXNAVWP)) {
3601 return;
3602 }
3603
3604 if (!posControl.wpPlannerActiveWPIndex) { // reset existing mission data before adding first WP
3605 resetWaypointList();
3606 }
3607
3608 gpsLocation_t wpLLH;
3609 geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
3610
3611 posControl.waypointList[posControl.wpPlannerActiveWPIndex].action = 1;
3612 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lat = wpLLH.lat;
3613 posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
3614 posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
3615 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
3616 posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
3617 posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
3618 posControl.waypointListValid = true;
3619
3620 if (posControl.wpPlannerActiveWPIndex) {
3621 posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
3622 }
3623
3624 posControl.wpPlannerActiveWPIndex += 1;
3625 posControl.waypointCount = posControl.geoWaypointCount = posControl.wpPlannerActiveWPIndex;
3626 posControl.wpMissionPlannerStatus = posControl.waypointCount == NAV_MAX_WAYPOINTS ? WP_PLAN_FULL : WP_PLAN_OK;
3627 boxWPModeIsReset = false;
3628 }
3629
3630 /**
3631 * Process NAV mode transition and WP/RTH state machine
3632 * Update rate: RX (data driven or 50Hz)
3633 */
3634 void updateWaypointsAndNavigationMode(void)
3635 {
3636 /* Initiate home position update */
3637 updateHomePosition();
3638
3639 /* Update flight statistics */
3640 updateNavigationFlightStatistics();
3641
3642 /* Update NAV ready status */
3643 updateReadyStatus();
3644
3645 // Update flight behaviour modifiers
3646 updateFlightBehaviorModifiers();
3647
3648 // Process switch to a different navigation mode (if needed)
3649 navProcessFSMEvents(selectNavEventFromBoxModeInput());
3650
3651 // Process pilot's RC input to adjust behaviour
3652 processNavigationRCAdjustments();
3653
3654 // Map navMode back to enabled flight modes
3655 switchNavigationFlightModes();
3656
3657 // Update WP mission planner
3658 updateWpMissionPlanner();
3659
3660 //Update Blackbox data
3661 navCurrentState = (int16_t)posControl.navPersistentId;
3662 }
3663
3664 /*-----------------------------------------------------------
3665 * NAV main control functions
3666 *-----------------------------------------------------------*/
3667 void navigationUsePIDs(void)
3668 {
3669 /** Multicopter PIDs */
3670 // Brake time parameter
3671 posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f;
3672
3673 // Position controller expo (taret vel expo for MC)
3674 posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f);
3675
3676 // Initialize position hold P-controller
3677 for (int axis = 0; axis < 2; axis++) {
3678 navPidInit(
3679 &posControl.pids.pos[axis],
3680 (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
3681 0.0f,
3682 0.0f,
3683 0.0f,
3684 NAV_DTERM_CUT_HZ
3685 );
3686
3687 navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
3688 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
3689 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
3690 (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
3691 pidProfile()->navVelXyDTermLpfHz
3692 );
3693 }
3694
3695 /*
3696 * Set coefficients used in MC VEL_XY
3697 */
3698 multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f;
3699 multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f;
3700 multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f;
3701
3702 #ifdef USE_MR_BRAKING_MODE
3703 multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;
3704 #endif
3705
3706 // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
3707 navPidInit(
3708 &posControl.pids.pos[Z],
3709 (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
3710 0.0f,
3711 0.0f,
3712 0.0f,
3713 NAV_DTERM_CUT_HZ
3714 );
3715
3716 navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
3717 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
3718 (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
3719 0.0f,
3720 NAV_DTERM_CUT_HZ
3721 );
3722
3723 // Initialize surface tracking PID
3724 navPidInit(&posControl.pids.surface, 2.0f,
3725 0.0f,
3726 0.0f,
3727 0.0f,
3728 NAV_DTERM_CUT_HZ
3729 );
3730
3731 /** Airplane PIDs */
3732 // Initialize fixed wing PID controllers
3733 navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
3734 (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
3735 (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
3736 0.0f,
3737 NAV_DTERM_CUT_HZ
3738 );
3739
3740 navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
3741 (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
3742 (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
3743 0.0f,
3744 NAV_DTERM_CUT_HZ
3745 );
3746
3747 navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
3748 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
3749 (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
3750 0.0f,
3751 2.0f
3752 );
3753 }
3754
3755 void navigationInit(void)
3756 {
3757 /* Initial state */
3758 posControl.navState = NAV_STATE_IDLE;
3759
3760 posControl.flags.horizontalPositionDataNew = false;
3761 posControl.flags.verticalPositionDataNew = false;
3762
3763 posControl.flags.estAltStatus = EST_NONE;
3764 posControl.flags.estPosStatus = EST_NONE;
3765 posControl.flags.estVelStatus = EST_NONE;
3766 posControl.flags.estHeadingStatus = EST_NONE;
3767 posControl.flags.estAglStatus = EST_NONE;
3768
3769 posControl.flags.forcedRTHActivated = false;
3770 posControl.flags.forcedEmergLandingActivated = false;
3771 posControl.waypointCount = 0;
3772 posControl.activeWaypointIndex = 0;
3773 posControl.waypointListValid = false;
3774 posControl.wpPlannerActiveWPIndex = 0;
3775 posControl.flags.wpMissionPlannerActive = false;
3776 #ifdef USE_MULTI_MISSION
3777 posControl.multiMissionCount = 0;
3778 posControl.loadedMultiMissionStartWP = 0;
3779 #endif
3780 /* Set initial surface invalid */
3781 posControl.actualState.surfaceMin = -1.0f;
3782
3783 /* Reset statistics */
3784 posControl.totalTripDistance = 0.0f;
3785
3786 /* Use system config */
3787 navigationUsePIDs();
3788
3789 if (
3790 mixerConfig()->platformType == PLATFORM_BOAT ||
3791 mixerConfig()->platformType == PLATFORM_ROVER ||
3792 navConfig()->fw.useFwNavYawControl
3793 ) {
3794 ENABLE_STATE(FW_HEADING_USE_YAW);
3795 } else {
3796 DISABLE_STATE(FW_HEADING_USE_YAW);
3797 }
3798 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
3799 /* configure WP missions at boot */
3800 #ifdef USE_MULTI_MISSION
3801 for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { // check number missions in NVM
3802 if (checkMissionCount(i)) {
3803 break;
3804 }
3805 }
3806 /* set index to 1 if saved mission index > available missions */
3807 if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
3808 navConfigMutable()->general.waypoint_multi_mission_index = 1;
3809 }
3810 #endif
3811 /* load mission on boot */
3812 if (navConfig()->general.waypoint_load_on_boot) {
3813 loadNonVolatileWaypointList(false);
3814 }
3815 #endif
3816 }
3817
3818 /*-----------------------------------------------------------
3819 * Access to estimated position/velocity data
3820 *-----------------------------------------------------------*/
3821 float getEstimatedActualVelocity(int axis)
3822 {
3823 return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
3824 }
3825
3826 float getEstimatedActualPosition(int axis)
3827 {
3828 return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
3829 }
3830
3831 /*-----------------------------------------------------------
3832 * Ability to execute RTH on external event
3833 *-----------------------------------------------------------*/
3834 void activateForcedRTH(void)
3835 {
3836 abortFixedWingLaunch();
3837 posControl.flags.forcedRTHActivated = true;
3838 checkSafeHomeState(true);
3839 navProcessFSMEvents(selectNavEventFromBoxModeInput());
3840 }
3841
3842 void abortForcedRTH(void)
3843 {
3844 // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
3845 // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
3846 posControl.flags.forcedRTHActivated = false;
3847 checkSafeHomeState(false);
3848 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
3849 }
3850
3851 rthState_e getStateOfForcedRTH(void)
3852 {
3853 /* If forced RTH activated and in AUTO_RTH or EMERG state */
3854 if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
3855 if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
3856 return RTH_HAS_LANDED;
3857 }
3858 else {
3859 return RTH_IN_PROGRESS;
3860 }
3861 }
3862 else {
3863 return RTH_IDLE;
3864 }
3865 }
3866
3867 /*-----------------------------------------------------------
3868 * Ability to execute Emergency Landing on external event
3869 *-----------------------------------------------------------*/
3870 void activateForcedEmergLanding(void)
3871 {
3872 abortFixedWingLaunch();
3873 posControl.flags.forcedEmergLandingActivated = true;
3874 navProcessFSMEvents(selectNavEventFromBoxModeInput());
3875 }
3876
3877 void abortForcedEmergLanding(void)
3878 {
3879 // Disable emergency landing and make sure we back out of navigation mode to IDLE
3880 // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update
3881 posControl.flags.forcedEmergLandingActivated = false;
3882 navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
3883 }
3884
3885 emergLandState_e getStateOfForcedEmergLanding(void)
3886 {
3887 /* If forced emergency landing activated and in EMERG state */
3888 if (posControl.flags.forcedEmergLandingActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_EMERG)) {
3889 if (posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
3890 return EMERG_LAND_HAS_LANDED;
3891 } else {
3892 return EMERG_LAND_IN_PROGRESS;
3893 }
3894 } else {
3895 return EMERG_LAND_IDLE;
3896 }
3897 }
3898
3899 bool isWaypointMissionRTHActive(void)
3900 {
3901 return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
3902 }
3903
3904 bool navigationIsExecutingAnEmergencyLanding(void)
3905 {
3906 return navGetCurrentStateFlags() & NAV_CTL_EMERG;
3907 }
3908
3909 bool navigationInAutomaticThrottleMode(void)
3910 {
3911 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3912 return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
3913 }
3914
3915 bool navigationIsControllingThrottle(void)
3916 {
3917 // Note that this makes a detour into mixer code to evaluate actual motor status
3918 return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE);
3919 }
3920
3921 bool navigationIsControllingAltitude(void) {
3922 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3923 return (stateFlags & NAV_CTL_ALT);
3924 }
3925
3926 bool navigationIsFlyingAutonomousMode(void)
3927 {
3928 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
3929 return (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
3930 }
3931
3932 bool navigationRTHAllowsLanding(void)
3933 {
3934 // WP mission RTH landing setting
3935 if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
3936 return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
3937 }
3938
3939 // normal RTH landing setting
3940 navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
3941 return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
3942 (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
3943 }
3944
3945 bool isNavLaunchEnabled(void)
3946 {
3947 return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
3948 }
3949
3950 int32_t navigationGetHomeHeading(void)
3951 {
3952 return posControl.rthState.homePosition.yaw;
3953 }
3954
3955 // returns m/s
3956 float calculateAverageSpeed() {
3957 float flightTime = getFlightTime();
3958 if (flightTime == 0.0f) return 0;
3959 return (float)getTotalTravelDistance() / (flightTime * 100);
3960 }
3961
3962 const navigationPIDControllers_t* getNavigationPIDControllers(void) {
3963 return &posControl.pids;
3964 }
3965
3966 bool isAdjustingPosition(void) {
3967 return posControl.flags.isAdjustingPosition;
3968 }
3969
3970 bool isAdjustingHeading(void) {
3971 return posControl.flags.isAdjustingHeading;
3972 }
3973
3974 int32_t getCruiseHeadingAdjustment(void) {
3975 return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
3976 }
INAV: Navigation-enabled flight control software