diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 69b1ed79fe..90696d8272 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -104,9 +104,10 @@ static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = { static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately static int last_prearm_ret = 1; ///< initialize to fail -void set_link_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, +void set_link_loss_nav_state(vehicle_status_s *status, + actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state); @@ -114,11 +115,11 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); -transition_result_t arming_state_transition(struct vehicle_status_s *status, - struct battery_status_s *battery, +transition_result_t arming_state_transition(vehicle_status_s *status, + battery_status_s *battery, const struct safety_s *safety, arming_state_t new_arming_state, - struct actuator_armed_s *armed, + actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, @@ -583,7 +584,7 @@ bool set_nav_state(struct vehicle_status_s *status, if (rc_lost && is_armed) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); - set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act); + set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); } else { switch (internal_state->main_state) { @@ -620,7 +621,7 @@ bool set_nav_state(struct vehicle_status_s *status, if (rc_lost && is_armed) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); - set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act); + set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ /* A local position estimate is enough for POSCTL for multirotors, @@ -683,7 +684,7 @@ bool set_nav_state(struct vehicle_status_s *status, } else if (data_link_loss_act_configured && status->data_link_lost) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); - set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act); + set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act); /* datalink loss DISABLED: * check if both, RC and datalink are lost during the mission @@ -694,7 +695,7 @@ bool set_nav_state(struct vehicle_status_s *status, enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); - set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act); + set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ @@ -721,14 +722,14 @@ bool set_nav_state(struct vehicle_status_s *status, // nothing to do - everything done in check_invalid_pos_nav_state } else if (status->data_link_lost && data_link_loss_act_configured && !landed) { - set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act); + set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act); /* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */ } else if (rc_lost && !data_link_loss_act_configured) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); - set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act); + set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); /* don't bother if RC is lost if datalink is connected */ @@ -900,12 +901,13 @@ bool set_nav_state(struct vehicle_status_s *status, return status->nav_state != nav_state_old; } -void set_rc_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, +void set_rc_loss_nav_state(vehicle_status_s *status, + actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act) { - set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); + set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); } bool check_invalid_pos_nav_state(struct vehicle_status_s *status, @@ -956,17 +958,19 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status, } -void set_data_link_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, +void set_data_link_loss_nav_state(vehicle_status_s *status, + actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act) { - set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS); + set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS); } -void set_link_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, +void set_link_loss_nav_state(vehicle_status_s *status, + actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state) { @@ -980,6 +984,9 @@ void set_link_loss_nav_state(struct vehicle_status_s *status, } else if (link_loss_act == link_loss_actions_t::AUTO_RTL && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { + + uint8_t main_state_prev = 0; + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, internal_state); status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a113c07286..07bf46608a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -145,10 +145,6 @@ bool set_nav_state(struct vehicle_status_s *status, const int offb_loss_rc_act, const int posctl_nav_loss_act); -void set_rc_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, - status_flags_s *status_flags, - const link_loss_actions_t link_loss_act); /* * Checks the validty of position data aaainst the requirements of the current navigation * mode and switches mode if position data required is not available. @@ -160,10 +156,11 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status, const bool use_rc, // true if a mode using RC control can be used as a fallback const bool using_global_pos); // true when the current mode requires a global position estimate -void set_data_link_loss_nav_state(struct vehicle_status_s *status, - struct actuator_armed_s *armed, - status_flags_s *status_flags, - const link_loss_actions_t link_loss_act); +void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act); + +void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, status_flags_s *status_flags, + commander_state_s *internal_state, const link_loss_actions_t link_loss_act); int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery,