PX4 Firmware
PX4 Autopilot Software http://px4.io
state_machine_helper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file state_machine_helper.cpp
36  * State machine helper functions implementations
37  *
38  * @author Thomas Gubler <thomas@px4.io>
39  * @author Julian Oes <julian@oes.ch>
40  * @author Sander Smeets <sander@droneslab.com>
41  */
42 #include <px4_platform_common/px4_config.h>
46 #include <systemlib/mavlink_log.h>
47 #include <drivers/drv_hrt.h>
48 
50 #include "state_machine_helper.h"
51 #include "commander_helper.h"
52 
53 static constexpr const char reason_no_rc[] = "no RC";
54 static constexpr const char reason_no_offboard[] = "no offboard";
55 static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offboard";
56 static constexpr const char reason_no_local_position[] = "no local position";
57 static constexpr const char reason_no_global_position[] = "no global position";
58 static constexpr const char reason_no_datalink[] = "no datalink";
59 
60 // This array defines the arming state transitions. The rows are the new state, and the columns
61 // are the current state. Using new state and current state you can index into the array which
62 // will be true for a valid transition or false for a invalid transition. In some cases even
63 // though the transition is marked as true additional checks must be made. See arming_state_transition
64 // code for those checks.
65 static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
66 = {
67  // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE
68  { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false },
69  { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false },
70  { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true },
71  { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false },
72  { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true },
73  { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI
74 };
75 
76 // You can index into the array with an arming_state_t in order to get its textual representation
77 const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
78  "INIT",
79  "STANDBY",
80  "ARMED",
81  "STANDBY_ERROR",
82  "SHUTDOWN",
83  "IN_AIR_RESTORE",
84 };
85 
86 static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
87 
90  uint8_t auto_recovery_nav_state);
91 
92 void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
93 
96  const offboard_loss_actions_t offboard_loss_act);
97 
100  const offboard_loss_rc_actions_t offboard_loss_rc_act);
101 
102 void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe,
103  const offboard_loss_actions_t offboard_loss_act,
104  const offboard_loss_rc_actions_t offboard_loss_rc_act);
105 
107  const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
109  const hrt_abstime &time_since_boot)
110 {
111  // Double check that our static arrays are still valid
112  static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
113  static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
114  "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
115 
117  arming_state_t current_arming_state = status->arming_state;
118  bool feedback_provided = false;
119 
120  const bool hil_enabled = (status->hil_state == vehicle_status_s::HIL_STATE_ON);
121 
122  /* only check transition if the new state is actually different from the current one */
123  if (new_arming_state == current_arming_state) {
125 
126  } else {
127 
128  /*
129  * Get sensing state if necessary
130  */
131  bool preflight_check_ret = true;
132 
133  const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT);
134 
135  /* only perform the pre-arm check if we have to */
136  if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
137  && !hil_enabled) {
138 
139  preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true,
140  time_since_boot);
141 
142  if (preflight_check_ret) {
143  status_flags->condition_system_sensors_initialized = true;
144  }
145 
146  feedback_provided = true;
147  }
148 
149  /* re-run the pre-flight check as long as sensors are failing */
150  if (!status_flags->condition_system_sensors_initialized
151  && fRunPreArmChecks
152  && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
153  || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
154  && !hil_enabled) {
155 
156  if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
157 
158  status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status,
159  *status_flags,
160  checkGNSS, false, false, time_since_boot);
161 
163  }
164  }
165 
166  // Check that we have a valid state transition
167  bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
168 
169  if (valid_transition) {
170  // We have a good transition. Now perform any secondary validation.
171  if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
172 
173  // Do not perform pre-arm checks if coming from in air restore
174  // Allow if vehicle_status_s::HIL_STATE_ON
175  if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && !hil_enabled) {
176 
177  bool prearm_check_ret = true;
178 
179  if (fRunPreArmChecks && preflight_check_ret) {
180  // only bother running prearm if preflight was successful
181  prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, *status_flags, safety, arm_requirements);
182  }
183 
184  if (!preflight_check_ret || !prearm_check_ret) {
185  // the prearm and preflight checks already print the rejection reason
186  feedback_provided = true;
187  valid_transition = false;
188  }
189  }
190  }
191  }
192 
193  if (hil_enabled) {
194  /* enforce lockdown in HIL */
195  armed->lockdown = true;
196  status_flags->condition_system_sensors_initialized = true;
197 
198  /* recover from a prearm fail */
199  if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
200  status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
201  }
202 
203  // HIL can always go to standby
204  if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
205  valid_transition = true;
206  }
207  }
208 
209  if (!hil_enabled &&
210  (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
211  (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
212 
213  // Sensors need to be initialized for STANDBY state, except for HIL
214  if (!status_flags->condition_system_sensors_initialized) {
215  feedback_provided = true;
216  valid_transition = false;
217  }
218  }
219 
220  // Finish up the state transition
221  if (valid_transition) {
222  armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
223  armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
224  || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
225  ret = TRANSITION_CHANGED;
226  status->arming_state = new_arming_state;
227 
228  if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
229  armed->armed_time_ms = hrt_absolute_time() / 1000;
230 
231  } else {
232  armed->armed_time_ms = 0;
233  }
234  }
235  }
236 
237  if (ret == TRANSITION_DENIED) {
238  /* print to MAVLink and console if we didn't provide any feedback yet */
239  if (!feedback_provided) {
240  mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s",
241  arming_state_names[status->arming_state], arming_state_names[new_arming_state]);
242  }
243  }
244 
245  return ret;
246 }
247 
249 {
250  // System is safe if:
251  // 1) Not armed
252  // 2) Armed, but in software lockdown (HIL)
253  // 3) Safety switch is present AND engaged -> actuators locked
254  const bool lockdown = (armed.lockdown || armed.manual_lockdown);
255 
256  return !armed.armed || (armed.armed && lockdown) || (safety.safety_switch_available && !safety.safety_off);
257 }
258 
262 {
263  // IMPORTANT: The assumption of callers of this function is that the execution of
264  // this check is essentially "free". Therefore any runtime checking in here has to be
265  // kept super lightweight. No complex logic or calls on external function should be
266  // implemented here.
267 
269 
270  /* transition may be denied even if the same state is requested because conditions may have changed */
271  switch (new_main_state) {
272  case commander_state_s::MAIN_STATE_MANUAL:
273  case commander_state_s::MAIN_STATE_STAB:
274  case commander_state_s::MAIN_STATE_ACRO:
275  case commander_state_s::MAIN_STATE_RATTITUDE:
276  ret = TRANSITION_CHANGED;
277  break;
278 
279  case commander_state_s::MAIN_STATE_ALTCTL:
280 
281  /* need at minimum altitude estimate */
282  if (status_flags.condition_local_altitude_valid ||
283  status_flags.condition_global_position_valid) {
284  ret = TRANSITION_CHANGED;
285  }
286 
287  break;
288 
289  case commander_state_s::MAIN_STATE_POSCTL:
290 
291  /* need at minimum local position estimate */
292  if (status_flags.condition_local_position_valid ||
293  status_flags.condition_global_position_valid) {
294  ret = TRANSITION_CHANGED;
295  }
296 
297  break;
298 
299  case commander_state_s::MAIN_STATE_AUTO_LOITER:
300 
301  /* need global position estimate */
302  if (status_flags.condition_global_position_valid) {
303  ret = TRANSITION_CHANGED;
304  }
305 
306  break;
307 
308  case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
309  case commander_state_s::MAIN_STATE_ORBIT:
310 
311  /* Follow and orbit only implemented for multicopter */
312  if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
313  ret = TRANSITION_CHANGED;
314  }
315 
316  break;
317 
318  case commander_state_s::MAIN_STATE_AUTO_MISSION:
319 
320  /* need global position, home position, and a valid mission */
321  if (status_flags.condition_global_position_valid &&
322  status_flags.condition_auto_mission_available) {
323 
324  ret = TRANSITION_CHANGED;
325  }
326 
327  break;
328 
329  case commander_state_s::MAIN_STATE_AUTO_RTL:
330 
331  /* need global position and home position */
332  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
333  ret = TRANSITION_CHANGED;
334  }
335 
336  break;
337 
338  case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
339  case commander_state_s::MAIN_STATE_AUTO_LAND:
340 
341  /* need local position */
342  if (status_flags.condition_local_position_valid) {
343  ret = TRANSITION_CHANGED;
344  }
345 
346  break;
347 
348  case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
349 
350  /* need local and global position, and precision land only implemented for multicopters */
351  if (status_flags.condition_local_position_valid
352  && status_flags.condition_global_position_valid
353  && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
354 
355  ret = TRANSITION_CHANGED;
356  }
357 
358  break;
359 
360  case commander_state_s::MAIN_STATE_OFFBOARD:
361 
362  /* need offboard signal */
363  if (!status_flags.offboard_control_signal_lost) {
364 
365  ret = TRANSITION_CHANGED;
366  }
367 
368  break;
369 
370  case commander_state_s::MAIN_STATE_MAX:
371  default:
372  break;
373  }
374 
375  if (ret == TRANSITION_CHANGED) {
376  if (internal_state->main_state != new_main_state) {
377  internal_state->main_state = new_main_state;
378  internal_state->timestamp = hrt_absolute_time();
379 
380  } else {
382  }
383  }
384 
385  return ret;
386 }
387 
388 /**
389  * Enable failsafe and report to user
390  */
391 void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
392 {
393  if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
394  mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason);
395  }
396 
397  status->failsafe = true;
398 }
399 
400 /**
401  * Check failsafe and main status and set navigation status for navigator accordingly
402  */
404  orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
405  const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
406  const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
407  const offboard_loss_rc_actions_t offb_loss_rc_act,
408  const position_nav_loss_actions_t posctl_nav_loss_act)
409 {
410  const navigation_state_t nav_state_old = status->nav_state;
411 
412  const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
413  const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
414  const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost);
415 
416  bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED);
417  bool old_failsafe = status->failsafe;
418  status->failsafe = false;
419 
420  // Safe to do reset flags here, as if loss state persists flags will be restored in the code below
421  reset_link_loss_globals(armed, old_failsafe, rc_loss_act);
422  reset_link_loss_globals(armed, old_failsafe, data_link_loss_act);
423  reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act);
424 
425  /* evaluate main state to decide in normal (non-failsafe) mode */
426  switch (internal_state->main_state) {
427  case commander_state_s::MAIN_STATE_ACRO:
428  case commander_state_s::MAIN_STATE_MANUAL:
429  case commander_state_s::MAIN_STATE_RATTITUDE:
430  case commander_state_s::MAIN_STATE_STAB:
431  case commander_state_s::MAIN_STATE_ALTCTL:
432 
433  /* require RC for all manual modes */
434  if (rc_lost && is_armed) {
435  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
436 
437  set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
438  vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
439 
440  } else {
441  switch (internal_state->main_state) {
442  case commander_state_s::MAIN_STATE_ACRO:
443  status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
444  break;
445 
446  case commander_state_s::MAIN_STATE_MANUAL:
447  status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
448  break;
449 
450  case commander_state_s::MAIN_STATE_RATTITUDE:
451  status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
452  break;
453 
454  case commander_state_s::MAIN_STATE_STAB:
455  status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
456  break;
457 
458  case commander_state_s::MAIN_STATE_ALTCTL:
459  status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
460  break;
461 
462  default:
463  status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
464  break;
465  }
466  }
467 
468  break;
469 
470  case commander_state_s::MAIN_STATE_POSCTL: {
471 
472  if (rc_lost && is_armed) {
473  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
474 
475  set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
476  vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
477 
478  /* As long as there is RC, we can fallback to ALTCTL, or STAB. */
479  /* A local position estimate is enough for POSCTL for multirotors,
480  * this enables POSCTL using e.g. flow.
481  * For fixedwing, a global position is needed. */
482 
483  } else if (is_armed
484  && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
485  !(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE),
487  // nothing to do - everything done in check_invalid_pos_nav_state
488 
489  } else {
490  status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
491  }
492  }
493  break;
494 
495  case commander_state_s::MAIN_STATE_AUTO_MISSION:
496 
497  /* go into failsafe
498  * - if commanded to do so
499  * - if we have an engine failure
500  * - if we have vtol transition failure
501  * - depending on datalink, RC and if the mission is finished */
502 
503  if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
504  // nothing to do - everything done in check_invalid_pos_nav_state
505  } else if (status->engine_failure) {
506  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
507 
508  } else if (status_flags.vtol_transition_failure) {
509  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
510 
511  } else if (status->mission_failure) {
512  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
513 
514  } else if (data_link_loss_act_configured && status->data_link_lost && is_armed) {
515  /* datalink loss enabled:
516  * check for datalink lost: this should always trigger RTGS */
517  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
518 
519  set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
520  vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
521 
522  } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
523  && mission_finished && is_armed) {
524  /* datalink loss DISABLED:
525  * check if both, RC and datalink are lost during the mission
526  * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
527  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
528 
529  set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
530  vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
531 
532  } else if (!stay_in_failsafe) {
533  /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
534  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
535  }
536 
537  break;
538 
539  case commander_state_s::MAIN_STATE_AUTO_LOITER:
540 
541  /* go into failsafe on a engine failure */
542  if (status->engine_failure) {
543  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
544 
545  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
546  // nothing to do - everything done in check_invalid_pos_nav_state
547  } else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
548  /* also go into failsafe if just datalink is lost, and we're actually in air */
549  set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
550  vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
551 
552  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
553 
554  } else if (rc_lost && !data_link_loss_act_configured && is_armed) {
555  /* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
556  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
557 
558  set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
559  vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
560 
561  } else if (status->rc_signal_lost) {
562  /* don't bother if RC is lost if datalink is connected */
563 
564  /* this mode is ok, we don't need RC for LOITERing */
565  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
566 
567  } else {
568  /* everything is perfect */
569  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
570  }
571 
572  break;
573 
574  case commander_state_s::MAIN_STATE_AUTO_RTL:
575 
576  /* require global position and home, also go into failsafe on an engine failure */
577 
578  if (status->engine_failure) {
579  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
580 
581  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
582  // nothing to do - everything done in check_invalid_pos_nav_state
583  } else {
584  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
585  }
586 
587  break;
588 
589  case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
590 
591  /* require global position and home */
592 
593  if (status->engine_failure) {
594  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
595 
596  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
597  // nothing to do - everything done in check_invalid_pos_nav_state
598 
599  } else {
600  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
601  }
602 
603  break;
604 
605  case commander_state_s::MAIN_STATE_ORBIT:
606  if (status->engine_failure) {
607  // failsafe: on engine failure
608  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
609 
610  // Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
611  // is not possible and therefore the internal_state needs to be adjusted.
612  internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
613 
614  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
615  // failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state
616 
617  // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
618  // is not possible and therefore the internal_state needs to be adjusted.
619  internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
620 
621  } else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
622  // failsafe: just datalink is lost and we're in air
623  set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
624  vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
625 
626  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
627 
628  // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
629  // is not possible and therefore the internal_state needs to be adjusted.
630  internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
631 
632  } else if (rc_lost && !data_link_loss_act_configured && is_armed) {
633  // failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled
634  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
635 
636  set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
637  vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
638 
639  // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
640  // is not possible and therefore the internal_state needs to be adjusted.
641  internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
642 
643  } else {
644  // no failsafe, RC is not mandatory for orbit
645  status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
646  }
647 
648  break;
649 
650  case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
651 
652  /* require local position */
653 
654  if (status->engine_failure) {
655  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
656 
657  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
658  // nothing to do - everything done in check_invalid_pos_nav_state
659 
660  } else {
661  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
662  }
663 
664  break;
665 
666  case commander_state_s::MAIN_STATE_AUTO_LAND:
667 
668  /* require local position */
669 
670  if (status->engine_failure) {
671  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
672 
673  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
674  // nothing to do - everything done in check_invalid_pos_nav_state
675 
676  } else {
677  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
678  }
679 
680  break;
681 
682  case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
683 
684  /* must be rotary wing plus same requirements as normal landing */
685 
686  if (status->engine_failure) {
687  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
688 
689  } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
690  // nothing to do - everything done in check_invalid_pos_nav_state
691 
692  } else {
693  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
694  }
695 
696  break;
697 
698  case commander_state_s::MAIN_STATE_OFFBOARD:
699 
700  /* require offboard control, otherwise stay where you are */
701  if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_loss_timeout) {
702  if (status->rc_signal_lost) {
703  // Offboard and RC are lost
704  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);
705  set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act);
706 
707  } else {
708  // Offboard is lost, RC is ok
709  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard);
710  set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act);
711  }
712 
713  } else {
714  status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
715  }
716 
717  default:
718  break;
719  }
720 
721  return status->nav_state != nav_state_old;
722 }
723 
725  const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos)
726 {
727  bool fallback_required = false;
728 
729  if (using_global_pos && !status_flags.condition_global_position_valid) {
730  fallback_required = true;
731 
732  } else if (!using_global_pos
733  && (!status_flags.condition_local_position_valid || !status_flags.condition_local_velocity_valid)) {
734 
735  fallback_required = true;
736  }
737 
738  if (fallback_required) {
739  if (use_rc) {
740  // fallback to a mode that gives the operator stick control
741  if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
742  && status_flags.condition_local_position_valid) {
743  status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
744 
745  } else if (status_flags.condition_local_altitude_valid) {
746  status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
747 
748  } else {
749  status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
750  }
751 
752  } else {
753  // go into a descent that does not require stick control
754  if (status_flags.condition_local_position_valid) {
755  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
756 
757  } else if (status_flags.condition_local_altitude_valid) {
758  if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
759  status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
760 
761  } else {
762  // TODO: FW position controller doesn't run without condition_global_position_valid
763  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
764  }
765 
766  } else {
767  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
768  }
769  }
770 
771  if (using_global_pos) {
772  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position);
773 
774  } else {
775  enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
776  }
777 
778  }
779 
780  return fallback_required;
781 
782 }
783 
786  uint8_t auto_recovery_nav_state)
787 {
788  // do the best you can according to the action set
789 
790  switch (link_loss_act) {
792  // If datalink loss failsafe is disabled then no action must be taken.
793  break;
794 
796  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
797  status->nav_state = auto_recovery_nav_state;
798  return;
799  }
800 
801  // FALLTHROUGH
803  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
804  main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
805  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
806  return;
807  }
808 
809  // FALLTHROUGH
811  if (status_flags.condition_global_position_valid) {
812  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
813  return;
814  }
815 
816  // FALLTHROUGH
818  if (status_flags.condition_global_position_valid) {
819  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
820  return;
821 
822  } else {
823  if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
824  if (status_flags.condition_local_position_valid) {
825  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
826  return;
827 
828  } else if (status_flags.condition_local_altitude_valid) {
829  status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
830  return;
831  }
832 
833  } else {
834  // TODO: FW position controller doesn't run without condition_global_position_valid
835  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
836  return;
837  }
838  }
839 
840  // FALLTHROUGH
842  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
843  armed->force_failsafe = true;
844  break;
845 
847  armed->lockdown = true;
848  break;
849  }
850 }
851 
852 void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act)
853 {
854  if (old_failsafe) {
855  if (link_loss_act == link_loss_actions_t::TERMINATE) {
856  armed->force_failsafe = false;
857 
858  } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
859  armed->lockdown = false;
860  }
861  }
862 }
863 
866  const offboard_loss_actions_t offboard_loss_act)
867 {
868  switch (offboard_loss_act) {
870  // If offboard loss failsafe is disabled then no action must be taken.
871  return;
872 
874  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
875  armed->force_failsafe = true;
876  return;
877 
879  armed->lockdown = true;
880  return;
881 
883  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
884  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
885  return;
886  }
887 
888  // FALLTHROUGH
890  if (status_flags.condition_global_position_valid) {
891  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
892  return;
893  }
894 
895  // FALLTHROUGH
897  if (status_flags.condition_global_position_valid) {
898  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
899  return;
900  }
901 
902  }
903 
904  // If none of the above worked, try to mitigate
905  if (status_flags.condition_local_altitude_valid) {
906  if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
907  status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
908 
909  } else {
910  // TODO: FW position controller doesn't run without condition_global_position_valid
911  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
912  }
913 
914  } else {
915  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
916  }
917 }
918 
921  const offboard_loss_rc_actions_t offboard_loss_rc_act)
922 {
923  switch (offboard_loss_rc_act) {
925  // If offboard loss failsafe is disabled then no action must be taken.
926  return;
927 
929  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
930  armed->force_failsafe = true;
931  return;
932 
934  armed->lockdown = true;
935  return;
936 
938  if (status_flags.condition_global_position_valid) {
939  status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
940  return;
941  }
942 
943  // FALLTHROUGH
945  if (status_flags.condition_local_altitude_valid) {
946  status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
947  return;
948  }
949 
950  // FALLTHROUGH
952  status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
953  return;
954 
956  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
957  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
958  return;
959  }
960 
961  // FALLTHROUGH
963  if (status_flags.condition_global_position_valid) {
964  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
965  return;
966  }
967 
968  // FALLTHROUGH
970  if (status_flags.condition_global_position_valid) {
971  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
972  return;
973  }
974 
975  }
976 
977  // If none of the above worked, try to mitigate
978  if (status_flags.condition_local_altitude_valid) {
979  //TODO: Add case for rover
980  if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
981  status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
982 
983  } else {
984  // TODO: FW position controller doesn't run without condition_global_position_valid
985  status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
986  }
987 
988  } else {
989  status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
990  }
991 }
992 
993 void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe,
994  const offboard_loss_actions_t offboard_loss_act,
995  const offboard_loss_rc_actions_t offboard_loss_rc_act)
996 {
997  if (old_failsafe) {
998  if (offboard_loss_act == offboard_loss_actions_t::TERMINATE
999  || offboard_loss_rc_act == offboard_loss_rc_actions_t::TERMINATE) {
1000  armed->force_failsafe = false;
1001 
1002  }
1003 
1004  if (offboard_loss_act == offboard_loss_actions_t::LOCKDOWN
1005  || offboard_loss_rc_act == offboard_loss_rc_actions_t::LOCKDOWN) {
1006  armed->lockdown = false;
1007  }
1008  }
1009 }
1010 
1011 
1012 
1013 
1015  const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
1016  const low_battery_action_t low_battery_action)
1017 {
1018  switch (battery_warning) {
1019  case battery_status_s::BATTERY_WARNING_NONE:
1020  break;
1021 
1022  case battery_status_s::BATTERY_WARNING_LOW:
1023  mavlink_log_critical(mavlink_log_pub, "Low battery level! Return advised");
1024  break;
1025 
1026  case battery_status_s::BATTERY_WARNING_CRITICAL:
1027 
1028  static constexpr char battery_critical[] = "Critical battery level!";
1029 
1030  switch (low_battery_action) {
1032  mavlink_log_critical(mavlink_log_pub, "%s, landing advised", battery_critical);
1033  break;
1034 
1036 
1037  // FALLTHROUGH
1039 
1040  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
1041  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
1042  internal_state->timestamp = hrt_absolute_time();
1043  mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical);
1044 
1045  } else {
1046  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1047  internal_state->timestamp = hrt_absolute_time();
1048  mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical);
1049  }
1050 
1051  break;
1052 
1053  case LOW_BAT_ACTION::LAND:
1054  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1055  internal_state->timestamp = hrt_absolute_time();
1056  mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
1057 
1058  break;
1059  }
1060 
1061  break;
1062 
1063  case battery_status_s::BATTERY_WARNING_EMERGENCY:
1064 
1065  static constexpr char battery_dangerous[] = "Dangerous battery level!";
1066 
1067  switch (low_battery_action) {
1069  mavlink_log_emergency(mavlink_log_pub, "%s, please land!", battery_dangerous);
1070  break;
1071 
1073  if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
1074  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
1075  internal_state->timestamp = hrt_absolute_time();
1076  mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical);
1077 
1078  } else {
1079  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1080  internal_state->timestamp = hrt_absolute_time();
1081  mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical);
1082  }
1083 
1084  break;
1085 
1087 
1088  // FALLTHROUGH
1089  case LOW_BAT_ACTION::LAND:
1090  internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1091  internal_state->timestamp = hrt_absolute_time();
1092  mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
1093 
1094  break;
1095  }
1096 
1097  break;
1098 
1099  case battery_status_s::BATTERY_WARNING_FAILED:
1100  mavlink_log_emergency(mavlink_log_pub, "Battery failure detected");
1101  break;
1102  }
1103 }
#define VEHICLE_TYPE_FIXED_WING
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning, const low_battery_action_t low_battery_action)
static orb_advert_t * mavlink_log_pub
static constexpr const char reason_no_datalink[]
static struct vehicle_status_s status
Definition: Commander.cpp:138
State machine helper functions definitions.
static struct vehicle_status_flags_s status_flags
Definition: Commander.cpp:155
static constexpr const char reason_no_rc[]
const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX]
static struct commander_state_s internal_state
Definition: Commander.cpp:142
link_loss_actions_t
uint8_t arming_state_t
Definition: uORB.h:257
bool safety_switch_available
Definition: safety.h:54
High-resolution timer with callouts and timekeeping.
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot)
Runs a preflight check on all sensors to see if they are properly calibrated and healthy.
static struct safety_s safety
Definition: Commander.cpp:140
uint32_t armed_time_ms
Commander helper functions definitions.
void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
Enable failsafe and report to user.
offboard_loss_actions_t
uint8_t navigation_state_t
Definition: uORB.h:260
static constexpr const char reason_no_rc_and_no_offboard[]
static hrt_abstime last_preflight_check
initialize so it gets checked immediately
static struct actuator_armed_s armed
Definition: Commander.cpp:139
bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
enum LOW_BAT_ACTION low_battery_action_t
static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
bool is_safe(const safety_s &safety, const actuator_armed_s &armed)
uint8_t main_state_t
Definition: uORB.h:258
static uint8_t arm_requirements
Definition: Commander.cpp:159
void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *armed, const vehicle_status_flags_s &status_flags, const offboard_loss_rc_actions_t offboard_loss_rc_act)
offboard_loss_rc_actions_t
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, const uint8_t arm_requirements)
Definition: preArmCheck.cpp:40
position_nav_loss_actions_t
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act)
transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state)
bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state, orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, const offboard_loss_rc_actions_t offb_loss_rc_act, const position_nav_loss_actions_t posctl_nav_loss_act)
Check failsafe and main status and set navigation status for navigator accordingly.
transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot)
static constexpr const char reason_no_global_position[]
Check if flight is safely possible if not prevent it and inform the user.
static constexpr const char reason_no_local_position[]
transition_result_t
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state)
bool safety_off
Definition: safety.h:55
static constexpr const char reason_no_offboard[]
void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, const vehicle_status_flags_s &status_flags, const offboard_loss_actions_t offboard_loss_act)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const offboard_loss_actions_t offboard_loss_act, const offboard_loss_rc_actions_t offboard_loss_rc_act)