diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3d99b4d5010..012825bef45 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3940,7 +3940,7 @@ void *commander_low_prio_loop(void *arg) switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { + if (is_safe(&safety, &armed)) { if (((int)(cmd.param1)) == 1) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b36c623b237..82435829207 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -477,7 +477,6 @@ bool StateMachineHelperTest::mainStateTransitionTest() bool StateMachineHelperTest::isSafeTest() { - struct vehicle_status_s current_state = {}; struct safety_s safety = {}; struct actuator_armed_s armed = {}; @@ -485,31 +484,31 @@ bool StateMachineHelperTest::isSafeTest() armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - ut_compare("is safe: not armed", is_safe(¤t_state, &safety, &armed), true); + ut_compare("is safe: not armed", is_safe(&safety, &armed), true); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - ut_compare("is safe: software lockdown", is_safe(¤t_state, &safety, &armed), true); + ut_compare("is safe: software lockdown", is_safe(&safety, &armed), true); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - ut_compare("not safe: safety off", is_safe(¤t_state, &safety, &armed), false); + ut_compare("not safe: safety off", is_safe(&safety, &armed), false); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - ut_compare("is safe: safety off", is_safe(¤t_state, &safety, &armed), true); + ut_compare("is safe: safety off", is_safe(&safety, &armed), true); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - ut_compare("not safe: no safety switch", is_safe(¤t_state, &safety, &armed), false); + ut_compare("not safe: no safety switch", is_safe(&safety, &armed), false); return true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5824754f52e..d2e76a39c03 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include @@ -111,14 +110,14 @@ static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked 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, - status_flags_s *status_flags, - const link_loss_actions_t link_loss_act, - uint8_t auto_recovery_nav_state); + struct actuator_armed_s *armed, + status_flags_s *status_flags, + const link_loss_actions_t link_loss_act, + uint8_t auto_recovery_nav_state); void reset_link_loss_globals(struct actuator_armed_s *armed, - const bool old_failsafe, - const link_loss_actions_t link_loss_act); + 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, @@ -127,15 +126,15 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log - status_flags_s *status_flags, + status_flags_s *status_flags, float avionics_power_rail_voltage, - bool can_arm_without_gps, - hrt_abstime time_since_boot) + bool can_arm_without_gps, + hrt_abstime time_since_boot) { // Double check that our static arrays are still valid static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, - "ARMING_STATE_IN_AIR_RESTORE = ARMING_STATE_MAX - 1"); + "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; @@ -169,7 +168,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, status_flags, battery, can_arm_without_gps, time_since_boot); - status_flags->condition_system_sensors_initialized = !prearm_ret; + status_flags->condition_system_sensors_initialized = (prearm_ret == OK); last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -244,18 +243,16 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, // Check avionics rail voltages if (avionics_power_rail_voltage < 4.5f) { mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", - (double)avionics_power_rail_voltage); + (double)avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", - (double)avionics_power_rail_voltage); + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); feedback_provided = true; } else if (avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", - (double)avionics_power_rail_voltage); + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); feedback_provided = true; } } @@ -308,8 +305,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (status_flags->condition_system_hotplug_timeout) { if (!status_flags->condition_system_prearm_error_reported) { - mavlink_log_critical(mavlink_log_pub, - "Not ready to fly: Sensors not set up correctly"); + mavlink_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly"); status_flags->condition_system_prearm_error_reported = true; } } @@ -346,21 +342,22 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state], - state_names[new_arming_state]); + state_names[new_arming_state]); } } return ret; } -bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) +bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed) { // System is safe if: // 1) Not armed // 2) Armed, but in software lockdown (HIL) // 3) Safety switch is present AND engaged -> actuators locked - const bool lockdown = (armed->lockdown || armed->manual_lockdown); - if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + const bool lockdown = (armed->lockdown || armed->manual_lockdown); + + if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; } else { @@ -654,14 +651,15 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } /** - * Enable failsafe and repot to user + * Enable failsafe and report to user */ -void enable_failsafe(struct vehicle_status_s *status, - bool old_failsafe, - orb_advert_t *mavlink_log_pub, const char *reason) { - if (old_failsafe == false) { +void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, + const char *reason) +{ + if (!old_failsafe) { mavlink_and_console_log_info(mavlink_log_pub, reason); } + status->failsafe = true; } @@ -688,7 +686,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd); bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED - || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); bool old_failsafe = status->failsafe; status->failsafe = false; @@ -822,7 +820,9 @@ bool set_nav_state(struct vehicle_status_s *status, * check if both, RC and datalink are lost during the mission * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */ - } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) { + } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed + && mission_finished) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act); @@ -1091,64 +1091,50 @@ void set_link_loss_nav_state(struct vehicle_status_s *status, { // do the best you can according to the action set if (link_loss_act == link_loss_actions_t::AUTO_RECOVER - && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) - { + && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = auto_recovery_nav_state; - } - else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid) - { + + } else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - else if (link_loss_act == link_loss_actions_t::AUTO_RTL - && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) - { + + } else if (link_loss_act == link_loss_actions_t::AUTO_RTL + && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { 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) - { + + } else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } - else if (link_loss_act == link_loss_actions_t::TERMINATE) - { + + } else if (link_loss_act == link_loss_actions_t::TERMINATE) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; armed->force_failsafe = true; - } - else if (link_loss_act == link_loss_actions_t::LOCKDOWN) - { + + } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) { armed->lockdown = true; // do the best you can according to the current state - } - else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) - { + + } else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - else if (status_flags->condition_local_position_valid) - { + + } else if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } - else if (status_flags->condition_local_altitude_valid) - { + + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } - else - { + + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } -void reset_link_loss_globals(struct actuator_armed_s *armed, - const bool old_failsafe, +void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act) { - if (old_failsafe) - { - if (link_loss_act == link_loss_actions_t::TERMINATE) - { + if (old_failsafe) { + if (link_loss_act == link_loss_actions_t::TERMINATE) { armed->force_failsafe = false; - } - else if (link_loss_act == link_loss_actions_t::LOCKDOWN) - { + + } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) { armed->lockdown = false; } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2e21dd1de07..1963a340d25 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -105,7 +105,7 @@ struct status_flags_s { bool ever_had_barometer_data; // Set to true if ever had valid barometer data before }; -bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); +bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *status, struct battery_status_s *battery, @@ -123,7 +123,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, status_flags_s *status_flags, struct commander_state_s *internal_state); -transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub); +transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub); void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason);