?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
WP: Allow WP mission to be activated only by flipping a switch through off-on sequenc...
[betaflight.git] / src / main / flight / navigation_rewrite.c
blob01957e96e21e2c5a3713245c28537f8193271773
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
22 #include "build_config.h"
23 #include "platform.h"
24 #include "debug.h"
25
26 #include "common/axis.h"
27 #include "common/maths.h"
28 #include "common/filter.h"
29
30 #include "drivers/system.h"
31 #include "drivers/sensor.h"
32 #include "drivers/accgyro.h"
33
34 #include "sensors/sensors.h"
35 #include "sensors/acceleration.h"
36 #include "sensors/boardalignment.h"
37
38 #include "io/beeper.h"
39 #include "io/gps.h"
40
41 #include "flight/pid.h"
42 #include "flight/imu.h"
43 #include "flight/navigation_rewrite.h"
44 #include "flight/navigation_rewrite_private.h"
45
46 #include "config/runtime_config.h"
47 #include "config/config.h"
48
49 /*-----------------------------------------------------------
50 * Compatibility for home position
51 *-----------------------------------------------------------*/
52 gpsLocation_t GPS_home;
53 uint16_t GPS_distanceToHome; // distance to home point in meters
54 int16_t GPS_directionToHome; // direction to home point in degrees
55
56 #if defined(NAV)
57 navigationPosControl_t posControl;
58 navSystemStatus_t NAV_Status;
59
60 #if defined(NAV_BLACKBOX)
61 int16_t navCurrentState;
62 int16_t navActualVelocity[3];
63 int16_t navDesiredVelocity[3];
64 int16_t navActualHeading;
65 int16_t navDesiredHeading;
66 int16_t navTargetPosition[3];
67 int32_t navLatestActualPosition[3];
68 int16_t navDebug[4];
69 int16_t navTargetSurface;
70 int16_t navActualSurface;
71 uint16_t navFlags;
72 #endif
73
74 static void updateDesiredRTHAltitude(void);
75 static void resetAltitudeController(void);
76 static void resetPositionController(void);
77 static void setupAltitudeController(void);
78 void resetNavigation(void);
79
80 static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint);
81 static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos);
82 void calculateInitialHoldPosition(t_fp_vector * pos);
83 void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance);
84
85 /*************************************************************************************************/
86 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
87 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
88 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
89 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState);
90 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState);
91 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
92 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
93 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
94 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState);
95 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState);
96 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navigationFSMState_t previousState);
97 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState);
98 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState);
99 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState);
100 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
101 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState);
102 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_GPS_FAILING(navigationFSMState_t previousState);
103 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
104 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState);
105 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState);
106 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState);
107 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
108 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
109 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
110 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
111 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
112 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
113 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
114 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
115
116 static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
117 /** Idle state ******************************************************/
118 [NAV_STATE_IDLE] = {
119 .onEntry = navOnEnteringState_NAV_STATE_IDLE,
120 .timeoutMs = 0,
121 .stateFlags = 0,
122 .mapToFlightModes = 0,
123 .mwState = MW_NAV_STATE_NONE,
124 .mwError = MW_NAV_ERROR_NONE,
125 .onEvent = {
126 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
127 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
128 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
129 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
130 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
131 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
132 }
133 },
134
135 /** ALTHOLD mode ***************************************************/
136 [NAV_STATE_ALTHOLD_INITIALIZE] = {
137 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
138 .timeoutMs = 0,
139 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
140 .mapToFlightModes = NAV_ALTHOLD_MODE,
141 .mwState = MW_NAV_STATE_NONE,
142 .mwError = MW_NAV_ERROR_NONE,
143 .onEvent = {
144 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS,
145 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
146 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
147 }
148 },
149
150 [NAV_STATE_ALTHOLD_IN_PROGRESS] = {
151 .onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
152 .timeoutMs = 10,
153 .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
154 .mapToFlightModes = NAV_ALTHOLD_MODE,
155 .mwState = MW_NAV_STATE_NONE,
156 .mwError = MW_NAV_ERROR_NONE,
157 .onEvent = {
158 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
159 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
160 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
161 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
162 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
163 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
164 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
165 }
166 },
167
168 /** POSHOLD_2D mode ************************************************/
169 [NAV_STATE_POSHOLD_2D_INITIALIZE] = {
170 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE,
171 .timeoutMs = 0,
172 .stateFlags = NAV_CTL_POS | NAV_REQUIRE_ANGLE,
173 .mapToFlightModes = NAV_POSHOLD_MODE,
174 .mwState = MW_NAV_STATE_HOLD_INFINIT,
175 .mwError = MW_NAV_ERROR_NONE,
176 .onEvent = {
177 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_2D_IN_PROGRESS,
178 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
179 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
180 }
181 },
182
183 [NAV_STATE_POSHOLD_2D_IN_PROGRESS] = {
184 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS,
185 .timeoutMs = 10,
186 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
187 .mapToFlightModes = NAV_POSHOLD_MODE,
188 .mwState = MW_NAV_STATE_HOLD_INFINIT,
189 .mwError = MW_NAV_ERROR_NONE,
190 .onEvent = {
191 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_2D_IN_PROGRESS, // re-process the state
192 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
193 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
194 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
195 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
196 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
197 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
198 }
199 },
200
201 /** POSHOLD_3D mode ************************************************/
202 [NAV_STATE_POSHOLD_3D_INITIALIZE] = {
203 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
204 .timeoutMs = 0,
205 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
206 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
207 .mwState = MW_NAV_STATE_HOLD_INFINIT,
208 .mwError = MW_NAV_ERROR_NONE,
209 .onEvent = {
210 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS,
211 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
212 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
213 }
214 },
215
216 [NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
217 .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
218 .timeoutMs = 10,
219 .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,
220 .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
221 .mwState = MW_NAV_STATE_HOLD_INFINIT,
222 .mwError = MW_NAV_ERROR_NONE,
223 .onEvent = {
224 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
225 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
226 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
227 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
228 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
229 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
230 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
231 }
232 },
233
234 /** RTH mode entry point ************************************************/
235 [NAV_STATE_RTH_INITIALIZE] = {
236 .onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
237 .timeoutMs = 0,
238 .stateFlags = NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
239 .mapToFlightModes = NAV_RTH_MODE,
240 .mwState = MW_NAV_STATE_RTH_START,
241 .mwError = MW_NAV_ERROR_SPOILED_GPS, // we are stuck in this state only if GPS is compromised
242 .onEvent = {
243 [NAV_FSM_EVENT_SWITCH_TO_RTH_2D] = NAV_STATE_RTH_2D_INITIALIZE,
244 [NAV_FSM_EVENT_SWITCH_TO_RTH_3D] = NAV_STATE_RTH_3D_INITIALIZE,
245 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
246 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
247 }
248 },
249
250 /** RTH_2D mode ************************************************/
251 [NAV_STATE_RTH_2D_INITIALIZE] = {
252 .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE,
253 .timeoutMs = 10,
254 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH,
255 .mapToFlightModes = NAV_RTH_MODE,
256 .mwState = MW_NAV_STATE_RTH_START,
257 .mwError = MW_NAV_ERROR_NONE,
258 .onEvent = {
259 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_INITIALIZE, // re-process the state
260 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_HEAD_HOME,
261 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
262 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
263 }
264 },
265
266 [NAV_STATE_RTH_2D_HEAD_HOME] = {
267 .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME,
268 .timeoutMs = 10,
269 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
270 .mapToFlightModes = NAV_RTH_MODE,
271 .mwState = MW_NAV_STATE_RTH_ENROUTE,
272 .mwError = MW_NAV_ERROR_NONE,
273 .onEvent = {
274 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_HEAD_HOME, // re-process the state
275 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_FINISHING,
276 [NAV_FSM_EVENT_ERROR] = NAV_STATE_RTH_2D_GPS_FAILING,
277 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
278 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
279 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
280 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
281 }
282 },
283
284 [NAV_STATE_RTH_2D_GPS_FAILING] = {
285 .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING,
286 .timeoutMs = 10,
287 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
288 .mapToFlightModes = NAV_RTH_MODE,
289 .mwState = MW_NAV_STATE_RTH_ENROUTE,
290 .mwError = MW_NAV_ERROR_SPOILED_GPS,
291 .onEvent = {
292 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_GPS_FAILING, // re-process the state
293 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_HEAD_HOME,
294 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
295 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
296 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
297 }
298 },
299
300 [NAV_STATE_RTH_2D_FINISHING] = {
301 .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHING,
302 .timeoutMs = 0,
303 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
304 .mapToFlightModes = NAV_RTH_MODE,
305 .mwState = MW_NAV_STATE_RTH_ENROUTE,
306 .mwError = MW_NAV_ERROR_NONE,
307 .onEvent = {
308 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_FINISHED,
309 }
310 },
311
312 [NAV_STATE_RTH_2D_FINISHED] = {
313 .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHED,
314 .timeoutMs = 10,
315 .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
316 .mapToFlightModes = NAV_RTH_MODE,
317 .mwState = MW_NAV_STATE_RTH_ENROUTE,
318 .mwError = MW_NAV_ERROR_NONE,
319 .onEvent = {
320 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_FINISHED, // re-process the state
321 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
322 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
323 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
324 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
325 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
326 }
327 },
328
329 /** RTH_3D mode ************************************************/
330 [NAV_STATE_RTH_3D_INITIALIZE] = {
331 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE,
332 .timeoutMs = 10,
333 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
334 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
335 .mwState = MW_NAV_STATE_RTH_START,
336 .mwError = MW_NAV_ERROR_NONE,
337 .onEvent = {
338 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_INITIALIZE, // re-process the state
339 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
340 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
341 [NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
342 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
343 }
344 },
345
346 [NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT] = {
347 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
348 .timeoutMs = 10,
349 .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
350 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
351 .mwState = MW_NAV_STATE_RTH_ENROUTE,
352 .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
353 .onEvent = {
354 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // re-process the state
355 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HEAD_HOME,
356 [NAV_FSM_EVENT_ERROR] = NAV_STATE_RTH_3D_GPS_FAILING,
357 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
358 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
359 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
360 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
361 }
362 },
363
364 [NAV_STATE_RTH_3D_HEAD_HOME] = {
365 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME,
366 .timeoutMs = 10,
367 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
368 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
369 .mwState = MW_NAV_STATE_RTH_ENROUTE,
370 .mwError = MW_NAV_ERROR_NONE,
371 .onEvent = {
372 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_HEAD_HOME, // re-process the state
373 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
374 [NAV_FSM_EVENT_ERROR] = NAV_STATE_RTH_3D_GPS_FAILING,
375 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
376 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
377 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
378 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
379 }
380 },
381
382 [NAV_STATE_RTH_3D_GPS_FAILING] = {
383 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_GPS_FAILING,
384 .timeoutMs = 10,
385 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
386 .mapToFlightModes = NAV_RTH_MODE,
387 .mwState = MW_NAV_STATE_RTH_ENROUTE,
388 .mwError = MW_NAV_ERROR_SPOILED_GPS,
389 .onEvent = {
390 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_GPS_FAILING, // re-process the state
391 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HEAD_HOME,
392 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
393 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
394 }
395 },
396
397 [NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING] = {
398 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
399 .timeoutMs = 5000,
400 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
401 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
402 .mwState = MW_NAV_STATE_LAND_SETTLE,
403 .mwError = MW_NAV_ERROR_NONE,
404 .onEvent = {
405 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_LANDING,
406 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
407 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
408 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
409 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
410 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
411 }
412 },
413
414 [NAV_STATE_RTH_3D_LANDING] = {
415 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_LANDING,
416 .timeoutMs = 10,
417 .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,
418 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
419 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
420 .mwError = MW_NAV_ERROR_LANDING,
421 .onEvent = {
422 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_LANDING, // re-process state
423 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHING,
424 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
425 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
426 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
427 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
428 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
429 }
430 },
431
432 [NAV_STATE_RTH_3D_FINISHING] = {
433 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHING,
434 .timeoutMs = 0,
435 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
436 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
437 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
438 .mwError = MW_NAV_ERROR_LANDING,
439 .onEvent = {
440 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHED,
441 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
442 }
443 },
444
445 [NAV_STATE_RTH_3D_FINISHED] = {
446 .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHED,
447 .timeoutMs = 10,
448 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
449 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
450 .mwState = MW_NAV_STATE_LANDED,
451 .mwError = MW_NAV_ERROR_NONE,
452 .onEvent = {
453 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_FINISHED, // re-process state
454 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
455 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
456 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
457 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
458 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
459 }
460 },
461
462 /** WAYPOINT mode ************************************************/
463 [NAV_STATE_WAYPOINT_INITIALIZE] = {
464 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
465 .timeoutMs = 0,
466 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
467 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
468 .mwState = MW_NAV_STATE_PROCESS_NEXT,
469 .mwError = MW_NAV_ERROR_NONE,
470 .onEvent = {
471 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
472 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
473 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
474 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
475 }
476 },
477
478 [NAV_STATE_WAYPOINT_PRE_ACTION] = {
479 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
480 .timeoutMs = 0,
481 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
482 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
483 .mwState = MW_NAV_STATE_PROCESS_NEXT,
484 .mwError = MW_NAV_ERROR_NONE,
485 .onEvent = {
486 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
487 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
488 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
489 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
490 }
491 },
492
493 [NAV_STATE_WAYPOINT_IN_PROGRESS] = {
494 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
495 .timeoutMs = 10,
496 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
497 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
498 .mwState = MW_NAV_STATE_WP_ENROUTE,
499 .mwError = MW_NAV_ERROR_NONE,
500 .onEvent = {
501 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state
502 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
503 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
504 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
505 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
506 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
507 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
508 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
509 }
510 },
511
512 [NAV_STATE_WAYPOINT_REACHED] = {
513 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
514 .timeoutMs = 0,
515 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
516 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
517 .mwState = MW_NAV_STATE_PROCESS_NEXT,
518 .mwError = MW_NAV_ERROR_NONE,
519 .onEvent = {
520 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
521 [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
522 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
523 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
524 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
525 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
526 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
527 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
528 }
529 },
530
531 [NAV_STATE_WAYPOINT_FINISHED] = {
532 .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
533 .timeoutMs = 0,
534 .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
535 .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
536 .mwState = MW_NAV_STATE_WP_ENROUTE,
537 .mwError = MW_NAV_ERROR_FINISH,
538 .onEvent = {
539 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
540 [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
541 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
542 [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
543 [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
544 [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
545 }
546 },
547
548 /** EMERGENCY LANDING ************************************************/
549 [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
550 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
551 .timeoutMs = 0,
552 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
553 .mapToFlightModes = 0,
554 .mwState = MW_NAV_STATE_LAND_START,
555 .mwError = MW_NAV_ERROR_LANDING,
556 .onEvent = {
557 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
558 [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
559 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
560 }
561 },
562
563 [NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
564 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
565 .timeoutMs = 10,
566 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
567 .mapToFlightModes = 0,
568 .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
569 .mwError = MW_NAV_ERROR_LANDING,
570 .onEvent = {
571 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state
572 [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
573 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
574 }
575 },
576
577 [NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
578 .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
579 .timeoutMs = 10,
580 .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
581 .mapToFlightModes = 0,
582 .mwState = MW_NAV_STATE_LANDED,
583 .mwError = MW_NAV_ERROR_LANDING,
584 .onEvent = {
585 [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
586 [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
587 }
588 },
589 };
590
591 static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
592 {
593 return navFSM[state].stateFlags;
594 }
595
596 static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
597 {
598 return navFSM[state].mapToFlightModes;
599 }
600
601 navigationFSMStateFlags_t navGetCurrentStateFlags(void)
602 {
603 return navGetStateFlags(posControl.navState);
604 }
605
606 /*************************************************************************************************/
607 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
608 {
609 UNUSED(previousState);
610 resetNavigation();
611 return NAV_FSM_EVENT_NONE;
612 }
613
614 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
615 {
616 /* If previous controller was NOT executing NAV_CTL_ALT controller, we must reset altitude setpoint */
617 if ((navGetStateFlags(previousState) & NAV_CTL_ALT) == 0) {
618 resetAltitudeController();
619 setupAltitudeController();
620 setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
621 }
622
623 return NAV_FSM_EVENT_SUCCESS;
624 }
625
626 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState)
627 {
628 UNUSED(previousState);
629
630 // If we enable terrain mode and surface offset is not set yet - do it
631 if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
632 setDesiredSurfaceOffset(posControl.actualState.surface);
633 }
634
635 return NAV_FSM_EVENT_NONE;
636 }
637
638 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState)
639 {
640 navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
641
642 if ((prevFlags & NAV_CTL_POS) == 0) {
643 resetPositionController();
644 }
645
646 if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
647 t_fp_vector targetHoldPos;
648 calculateInitialHoldPosition(&targetHoldPos);
649 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
650 }
651
652 return NAV_FSM_EVENT_SUCCESS;
653 }
654
655 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState)
656 {
657 UNUSED(previousState);
658 return NAV_FSM_EVENT_NONE;
659 }
660
661 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
662 {
663 navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
664
665 if ((prevFlags & NAV_CTL_POS) == 0) {
666 resetPositionController();
667 }
668
669 if ((prevFlags & NAV_CTL_ALT) == 0) {
670 resetAltitudeController();
671 setupAltitudeController();
672 }
673
674 if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
675 setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
676 }
677
678 if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
679 t_fp_vector targetHoldPos;
680 calculateInitialHoldPosition(&targetHoldPos);
681 setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
682 }
683
684 return NAV_FSM_EVENT_SUCCESS;
685 }
686
687 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState)
688 {
689 UNUSED(previousState);
690
691 // If we enable terrain mode and surface offset is not set yet - do it
692 if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
693 setDesiredSurfaceOffset(posControl.actualState.surface);
694 }
695
696 return NAV_FSM_EVENT_NONE;
697 }
698
699 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
700 {
701 if (STATE(GPS_FIX_HOME) && posControl.flags.hasValidHeadingSensor) {
702 navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
703
704 if ((prevFlags & NAV_CTL_POS) == 0) {
705 resetPositionController();
706 }
707
708 if ((prevFlags & NAV_CTL_ALT) == 0) {
709 resetAltitudeController();
710 setupAltitudeController();
711 }
712
713 // Switch between 2D and 3D RTH depending on altitude sensor availability
714 if (posControl.flags.hasValidAltitudeSensor) {
715 // We might have GPS unavailable - in case of 3D RTH set current altitude target
716 setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_Z);
717
718 return NAV_FSM_EVENT_SWITCH_TO_RTH_3D;
719 }
720 else {
721 return NAV_FSM_EVENT_SWITCH_TO_RTH_2D;
722 }
723 }
724 else {
725 // No home position set or no heading sensor
726 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
727 }
728 }
729
730 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState)
731 {
732 UNUSED(previousState);
733
734 if (STATE(FIXED_WING) && (posControl.homeDistance < posControl.navConfig->min_rth_distance)) {
735 // Prevent RTH from activating on airplanes if too close to home
736 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
737 }
738 else {
739 if (posControl.flags.hasValidPositionSensor) {
740 // If close to home - reset home position
741 if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
742 setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
743 }
744
745 return NAV_FSM_EVENT_SUCCESS;
746 }
747 /* Position sensor failure timeout - land */
748 else if (checkForPositionSensorTimeout()) {
749 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
750 }
751 /* No valid POS sensor but still within valid timeout - wait */
752 else {
753 return NAV_FSM_EVENT_NONE;
754 }
755 }
756 }
757
758 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState)
759 {
760 UNUSED(previousState);
761
762 // If no position sensor available - switch to NAV_STATE_RTH_2D_GPS_FAILING
763 if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor)) {
764 return NAV_FSM_EVENT_ERROR; // NAV_STATE_RTH_2D_GPS_FAILING
765 }
766
767 if (isWaypointReached(&posControl.homeWaypointAbove)) {
768 // Successfully reached position target
769 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING
770 }
771 else {
772 // Update XY-position target
773 if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
774 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
775 }
776 else {
777 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
778 }
779
780 return NAV_FSM_EVENT_NONE;
781 }
782 }
783
784 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navigationFSMState_t previousState)
785 {
786 UNUSED(previousState);
787
788 /* Wait for GPS to be online again */
789 if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor && STATE(GPS_FIX_HOME)) {
790 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME
791 }
792 /* No HOME set or position sensor failure timeout - land */
793 else if (!STATE(GPS_FIX_HOME) || !posControl.flags.hasValidHeadingSensor || checkForPositionSensorTimeout()) {
794 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
795 }
796 /* No valid POS sensor but still within valid timeout - wait */
797 else {
798 return NAV_FSM_EVENT_NONE;
799 }
800 }
801
802 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState)
803 {
804 UNUSED(previousState);
805 setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
806 return NAV_FSM_EVENT_SUCCESS;
807 }
808
809 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState)
810 {
811 // Same logic as PH_2D
812 return navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(previousState);
813 }
814
815 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState)
816 {
817 UNUSED(previousState);
818
819 if (STATE(FIXED_WING) && (posControl.homeDistance < posControl.navConfig->min_rth_distance)) {
820 // Prevent RTH from activating on airplanes if too close to home
821 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
822 }
823 else {
824 if (posControl.flags.hasValidPositionSensor) {
825 // If close to home - reset home position and land
826 if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
827 setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
828 setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z);
829
830 return NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
831 }
832 else {
833 t_fp_vector targetHoldPos;
834
835 if (!STATE(FIXED_WING)) {
836 // Multicopter, hover and climb
837 calculateInitialHoldPosition(&targetHoldPos);
838 } else {
839 // Airplane - climbout before turning around
840 calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
841 }
842
843 setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY);
844
845 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT
846 }
847 }
848 /* Position sensor failure timeout - land */
849 else if (checkForPositionSensorTimeout()) {
850 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
851 }
852 /* No valid POS sensor but still within valid timeout - wait */
853 else {
854 return NAV_FSM_EVENT_NONE;
855 }
856 }
857 }
858
859 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
860 {
861 UNUSED(previousState);
862
863 // If no position sensor available - land immediately
864 if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) {
865 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
866 }
867
868 if ((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) {
869 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME
870 }
871 else {
872 // Climb to safe altitude and turn to correct direction
873 if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
874 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
875 }
876 else {
877 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
878 }
879
880 return NAV_FSM_EVENT_NONE;
881 }
882 }
883
884 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState)
885 {
886 UNUSED(previousState);
887
888 // If no position sensor available - land immediately
889 if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor)) {
890 return NAV_FSM_EVENT_ERROR; // NAV_STATE_RTH_3D_GPS_FAILING
891 }
892
893 if (isWaypointReached(&posControl.homeWaypointAbove)) {
894 if (!STATE(FIXED_WING)) {
895 // Successfully reached position target - update XYZ-position
896 setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
897 return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
898 } else {
899 // Don't switch to landing for airplanes
900 return NAV_FSM_EVENT_NONE;
901 }
902 }
903 else {
904 // Update XYZ-position target
905 if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
906 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
907 }
908 else {
909 setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
910 }
911 return NAV_FSM_EVENT_NONE;
912 }
913 }
914
915 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_GPS_FAILING(navigationFSMState_t previousState)
916 {
917 /* Same logic as for 2D GPS RTH */
918 return navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(previousState);
919 }
920
921 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
922 {
923 UNUSED(previousState);
924
925 // If no position sensor available - land immediately
926 if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) {
927 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
928 }
929
930 resetLandingDetector();
931
932 return NAV_FSM_EVENT_NONE;
933 }
934
935 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState)
936 {
937 UNUSED(previousState);
938
939 if (!ARMING_FLAG(ARMED)) {
940 return NAV_FSM_EVENT_SUCCESS;
941 }
942 else if (isLandingDetected()) {
943 return NAV_FSM_EVENT_SUCCESS;
944 }
945 else {
946 // A safeguard - if sonar is available and it is reading < 50cm altitude - drop to low descend speed
947 if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface >= 0 && posControl.actualState.surface < 50.0f) {
948 // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
949 updateAltitudeTargetFromClimbRate(-0.15f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
950 }
951 else {
952 // Gradually reduce descent speed depending on actual altitude.
953 if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1500.0f)) {
954 updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
955 }
956 else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 500.0f)) {
957 updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
958 }
959 else {
960 updateAltitudeTargetFromClimbRate(-0.25f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
961 }
962 }
963
964 return NAV_FSM_EVENT_NONE;
965 }
966 }
967
968 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState)
969 {
970 UNUSED(previousState);
971
972 if (posControl.navConfig->flags.disarm_on_landing) {
973 mwDisarm();
974 }
975
976 return NAV_FSM_EVENT_SUCCESS;
977 }
978
979 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState)
980 {
981 // Stay in this state
982 UNUSED(previousState);
983 updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); // FIXME
984 return NAV_FSM_EVENT_NONE;
985 }
986
987 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState)
988 {
989 UNUSED(previousState);
990
991 if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
992 return NAV_FSM_EVENT_ERROR;
993 }
994 else {
995 // Prepare controllers
996 resetPositionController();
997 resetAltitudeController();
998 setupAltitudeController();
999
1000 posControl.activeWaypointIndex = 0;
1001 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1002 }
1003 }
1004
1005 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
1006 {
1007 /* A helper function to do waypoint-specific action */
1008 UNUSED(previousState);
1009
1010 switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
1011 case NAV_WP_ACTION_WAYPOINT:
1012 calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
1013 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1014
1015 case NAV_WP_ACTION_RTH:
1016 default:
1017 calcualteAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos);
1018 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
1019 };
1020 }
1021
1022 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
1023 {
1024 UNUSED(previousState);
1025
1026 // If no position sensor available - land immediately
1027 if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
1028 switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
1029 case NAV_WP_ACTION_WAYPOINT:
1030 case NAV_WP_ACTION_RTH:
1031 default:
1032 if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
1033 // Waypoint reached
1034 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
1035 }
1036 else {
1037 // Update XY-position target to active waypoint
1038 setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
1039 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1040 }
1041 break;
1042 }
1043 }
1044 /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
1045 else if (checkForPositionSensorTimeout()) {
1046 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1047 }
1048 else {
1049 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1050 }
1051 }
1052
1053 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
1054 {
1055 UNUSED(previousState);
1056
1057 bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
1058 (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
1059
1060 if (isLastWaypoint) {
1061 // Last waypoint reached
1062 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
1063 }
1064 else {
1065 // Waypoint reached, do something and move on to next waypoint
1066 posControl.activeWaypointIndex++;
1067 return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
1068 }
1069 }
1070
1071 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState)
1072 {
1073 UNUSED(previousState);
1074
1075 // If no position sensor available - land immediately
1076 if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
1077 return NAV_FSM_EVENT_NONE;
1078 }
1079 /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
1080 else if (checkForPositionSensorTimeout()) {
1081 return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
1082 }
1083 else {
1084 return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
1085 }
1086 }
1087
1088 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
1089 {
1090 // TODO:
1091 UNUSED(previousState);
1092
1093 // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
1094 resetAltitudeController();
1095
1096 return NAV_FSM_EVENT_SUCCESS;
1097 }
1098
1099 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
1100 {
1101 // TODO:
1102 UNUSED(previousState);
1103 return NAV_FSM_EVENT_NONE;
1104 }
1105
1106 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
1107 {
1108 // TODO:
1109 UNUSED(previousState);
1110 return NAV_FSM_EVENT_SUCCESS;
1111 }
1112
1113 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
1114 {
1115 navigationFSMState_t previousState;
1116
1117 previousState = posControl.navState;
1118 posControl.navState = newState;
1119 return previousState;
1120 }
1121
1122 static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
1123 {
1124 uint32_t currentMillis = millis();
1125 navigationFSMState_t previousState;
1126 static uint32_t lastStateProcessTime = 0;
1127
1128 /* If timeout event defined and timeout reached - switch state */
1129 if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
1130 ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
1131 /* Update state */
1132 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
1133
1134 /* Call new state's entry function */
1135 while (navFSM[posControl.navState].onEntry) {
1136 navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
1137
1138 if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
1139 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
1140 }
1141 else {
1142 break;
1143 }
1144 }
1145
1146 lastStateProcessTime = currentMillis;
1147 }
1148
1149 /* Inject new event */
1150 if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
1151 /* Update state */
1152 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
1153
1154 /* Call new state's entry function */
1155 while (navFSM[posControl.navState].onEntry) {
1156 navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
1157
1158 if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
1159 previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
1160 }
1161 else {
1162 break;
1163 }
1164 }
1165
1166 lastStateProcessTime = currentMillis;
1167 }
1168
1169 /* Update public system state information */
1170 NAV_Status.mode = MW_GPS_MODE_NONE;
1171
1172 if (ARMING_FLAG(ARMED)) {
1173 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
1174
1175 if (navStateFlags & NAV_AUTO_RTH) {
1176 NAV_Status.mode = MW_GPS_MODE_RTH;
1177 }
1178 else if (navStateFlags & NAV_AUTO_WP) {
1179 NAV_Status.mode = MW_GPS_MODE_NAV;
1180 }
1181 else if (navStateFlags & NAV_CTL_EMERG) {
1182 NAV_Status.mode = MW_GPS_MODE_EMERG;
1183 }
1184 else if (navStateFlags & NAV_CTL_POS) {
1185 NAV_Status.mode = MW_GPS_MODE_HOLD;
1186 }
1187 }
1188
1189 NAV_Status.state = navFSM[posControl.navState].mwState;
1190 NAV_Status.error = navFSM[posControl.navState].mwError;
1191
1192 NAV_Status.flags = 0;
1193 if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
1194 if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
1195
1196 NAV_Status.activeWpNumber = posControl.activeWaypointIndex + 1;
1197 NAV_Status.activeWpAction = 0;
1198 if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
1199 NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
1200 }
1201 }
1202
1203 /*-----------------------------------------------------------
1204 * Float point PID-controller implementation
1205 *-----------------------------------------------------------*/
1206 // Implementation of PID with back-calculation I-term anti-windup
1207 // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
1208 // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
1209 float navPidApply2(float setpoint, float measurement, float dt, pidController_t *pid, float outMin, float outMax, bool dTermErrorTracking)
1210 {
1211 float newProportional, newDerivative;
1212 float error = setpoint - measurement;
1213
1214 /* P-term */
1215 newProportional = error * pid->param.kP;
1216
1217 /* D-term */
1218 if (dTermErrorTracking) {
1219 /* Error-tracking D-term */
1220 newDerivative = (error - pid->last_input) / dt;
1221 pid->last_input = error;
1222 }
1223 else {
1224 /* Measurement tracking D-term */
1225 newDerivative = -(measurement - pid->last_input) / dt;
1226 pid->last_input = measurement;
1227 }
1228
1229 newDerivative = pid->param.kD * filterApplyPt1(newDerivative, &pid->dterm_filter_state, NAV_DTERM_CUT_HZ, dt);
1230
1231 /* Pre-calculate output and limit it if actuator is saturating */
1232 float outVal = newProportional + pid->integrator + newDerivative;
1233 float outValConstrained = constrainf(outVal, outMin, outMax);
1234
1235 /* Update I-term */
1236 pid->integrator += (error * pid->param.kI * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
1237
1238 return outValConstrained;
1239 }
1240
1241 void navPidReset(pidController_t *pid)
1242 {
1243 pid->integrator = 0.0f;
1244 pid->last_input = 0.0f;
1245 pid->dterm_filter_state.state = 0.0f;
1246 pid->dterm_filter_state.RC = 0.0f;
1247 }
1248
1249 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
1250 {
1251 pid->param.kP = _kP;
1252 pid->param.kI = _kI;
1253 pid->param.kD = _kD;
1254
1255 if (_kI > 1e-6f && _kP > 1e-6f) {
1256 float Ti = _kP / _kI;
1257 float Td = _kD / _kP;
1258 pid->param.kT = 2.0f / (Ti + Td);
1259 }
1260 else {
1261 pid->param.kI = 0.0;
1262 pid->param.kT = 0.0;
1263 }
1264
1265 navPidReset(pid);
1266 }
1267
1268 /*-----------------------------------------------------------
1269 * Float point P-controller implementation
1270 *-----------------------------------------------------------*/
1271 void navPInit(pController_t *p, float _kP)
1272 {
1273 p->param.kP = _kP;
1274 }
1275
1276 /*-----------------------------------------------------------
1277 * Detects if thrust vector is facing downwards
1278 *-----------------------------------------------------------*/
1279 bool isThrustFacingDownwards(void)
1280 {
1281 // Tilt angle <= 80 deg; cos(80) = 0.17364817766693034885171662676931
1282 return (calculateCosTiltAngle() >= 0.173648178f);
1283 }
1284
1285 /*-----------------------------------------------------------
1286 * Checks if position sensor (GPS) is failing for a specified timeout (if enabled)
1287 *-----------------------------------------------------------*/
1288 bool checkForPositionSensorTimeout(void)
1289 {
1290 if (posControl.navConfig->pos_failure_timeout) {
1291 if (!posControl.flags.hasValidPositionSensor && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * posControl.navConfig->pos_failure_timeout))) {
1292 return true;
1293 }
1294 else {
1295 return false;
1296 }
1297 }
1298 else {
1299 // Timeout not defined, never fail
1300 return false;
1301 }
1302 }
1303
1304 /*-----------------------------------------------------------
1305 * Processes an update to XY-position and velocity
1306 *-----------------------------------------------------------*/
1307 void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY)
1308 {
1309 posControl.actualState.pos.V.X = newX;
1310 posControl.actualState.pos.V.Y = newY;
1311
1312 posControl.actualState.vel.V.X = newVelX;
1313 posControl.actualState.vel.V.Y = newVelY;
1314
1315 posControl.flags.hasValidPositionSensor = hasValidSensor;
1316 posControl.flags.hasValidHeadingSensor = isImuHeadingValid();
1317
1318 if (hasValidSensor) {
1319 posControl.flags.horizontalPositionNewData = 1;
1320 posControl.lastValidPositionTimeMs = millis();
1321 }
1322 else {
1323 posControl.flags.horizontalPositionNewData = 0;
1324 }
1325
1326 #if defined(NAV_BLACKBOX)
1327 navLatestActualPosition[X] = newX;
1328 navLatestActualPosition[Y] = newY;
1329 navActualVelocity[X] = constrain(newVelX, -32678, 32767);
1330 navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
1331 #endif
1332 }
1333
1334 /*-----------------------------------------------------------
1335 * Processes an update to Z-position and velocity
1336 *-----------------------------------------------------------*/
1337 void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity)
1338 {
1339 posControl.actualState.pos.V.Z = newAltitude;
1340 posControl.actualState.vel.V.Z = newVelocity;
1341
1342 posControl.flags.hasValidAltitudeSensor = hasValidSensor;
1343
1344 // Update altitude that would be used when executing RTH
1345 if (hasValidSensor) {
1346 updateDesiredRTHAltitude();
1347 posControl.flags.verticalPositionNewData = 1;
1348 posControl.lastValidAltitudeTimeMs = millis();
1349 }
1350 else {
1351 posControl.flags.verticalPositionNewData = 0;
1352 }
1353
1354 #if defined(NAV_BLACKBOX)
1355 navLatestActualPosition[Z] = constrain(newAltitude, -32678, 32767);
1356 navActualVelocity[Z] = constrain(newVelocity, -32678, 32767);
1357 #endif
1358 }
1359
1360 /*-----------------------------------------------------------
1361 * Processes an update to surface distance
1362 *-----------------------------------------------------------*/
1363 void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity)
1364 {
1365 posControl.actualState.surface = surfaceDistance;
1366 posControl.actualState.surfaceVel = surfaceVelocity;
1367
1368 if (ARMING_FLAG(ARMED)) {
1369 if (surfaceDistance > 0) {
1370 if (posControl.actualState.surfaceMin > 0) {
1371 posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
1372 }
1373 else {
1374 posControl.actualState.surfaceMin = surfaceDistance;
1375 }
1376 }
1377 }
1378 else {
1379 posControl.actualState.surfaceMin = -1;
1380 }
1381
1382 posControl.flags.hasValidSurfaceSensor = hasValidSensor;
1383
1384 if (hasValidSensor) {
1385 posControl.flags.surfaceDistanceNewData = 1;
1386 }
1387 else {
1388 posControl.flags.surfaceDistanceNewData = 0;
1389 }
1390
1391 #if defined(NAV_BLACKBOX)
1392 navActualSurface = surfaceDistance;
1393 #endif
1394 }
1395
1396 /*-----------------------------------------------------------
1397 * Processes an update to estimated heading
1398 *-----------------------------------------------------------*/
1399 void updateActualHeading(int32_t newHeading)
1400 {
1401 /* Update heading */
1402 posControl.actualState.yaw = newHeading;
1403
1404 /* Precompute sin/cos of yaw angle */
1405 posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
1406 posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
1407
1408 posControl.flags.headingNewData = 1;
1409 }
1410
1411 /*-----------------------------------------------------------
1412 * Calculates distance and bearing to destination point
1413 *-----------------------------------------------------------*/
1414 uint32_t calculateDistanceToDestination(t_fp_vector * destinationPos)
1415 {
1416 float deltaX = destinationPos->V.X - posControl.actualState.pos.V.X;
1417 float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y;
1418
1419 return sqrtf(sq(deltaX) + sq(deltaY));
1420 }
1421
1422 int32_t calculateBearingToDestination(t_fp_vector * destinationPos)
1423 {
1424 float deltaX = destinationPos->V.X - posControl.actualState.pos.V.X;
1425 float deltaY = destinationPos->V.Y - posControl.actualState.pos.V.Y;
1426
1427 return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
1428 }
1429
1430 /*-----------------------------------------------------------
1431 * Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing
1432 *-----------------------------------------------------------*/
1433 bool isWaypointMissed(navWaypointPosition_t * waypoint)
1434 {
1435 int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
1436 bearingError = wrap_18000(bearingError);
1437
1438 return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
1439 }
1440
1441 bool isWaypointReached(navWaypointPosition_t * waypoint)
1442 {
1443 // We consider waypoint reached if within specified radius
1444 uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos);
1445 return (wpDistance <= posControl.navConfig->waypoint_radius);
1446 }
1447
1448 static void updateHomePositionCompatibility(void)
1449 {
1450 geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.homePosition.pos, &GPS_home);
1451 GPS_distanceToHome = posControl.homeDistance / 100;
1452 GPS_directionToHome = posControl.homeDirection / 100;
1453 }
1454
1455 /*-----------------------------------------------------------
1456 * Reset home position to current position
1457 *-----------------------------------------------------------*/
1458 static void updateDesiredRTHAltitude(void)
1459 {
1460 if (ARMING_FLAG(ARMED)) {
1461 if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
1462 switch (posControl.navConfig->flags.rth_alt_control_style) {
1463 case NAV_RTH_NO_ALT:
1464 posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
1465 break;
1466 case NAX_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
1467 posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z + posControl.navConfig->rth_altitude;
1468 break;
1469 case NAV_RTH_MAX_ALT:
1470 posControl.homeWaypointAbove.pos.V.Z = MAX(posControl.homeWaypointAbove.pos.V.Z, posControl.actualState.pos.V.Z);
1471 break;
1472 case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
1473 posControl.homeWaypointAbove.pos.V.Z = MAX(posControl.homePosition.pos.V.Z + posControl.navConfig->rth_altitude, posControl.actualState.pos.V.Z);
1474 break;
1475 case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
1476 default:
1477 posControl.homeWaypointAbove.pos.V.Z = posControl.homePosition.pos.V.Z + posControl.navConfig->rth_altitude;
1478 break;
1479 }
1480 }
1481 }
1482 else {
1483 posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
1484 }
1485 }
1486
1487 /*-----------------------------------------------------------
1488 * Reset home position to current position
1489 *-----------------------------------------------------------*/
1490 void setHomePosition(t_fp_vector * pos, int32_t yaw)
1491 {
1492 posControl.homePosition.pos = *pos;
1493 posControl.homePosition.yaw = yaw;
1494
1495 posControl.homeDistance = 0;
1496 posControl.homeDirection = 0;
1497
1498 // Update target RTH altitude as a waypoint above home
1499 posControl.homeWaypointAbove = posControl.homePosition;
1500 updateDesiredRTHAltitude();
1501
1502 updateHomePositionCompatibility();
1503 ENABLE_STATE(GPS_FIX_HOME);
1504 }
1505
1506 /*-----------------------------------------------------------
1507 * Update home position, calculate distance and bearing to home
1508 *-----------------------------------------------------------*/
1509 void updateHomePosition(void)
1510 {
1511 // Disarmed and have a valid position, constantly update home
1512 if (!ARMING_FLAG(ARMED)) {
1513 if (posControl.flags.hasValidPositionSensor) {
1514 setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
1515 }
1516 }
1517 else {
1518 // If pilot so desires he may reset home position to current position
1519 if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && posControl.flags.hasValidPositionSensor) {
1520 setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
1521 }
1522
1523 // Update distance and direction to home if armed (home is not updated when armed)
1524 if (STATE(GPS_FIX_HOME)) {
1525 posControl.homeDistance = calculateDistanceToDestination(&posControl.homePosition.pos);
1526 posControl.homeDirection = calculateBearingToDestination(&posControl.homePosition.pos);
1527 updateHomePositionCompatibility();
1528 }
1529 }
1530 }
1531
1532 /*-----------------------------------------------------------
1533 * Set surface tracking target
1534 *-----------------------------------------------------------*/
1535 void setDesiredSurfaceOffset(float surfaceOffset)
1536 {
1537 if (surfaceOffset > 0) {
1538 posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE);
1539 }
1540 else {
1541 posControl.desiredState.surface = -1;
1542 }
1543
1544 #if defined(NAV_BLACKBOX)
1545 navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
1546 #endif
1547 }
1548
1549 /*-----------------------------------------------------------
1550 * Calculate platform-specific hold position (account for deceleration)
1551 *-----------------------------------------------------------*/
1552 void calculateInitialHoldPosition(t_fp_vector * pos)
1553 {
1554 if (STATE(FIXED_WING)) { // FIXED_WING
1555 calculateFixedWingInitialHoldPosition(pos);
1556 }
1557 else {
1558 calculateMulticopterInitialHoldPosition(pos);
1559 }
1560 }
1561
1562 /*-----------------------------------------------------------
1563 * Set active XYZ-target and desired heading
1564 *-----------------------------------------------------------*/
1565 void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask)
1566 {
1567 // XY-position
1568 if ((useMask & NAV_POS_UPDATE_XY) != 0) {
1569 posControl.desiredState.pos.V.X = pos->V.X;
1570 posControl.desiredState.pos.V.Y = pos->V.Y;
1571 }
1572
1573 // Z-position
1574 if ((useMask & NAV_POS_UPDATE_Z) != 0) {
1575 posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking
1576 posControl.desiredState.pos.V.Z = pos->V.Z;
1577 }
1578
1579 // Heading
1580 if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
1581 // Heading
1582 posControl.desiredState.yaw = yaw;
1583 }
1584 else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) {
1585 posControl.desiredState.yaw = calculateBearingToDestination(pos);
1586 }
1587 else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
1588 posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
1589 }
1590
1591 #if defined(NAV_BLACKBOX)
1592 navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
1593 navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
1594 navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
1595 navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
1596 #endif
1597 }
1598
1599 void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance)
1600 {
1601 farAwayPos->V.X = posControl.actualState.pos.V.X + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
1602 farAwayPos->V.Y = posControl.actualState.pos.V.Y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
1603 farAwayPos->V.Z = posControl.actualState.pos.V.Z;
1604 }
1605
1606 /*-----------------------------------------------------------
1607 * NAV land detector
1608 *-----------------------------------------------------------*/
1609 static uint32_t landingTimer;
1610
1611 void resetLandingDetector(void)
1612 {
1613 landingTimer = micros();
1614 }
1615
1616 bool isLandingDetected(void)
1617 {
1618 bool landingDetected;
1619
1620 if (STATE(FIXED_WING)) { // FIXED_WING
1621 landingDetected = isFixedWingLandingDetected(&landingTimer);
1622 }
1623 else {
1624 landingDetected = isMulticopterLandingDetected(&landingTimer);
1625 }
1626
1627 return landingDetected;
1628 }
1629
1630 /*-----------------------------------------------------------
1631 * Z-position controller
1632 *-----------------------------------------------------------*/
1633 void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode)
1634 {
1635 // FIXME: On FIXED_WING and multicopter this should work in a different way
1636 // Calculate new altitude target
1637
1638 /* Move surface tracking setpoint if it is set */
1639 if (mode == CLIMB_RATE_RESET_SURFACE_TARGET) {
1640 posControl.desiredState.surface = -1;
1641 }
1642 else {
1643 if (posControl.flags.isTerrainFollowEnabled) {
1644 if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) {
1645 posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / (posControl.pids.pos[Z].param.kP * posControl.pids.surface.param.kP)), 1.0f, INAV_SURFACE_MAX_DISTANCE);
1646 }
1647 }
1648 else {
1649 posControl.desiredState.surface = -1;
1650 }
1651 }
1652
1653 posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + (climbRate / posControl.pids.pos[Z].param.kP);
1654
1655 #if defined(NAV_BLACKBOX)
1656 navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
1657 navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
1658 #endif
1659 }
1660
1661 static void resetAltitudeController(void)
1662 {
1663 if (STATE(FIXED_WING)) {
1664 resetFixedWingAltitudeController();
1665 }
1666 else {
1667 resetMulticopterAltitudeController();
1668 }
1669 }
1670
1671 static void setupAltitudeController(void)
1672 {
1673 if (STATE(FIXED_WING)) {
1674 setupFixedWingAltitudeController();
1675 }
1676 else {
1677 setupMulticopterAltitudeController();
1678 }
1679 }
1680
1681 static bool adjustAltitudeFromRCInput(void)
1682 {
1683 if (STATE(FIXED_WING)) {
1684 return adjustFixedWingAltitudeFromRCInput();
1685 }
1686 else {
1687 return adjustMulticopterAltitudeFromRCInput();
1688 }
1689 }
1690
1691 /*-----------------------------------------------------------
1692 * Heading controller (pass-through to MAG mode)
1693 *-----------------------------------------------------------*/
1694 static void resetHeadingController(void)
1695 {
1696 if (STATE(FIXED_WING)) {
1697 resetFixedWingHeadingController();
1698 }
1699 else {
1700 resetMulticopterHeadingController();
1701 }
1702 }
1703
1704 static bool adjustHeadingFromRCInput(void)
1705 {
1706 if (STATE(FIXED_WING)) {
1707 return adjustFixedWingHeadingFromRCInput();
1708 }
1709 else {
1710 return adjustMulticopterHeadingFromRCInput();
1711 }
1712 }
1713
1714 /*-----------------------------------------------------------
1715 * XY Position controller
1716 *-----------------------------------------------------------*/
1717 static void resetPositionController(void)
1718 {
1719 if (STATE(FIXED_WING)) {
1720 resetFixedWingPositionController();
1721 }
1722 else {
1723 resetMulticopterPositionController();
1724 }
1725 }
1726
1727 static bool adjustPositionFromRCInput(void)
1728 {
1729 if (STATE(FIXED_WING)) {
1730 return adjustFixedWingPositionFromRCInput();
1731 }
1732 else {
1733 return adjustMulticopterPositionFromRCInput();
1734 }
1735
1736 #if defined(NAV_BLACKBOX)
1737 navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
1738 navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
1739 #endif
1740 }
1741
1742 /*-----------------------------------------------------------
1743 * WP controller
1744 *-----------------------------------------------------------*/
1745 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
1746 {
1747 /* Default waypoint to send */
1748 wpData->action = NAV_WP_ACTION_RTH;
1749 wpData->lat = 0;
1750 wpData->lon = 0;
1751 wpData->alt = 0;
1752 wpData->p1 = 0;
1753 wpData->p2 = 0;
1754 wpData->p3 = 0;
1755 wpData->flag = NAV_WP_FLAG_LAST;
1756
1757 // WP #0 - special waypoint - HOME
1758 if (wpNumber == 0) {
1759 if (STATE(GPS_FIX_HOME)) {
1760 wpData->lat = GPS_home.lat;
1761 wpData->lon = GPS_home.lon;
1762 wpData->alt = GPS_home.alt;
1763 }
1764 }
1765 // WP #255 - special waypoint - directly get actualPosition
1766 else if (wpNumber == 255) {
1767 gpsLocation_t wpLLH;
1768
1769 geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH);
1770
1771 wpData->lat = wpLLH.lat;
1772 wpData->lon = wpLLH.lon;
1773 wpData->alt = wpLLH.alt;
1774 }
1775 // WP #1 - #15 - common waypoints - pre-programmed mission
1776 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
1777 if (wpNumber <= posControl.waypointCount) {
1778 *wpData = posControl.waypointList[wpNumber - 1];
1779 }
1780 }
1781 }
1782
1783 void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
1784 {
1785 gpsLocation_t wpLLH;
1786 navWaypointPosition_t wpPos;
1787
1788 // Pre-fill structure to convert to local coordinates
1789 wpLLH.lat = wpData->lat;
1790 wpLLH.lon = wpData->lon;
1791 wpLLH.alt = wpData->alt;
1792
1793 // WP #0 - special waypoint - HOME
1794 if ((wpNumber == 0) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid) {
1795 // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
1796 geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
1797 setHomePosition(&wpPos.pos, 0);
1798 }
1799 // WP #255 - special waypoint - directly set desiredPosition
1800 // Only valid when armed and in poshold mode
1801 else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
1802 ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
1803 (posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
1804 // Convert to local coordinates
1805 geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
1806
1807 navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
1808
1809 // If we received global altitude == 0, use current altitude
1810 if (wpData->alt != 0) {
1811 waypointUpdateFlags |= NAV_POS_UPDATE_Z;
1812 }
1813
1814 if (wpData->p1 > 0 && wpData->p1 < 360) {
1815 waypointUpdateFlags |= NAV_POS_UPDATE_HEADING;
1816 }
1817
1818 setDesiredPosition(&wpPos.pos, DEGREES_TO_DECIDEGREES(wpData->p1), waypointUpdateFlags);
1819 }
1820 // WP #1 - #15 - common waypoints - pre-programmed mission
1821 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
1822 if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_RTH) {
1823 // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
1824 if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
1825 posControl.waypointList[wpNumber - 1] = *wpData;
1826 posControl.waypointCount = wpNumber;
1827 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
1828 }
1829 }
1830 }
1831 }
1832
1833 void resetWaypointList(void)
1834 {
1835 /* Can only reset waypoint list if not armed */
1836 if (!ARMING_FLAG(ARMED)) {
1837 posControl.waypointCount = 0;
1838 posControl.waypointListValid = false;
1839 }
1840 }
1841
1842 static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos)
1843 {
1844 posControl.activeWaypoint.pos = *pos;
1845
1846 // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
1847 posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
1848
1849 // Set desired position to next waypoint (XYZ-controller)
1850 setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
1851 }
1852
1853 static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint)
1854 {
1855 gpsLocation_t wpLLH;
1856 t_fp_vector localPos;
1857
1858 wpLLH.lat = waypoint->lat;
1859 wpLLH.lon = waypoint->lon;
1860 wpLLH.alt = waypoint->alt;
1861
1862 geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &localPos, GEO_ALT_RELATIVE);
1863 calcualteAndSetActiveWaypointToLocalPosition(&localPos);
1864 }
1865
1866 /**
1867 * Returns TRUE if we are in WP mode and executing last waypoint on the list, or in RTH mode, or in PH mode
1868 * In RTH mode our only and last waypoint is home
1869 * In PH mode our waypoint is hold position */
1870 bool isApproachingLastWaypoint(void)
1871 {
1872 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
1873 if (posControl.waypointCount == 0) {
1874 /* No waypoints */
1875 return true;
1876 }
1877 else if ((posControl.activeWaypointIndex == (posControl.waypointCount - 1)) ||
1878 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)) {
1879 return true;
1880 }
1881 else {
1882 return false;
1883 }
1884 }
1885 else if (navGetStateFlags(posControl.navState) & NAV_CTL_POS) {
1886 // If POS controller is active we are in Poshold or RTH mode - assume last waypoint
1887 return true;
1888 }
1889 else {
1890 return false;
1891 }
1892 }
1893
1894 float getActiveWaypointSpeed(void)
1895 {
1896 uint16_t waypointSpeed = posControl.navConfig->max_speed;
1897
1898 if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
1899 if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
1900 waypointSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
1901
1902 if (waypointSpeed < 50 || waypointSpeed > posControl.navConfig->max_speed) {
1903 waypointSpeed = posControl.navConfig->max_speed;
1904 }
1905 }
1906 }
1907
1908 return waypointSpeed;
1909 }
1910
1911 /*-----------------------------------------------------------
1912 * A function to reset navigation PIDs and states
1913 *-----------------------------------------------------------*/
1914 void resetNavigation(void)
1915 {
1916 resetAltitudeController();
1917 resetHeadingController();
1918 resetPositionController();
1919 }
1920
1921 /*-----------------------------------------------------------
1922 * Process adjustments to alt, pos and yaw controllers
1923 *-----------------------------------------------------------*/
1924 static void processNavigationRCAdjustments(void)
1925 {
1926 /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
1927 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
1928 if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
1929 posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
1930 }
1931 else {
1932 posControl.flags.isAdjustingAltitude = false;
1933 }
1934
1935 if ((navStateFlags & NAV_RC_POS) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
1936 posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
1937 }
1938 else {
1939 posControl.flags.isAdjustingPosition = false;
1940 }
1941
1942 if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
1943 posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
1944 }
1945 else {
1946 posControl.flags.isAdjustingHeading = false;
1947 }
1948 }
1949
1950 /*-----------------------------------------------------------
1951 * A main function to call position controllers at loop rate
1952 *-----------------------------------------------------------*/
1953 void applyWaypointNavigationAndAltitudeHold(void)
1954 {
1955 uint32_t currentTime = micros();
1956
1957 #if defined(NAV_BLACKBOX)
1958 navFlags = 0;
1959 if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
1960 if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
1961 if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
1962 if ((STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) navFlags |= (1 << 3);
1963 if (isGPSGlitchDetected()) navFlags |= (1 << 4);
1964 #endif
1965
1966 // No navigation when disarmed
1967 if (!ARMING_FLAG(ARMED)) {
1968 // If we are disarmed, abort forced RTH
1969 posControl.flags.forcedRTHActivated = false;
1970 return;
1971 }
1972
1973 /* Process controllers */
1974 navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
1975 if (STATE(FIXED_WING)) {
1976 applyFixedWingNavigationController(navStateFlags, currentTime);
1977 }
1978 else {
1979 applyMulticopterNavigationController(navStateFlags, currentTime);
1980 }
1981
1982 #if defined(NAV_BLACKBOX)
1983 if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 5);
1984 if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 6);
1985 if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 7);
1986 #endif
1987 }
1988
1989 /*-----------------------------------------------------------
1990 * Set CF's FLIGHT_MODE from current NAV_MODE
1991 *-----------------------------------------------------------*/
1992 void swithNavigationFlightModes(void)
1993 {
1994 flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
1995 flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE) & (~enabledNavFlightModes);
1996 DISABLE_FLIGHT_MODE(disabledFlightModes);
1997 ENABLE_FLIGHT_MODE(enabledNavFlightModes);
1998 }
1999
2000 /*-----------------------------------------------------------
2001 * desired NAV_MODE from combination of FLIGHT_MODE flags
2002 *-----------------------------------------------------------*/
2003 static bool canActivateAltHoldMode(void)
2004 {
2005 return posControl.flags.hasValidAltitudeSensor;
2006 }
2007
2008 static bool canActivatePosHoldMode(void)
2009 {
2010 return posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor;
2011 }
2012
2013 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
2014 {
2015 static bool canActivateWaypoint = false;
2016
2017 //We can switch modes only when ARMED
2018 if (ARMING_FLAG(ARMED)) {
2019 // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
2020 bool canActivateAltHold = canActivateAltHoldMode();
2021 bool canActivatePosHold = canActivatePosHoldMode();
2022
2023 // RTH/Failsafe_RTH can override PASSTHRU
2024 if (posControl.flags.forcedRTHActivated || IS_RC_MODE_ACTIVE(BOXNAVRTH)) {
2025 // If we request forced RTH - attempt to activate it no matter what
2026 // This might switch to emergency landing controller if GPS is unavailable
2027 canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
2028 return NAV_FSM_EVENT_SWITCH_TO_RTH;
2029 }
2030
2031 // PASSTHRU mode has priority over WP/PH/AH
2032 if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
2033 canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
2034 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2035 }
2036
2037 if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
2038 if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
2039 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
2040 }
2041 else {
2042 // Arm the state variable if the WP BOX mode is not enabled
2043 canActivateWaypoint = true;
2044 }
2045
2046 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
2047 if ((FLIGHT_MODE(NAV_ALTHOLD_MODE) && FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
2048 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
2049 }
2050
2051 if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
2052 if ((FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold))
2053 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D;
2054 }
2055
2056 if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
2057 if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
2058 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
2059 }
2060 }
2061 else {
2062 canActivateWaypoint = false;
2063 }
2064
2065 return NAV_FSM_EVENT_SWITCH_TO_IDLE;
2066 }
2067
2068 /*-----------------------------------------------------------
2069 * An indicator that throttle tilt compensation is forced
2070 *-----------------------------------------------------------*/
2071 bool navigationRequiresThrottleTiltCompensation(void)
2072 {
2073 return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
2074 }
2075
2076 /*-----------------------------------------------------------
2077 * An indicator that ANGLE mode must be forced per NAV requirement
2078 *-----------------------------------------------------------*/
2079 bool naivationRequiresAngleMode(void)
2080 {
2081 navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
2082 return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
2083 }
2084
2085 /**
2086 * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
2087 */
2088 int8_t naivationGetHeadingControlState(void)
2089 {
2090 if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
2091 if (posControl.flags.isAdjustingHeading) {
2092 return NAV_HEADING_CONTROL_MANUAL;
2093 }
2094 else {
2095 return NAV_HEADING_CONTROL_AUTO;
2096 }
2097 }
2098 else {
2099 return NAV_HEADING_CONTROL_NONE;
2100 }
2101 }
2102
2103 bool naivationBlockArming(void)
2104 {
2105 bool shouldBlockArming = false;
2106
2107 if (!posControl.navConfig->flags.extra_arming_safety)
2108 return false;
2109
2110 // Apply extra arming safety only if pilot has any of GPS modes configured
2111 if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !(posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME))) {
2112 shouldBlockArming = true;
2113 }
2114
2115 // Don't allow arming if any of NAV modes is active
2116 if ((!ARMING_FLAG(ARMED)) && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD))) {
2117 shouldBlockArming = true;
2118 }
2119
2120 return shouldBlockArming;
2121 }
2122
2123
2124 bool navigationPositionEstimateIsHealthy(void)
2125 {
2126 return posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME);
2127 }
2128
2129 /**
2130 * Indicate ready/not ready status
2131 */
2132 static void updateReadyStatus(void)
2133 {
2134 static bool posReadyBeepDone = false;
2135
2136 /* Beep out READY_BEEP once when position lock is firstly acquired and HOME set */
2137 if (navigationPositionEstimateIsHealthy() && !posReadyBeepDone) {
2138 beeper(BEEPER_READY_BEEP);
2139 posReadyBeepDone = true;
2140 }
2141 }
2142
2143 void updateFlightBehaviorModifiers(void)
2144 {
2145 posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
2146 posControl.flags.isTerrainFollowEnabled = IS_RC_MODE_ACTIVE(BOXSURFACE);
2147 }
2148
2149 /**
2150 * Process NAV mode transition and WP/RTH state machine
2151 * Update rate: RX (data driven or 50Hz)
2152 */
2153 void updateWaypointsAndNavigationMode(void)
2154 {
2155 /* Initiate home position update */
2156 updateHomePosition();
2157
2158 /* Update NAV ready status */
2159 updateReadyStatus();
2160
2161 // Update flight behaviour modifiers
2162 updateFlightBehaviorModifiers();
2163
2164 // Process switch to a different navigation mode (if needed)
2165 navProcessFSMEvents(selectNavEventFromBoxModeInput());
2166
2167 // Process pilot's RC input to adjust behaviour
2168 processNavigationRCAdjustments();
2169
2170 // Map navMode back to enabled flight modes
2171 swithNavigationFlightModes();
2172
2173 #if defined(NAV_BLACKBOX)
2174 navCurrentState = (int16_t)posControl.navState;
2175 #endif
2176 }
2177
2178 /*-----------------------------------------------------------
2179 * NAV main control functions
2180 *-----------------------------------------------------------*/
2181 void navigationUseConfig(navConfig_t *navConfigToUse)
2182 {
2183 posControl.navConfig = navConfigToUse;
2184 }
2185
2186 void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig)
2187 {
2188 posControl.rcControlsConfig = initialRcControlsConfig;
2189 }
2190
2191 void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig)
2192 {
2193 posControl.flight3DConfig = initialFlight3DConfig;
2194 }
2195
2196 void navigationUseRxConfig(rxConfig_t * initialRxConfig)
2197 {
2198 posControl.rxConfig = initialRxConfig;
2199 }
2200
2201 void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig)
2202 {
2203 posControl.escAndServoConfig = initialEscAndServoConfig;
2204 }
2205
2206 void navigationUsePIDs(pidProfile_t *initialPidProfile)
2207 {
2208 int axis;
2209
2210 posControl.pidProfile = initialPidProfile;
2211
2212 // Brake time parameter
2213 posControl.posDecelerationTime = (float)posControl.pidProfile->I8[PIDPOS] / 100.0f;
2214
2215 // Position controller expo (taret vel expo for MC)
2216 posControl.posResponseExpo = constrainf((float)posControl.pidProfile->D8[PIDPOS] / 100.0f, 0.0f, 1.0f);
2217
2218 // Initialize position hold P-controller
2219 for (axis = 0; axis < 2; axis++) {
2220 navPInit(&posControl.pids.pos[axis], (float)posControl.pidProfile->P8[PIDPOS] / 100.0f);
2221
2222 navPidInit(&posControl.pids.vel[axis], (float)posControl.pidProfile->P8[PIDPOSR] / 100.0f,
2223 (float)posControl.pidProfile->I8[PIDPOSR] / 100.0f,
2224 (float)posControl.pidProfile->D8[PIDPOSR] / 100.0f);
2225 }
2226
2227 // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
2228 navPInit(&posControl.pids.pos[Z], (float)posControl.pidProfile->P8[PIDALT] / 100.0f);
2229
2230 navPidInit(&posControl.pids.vel[Z], (float)posControl.pidProfile->P8[PIDVEL] / 100.0f,
2231 (float)posControl.pidProfile->I8[PIDVEL] / 100.0f,
2232 (float)posControl.pidProfile->D8[PIDVEL] / 100.0f);
2233
2234 // Initialize surface tracking PID
2235 navPidInit(&posControl.pids.surface, 2.0f,
2236 1.0f,
2237 0.0f);
2238
2239 // Initialize fixed wing PID controllers
2240 navPidInit(&posControl.pids.fw_nav, (float)posControl.pidProfile->P8[PIDNAVR] / 100.0f,
2241 (float)posControl.pidProfile->I8[PIDNAVR] / 100.0f,
2242 (float)posControl.pidProfile->D8[PIDNAVR] / 100.0f);
2243
2244 navPidInit(&posControl.pids.fw_alt, (float)posControl.pidProfile->P8[PIDALT] / 100.0f,
2245 (float)posControl.pidProfile->I8[PIDALT] / 100.0f,
2246 (float)posControl.pidProfile->D8[PIDALT] / 100.0f);
2247 }
2248
2249 void navigationInit(navConfig_t *initialnavConfig,
2250 pidProfile_t *initialPidProfile,
2251 rcControlsConfig_t *initialRcControlsConfig,
2252 rxConfig_t * initialRxConfig,
2253 flight3DConfig_t * initialFlight3DConfig,
2254 escAndServoConfig_t * initialEscAndServoConfig)
2255 {
2256 /* Initial state */
2257 posControl.navState = NAV_STATE_IDLE;
2258
2259 posControl.flags.horizontalPositionNewData = 0;
2260 posControl.flags.verticalPositionNewData = 0;
2261 posControl.flags.surfaceDistanceNewData = 0;
2262 posControl.flags.headingNewData = 0;
2263
2264 posControl.flags.hasValidAltitudeSensor = 0;
2265 posControl.flags.hasValidPositionSensor = 0;
2266 posControl.flags.hasValidSurfaceSensor = 0;
2267 posControl.flags.hasValidHeadingSensor = 0;
2268
2269 posControl.flags.forcedRTHActivated = 0;
2270 posControl.waypointCount = 0;
2271 posControl.activeWaypointIndex = 0;
2272 posControl.waypointListValid = false;
2273
2274 /* Set initial surface invalid */
2275 posControl.actualState.surface = -1.0f;
2276 posControl.actualState.surfaceVel = 0.0f;
2277 posControl.actualState.surfaceMin = -1.0f;
2278
2279 /* Use system config */
2280 navigationUseConfig(initialnavConfig);
2281 navigationUsePIDs(initialPidProfile);
2282 navigationUseRcControlsConfig(initialRcControlsConfig);
2283 navigationUseRxConfig(initialRxConfig);
2284 navigationUseEscAndServoConfig(initialEscAndServoConfig);
2285 navigationUseFlight3DConfig(initialFlight3DConfig);
2286 }
2287
2288 /*-----------------------------------------------------------
2289 * Access to estimated position/velocity data
2290 *-----------------------------------------------------------*/
2291 float getEstimatedActualVelocity(int axis)
2292 {
2293 return posControl.actualState.vel.A[axis];
2294 }
2295
2296 float getEstimatedActualPosition(int axis)
2297 {
2298 return posControl.actualState.pos.A[axis];
2299 }
2300
2301 /*-----------------------------------------------------------
2302 * Ability to execute RTH on external event
2303 *-----------------------------------------------------------*/
2304 void activateForcedRTH(void)
2305 {
2306 posControl.flags.forcedRTHActivated = true;
2307 navProcessFSMEvents(selectNavEventFromBoxModeInput());
2308 }
2309
2310 void abortForcedRTH(void)
2311 {
2312 posControl.flags.forcedRTHActivated = false;
2313 navProcessFSMEvents(selectNavEventFromBoxModeInput());
2314 }
2315
2316 rthState_e getStateOfForcedRTH(void)
2317 {
2318 /* If forced RTH activated and in AUTO_RTH or EMERG state */
2319 if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
2320 if (posControl.navState == NAV_STATE_RTH_2D_FINISHED || posControl.navState == NAV_STATE_RTH_3D_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
2321 return RTH_HAS_LANDED;
2322 }
2323 else {
2324 return RTH_IN_PROGRESS;
2325 }
2326 }
2327 else {
2328 return RTH_IDLE;
2329 }
2330 }
2331
2332 #else // NAV
2333
2334 #ifdef GPS
2335 /* Fallback if navigation is not compiled in - handle GPS home coordinates */
2336 static float GPS_scaleLonDown;
2337
2338 static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing)
2339 {
2340 float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
2341 float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
2342
2343 *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
2344
2345 *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg
2346
2347 if (*bearing < 0)
2348 *bearing += 36000;
2349 }
2350
2351 void onNewGPSData()
2352 {
2353 if (!(sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5))
2354 return;
2355
2356 if (ARMING_FLAG(ARMED)) {
2357 if (STATE(GPS_FIX_HOME)) {
2358 uint32_t dist;
2359 int32_t dir;
2360 GPS_distance_cm_bearing(gpsSol.llh.lat, gpsSol.llh.lon, GPS_home.lat, GPS_home.lon, &dist, &dir);
2361 GPS_distanceToHome = dist / 100;
2362 GPS_directionToHome = dir / 100;
2363 } else {
2364 GPS_distanceToHome = 0;
2365 GPS_directionToHome = 0;
2366 }
2367 }
2368 else {
2369 // Set home position to current GPS coordinates
2370 ENABLE_STATE(GPS_FIX_HOME);
2371 GPS_home.lat = gpsSol.llh.lat;
2372 GPS_home.lon = gpsSol.llh.lon;
2373 GPS_home.alt = gpsSol.llh.alt;
2374 GPS_distanceToHome = 0;
2375 GPS_directionToHome = 0;
2376 GPS_scaleLonDown = cos_approx((ABS((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
2377 }
2378 }
2379 #endif
2380
2381 #endif // NAV