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