42 #include <px4_platform_common/px4_config.h> 65 static constexpr
const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
68 {
true,
true,
false,
true,
false,
false },
69 {
true,
true,
true,
false,
false,
false },
70 {
false,
true,
true,
false,
false,
true },
71 {
true,
true,
true,
true,
false,
false },
72 {
true,
true,
false,
true,
true,
true },
73 {
false,
false,
false,
false,
false,
false },
90 uint8_t auto_recovery_nav_state);
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");
118 bool feedback_provided =
false;
120 const bool hil_enabled = (status->
hil_state == vehicle_status_s::HIL_STATE_ON);
123 if (new_arming_state == current_arming_state) {
131 bool preflight_check_ret =
true;
136 if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
142 if (preflight_check_ret) {
146 feedback_provided =
true;
152 && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
153 || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
160 checkGNSS,
false,
false, time_since_boot);
169 if (valid_transition) {
171 if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
175 if (status->
arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && !hil_enabled) {
177 bool prearm_check_ret =
true;
179 if (fRunPreArmChecks && preflight_check_ret) {
184 if (!preflight_check_ret || !prearm_check_ret) {
186 feedback_provided =
true;
187 valid_transition =
false;
199 if (status->
arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
200 status->
arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
204 if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
205 valid_transition =
true;
210 (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
211 (status->
arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
215 feedback_provided =
true;
216 valid_transition =
false;
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);
228 if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
239 if (!feedback_provided) {
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:
279 case commander_state_s::MAIN_STATE_ALTCTL:
289 case commander_state_s::MAIN_STATE_POSCTL:
299 case commander_state_s::MAIN_STATE_AUTO_LOITER:
308 case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
309 case commander_state_s::MAIN_STATE_ORBIT:
312 if (status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
318 case commander_state_s::MAIN_STATE_AUTO_MISSION:
329 case commander_state_s::MAIN_STATE_AUTO_RTL:
338 case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
339 case commander_state_s::MAIN_STATE_AUTO_LAND:
348 case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
353 && status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
360 case commander_state_s::MAIN_STATE_OFFBOARD:
370 case commander_state_s::MAIN_STATE_MAX:
376 if (internal_state->
main_state != new_main_state) {
393 if (!old_failsafe && status->
arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
414 const bool rc_lost = rc_loss_act_configured && (status->
rc_signal_lost);
416 bool is_armed = (status->
arming_state == vehicle_status_s::ARMING_STATE_ARMED);
417 bool old_failsafe = status->
failsafe;
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:
434 if (rc_lost && is_armed) {
438 vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
442 case commander_state_s::MAIN_STATE_ACRO:
443 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
446 case commander_state_s::MAIN_STATE_MANUAL:
447 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
450 case commander_state_s::MAIN_STATE_RATTITUDE:
451 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
454 case commander_state_s::MAIN_STATE_STAB:
455 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
458 case commander_state_s::MAIN_STATE_ALTCTL:
459 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
463 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
470 case commander_state_s::MAIN_STATE_POSCTL: {
472 if (rc_lost && is_armed) {
476 vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
490 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
495 case commander_state_s::MAIN_STATE_AUTO_MISSION:
506 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
509 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
512 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
514 }
else if (data_link_loss_act_configured && status->
data_link_lost && is_armed) {
520 vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
523 && mission_finished && is_armed) {
530 vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
532 }
else if (!stay_in_failsafe) {
534 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
539 case commander_state_s::MAIN_STATE_AUTO_LOITER:
543 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
547 }
else if (status->
data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
550 vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
554 }
else if (rc_lost && !data_link_loss_act_configured && is_armed) {
559 vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
565 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
569 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
574 case commander_state_s::MAIN_STATE_AUTO_RTL:
579 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
584 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
589 case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
594 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
600 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
605 case commander_state_s::MAIN_STATE_ORBIT:
608 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
612 internal_state->
main_state = commander_state_s::MAIN_STATE_POSCTL;
619 internal_state->
main_state = commander_state_s::MAIN_STATE_POSCTL;
621 }
else if (status->
data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
624 vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
630 internal_state->
main_state = commander_state_s::MAIN_STATE_POSCTL;
632 }
else if (rc_lost && !data_link_loss_act_configured && is_armed) {
637 vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
641 internal_state->
main_state = commander_state_s::MAIN_STATE_POSCTL;
645 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
650 case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
655 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
661 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
666 case commander_state_s::MAIN_STATE_AUTO_LAND:
671 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
677 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
682 case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
687 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
693 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
698 case commander_state_s::MAIN_STATE_OFFBOARD:
714 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
721 return status->
nav_state != nav_state_old;
727 bool fallback_required =
false;
730 fallback_required =
true;
732 }
else if (!using_global_pos
735 fallback_required =
true;
738 if (fallback_required) {
741 if (status->
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
743 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
746 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
749 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
755 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
758 if (status->
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
759 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
763 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
767 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
771 if (using_global_pos) {
780 return fallback_required;
786 uint8_t auto_recovery_nav_state)
790 switch (link_loss_act) {
797 status->
nav_state = auto_recovery_nav_state;
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;
812 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
819 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
823 if (status->
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
825 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
829 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
835 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
842 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
868 switch (offboard_loss_act) {
874 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
884 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
891 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
898 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
906 if (status->
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
907 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
911 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
915 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
923 switch (offboard_loss_rc_act) {
929 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
939 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
946 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
952 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
957 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
964 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
971 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
980 if (status->
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
981 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
985 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
989 status->
nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
1018 switch (battery_warning) {
1019 case battery_status_s::BATTERY_WARNING_NONE:
1022 case battery_status_s::BATTERY_WARNING_LOW:
1026 case battery_status_s::BATTERY_WARNING_CRITICAL:
1028 static constexpr
char battery_critical[] =
"Critical battery level!";
1030 switch (low_battery_action) {
1041 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
1046 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1054 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1063 case battery_status_s::BATTERY_WARNING_EMERGENCY:
1065 static constexpr
char battery_dangerous[] =
"Dangerous battery level!";
1067 switch (low_battery_action) {
1074 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
1079 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1090 internal_state->
main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
1099 case battery_status_s::BATTERY_WARNING_FAILED:
#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
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
static constexpr const char reason_no_datalink[]
static struct vehicle_status_s status
State machine helper functions definitions.
static struct vehicle_status_flags_s status_flags
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
bool safety_switch_available
bool condition_global_position_valid
bool condition_home_position_valid
High-resolution timer with callouts and timekeeping.
bool offboard_control_loss_timeout
bool condition_local_velocity_valid
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
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.
bool condition_system_sensors_initialized
bool condition_local_altitude_valid
uint8_t navigation_state_t
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
bool offboard_control_signal_lost
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.
bool condition_auto_mission_available
enum LOW_BAT_ACTION low_battery_action_t
bool vtol_transition_failure
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.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
bool is_safe(const safety_s &safety, const actuator_armed_s &armed)
static uint8_t arm_requirements
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)
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)
#define mavlink_log_emergency(_pub, _text,...)
Send a mavlink emergency message and print to console.
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[]
bool condition_local_position_valid
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)
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)