diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 660f5cc697..29d78bfcf0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -410,10 +410,10 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = prearm_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = prearm_check(&mavlink_log_pub, false, true, &status_flags, battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = prearm_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; @@ -622,8 +622,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co // Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, - &battery, - &safety, + battery, + safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, @@ -1854,17 +1854,10 @@ Commander::run() arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, - &battery, - &safety, - new_arming_state, - &armed, - true /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp))) { + if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, new_arming_state, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, + &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) + ) { + status_changed = true; } } @@ -2223,17 +2216,10 @@ Commander::run() /* If in INIT state, try to proceed to STANDBY state */ if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { - arming_ret = arming_state_transition(&status, - &battery, - &safety, - vehicle_status_s::ARMING_STATE_STANDBY, - &armed, - true /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp)); + + arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_DENIED) { /* do not complain if not allowed into standby */ @@ -2453,17 +2439,8 @@ Commander::run() print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { - arming_ret = arming_state_transition(&status, - &battery, - &safety, - vehicle_status_s::ARMING_STATE_STANDBY, - &armed, - true /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp)); + arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } stick_off_counter++; /* do not reset the counter when holding the arm button longer than needed */ @@ -2503,17 +2480,8 @@ Commander::run() print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - arming_ret = arming_state_transition(&status, - &battery, - &safety, - vehicle_status_s::ARMING_STATE_ARMED, - &armed, - true /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp)); + arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret != TRANSITION_CHANGED) { usleep(100000); @@ -4010,17 +3978,9 @@ void *commander_low_prio_loop(void *arg) int calib_ret = PX4_ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, - &battery, - &safety, - vehicle_status_s::ARMING_STATE_INIT, - &armed, - false /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp))) { + if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_INIT, &armed, + false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); break; @@ -4117,17 +4077,8 @@ void *commander_low_prio_loop(void *arg) !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); - arming_state_transition(&status, - &battery, - &safety, - vehicle_status_s::ARMING_STATE_STANDBY, - &armed, - false /* fRunPreArmChecks */, - &mavlink_log_pub, - &status_flags, - avionics_power_rail_voltage, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp)); + arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } else { tune_negative(true); 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 072e21fcac..5f28465881 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -261,7 +261,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed, + transition_result_t result = arming_state_transition(&status, battery, safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c1808c6c82..da6163c7c4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -96,16 +96,16 @@ void set_link_loss_nav_state(vehicle_status_s *status, void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); 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, + const battery_status_s& battery, + const safety_s& safety, + const arming_state_t new_arming_state, actuator_armed_s *armed, - bool fRunPreArmChecks, + const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log vehicle_status_flags_s *status_flags, - float avionics_power_rail_voltage, - uint8_t arm_requirements, - hrt_abstime time_since_boot) + const float avionics_power_rail_voltage, + const uint8_t arm_requirements, + const 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"); @@ -146,9 +146,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, true, true, time_since_boot); - prearm_ret = prearm_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags, battery, arm_requirements, - time_since_boot); + prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, + status_flags, battery, arm_requirements, time_since_boot); if (!preflight_check) { prearm_ret = false; @@ -219,7 +218,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, // Fail transition if we need safety switch press - } else if (safety->safety_switch_available && !safety->safety_off) { + } else if (safety.safety_switch_available && !safety.safety_off) { mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!"); feedback_provided = true; @@ -1045,9 +1044,9 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c } } -int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, - vehicle_status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, - hrt_abstime time_since_boot) +int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report, + vehicle_status_flags_s *status_flags, const battery_status_s& battery, const uint8_t arm_requirements, + const hrt_abstime& time_since_boot) { bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && status_flags->condition_system_hotplug_timeout); @@ -1061,7 +1060,7 @@ int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool p } } - if (!status_flags->circuit_breaker_engaged_power_check && battery->warning >= battery_status_s::BATTERY_WARNING_LOW) { + if (!status_flags->circuit_breaker_engaged_power_check && battery.warning >= battery_status_s::BATTERY_WARNING_LOW) { prearm_ok = false; if (reportFailures) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cc29012838..a9f1f8c67a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -79,17 +79,17 @@ extern const char *const arming_state_names[]; bool is_safe(const safety_s& safety, const actuator_armed_s& armed); -transition_result_t arming_state_transition(struct vehicle_status_s *status, - struct battery_status_s *battery, - const struct safety_s *safety, - arming_state_t new_arming_state, - struct actuator_armed_s *armed, - bool fRunPreArmChecks, +transition_result_t arming_state_transition(vehicle_status_s *status, + const battery_status_s& battery, + const safety_s& safety, + const arming_state_t new_arming_state, + actuator_armed_s *armed, + const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log vehicle_status_flags_s *status_flags, - float avionics_power_rail_voltage, - uint8_t arm_requirements, - hrt_abstime time_since_boot); + const float avionics_power_rail_voltage, + const uint8_t arm_requirements, + const hrt_abstime& time_since_boot); 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); @@ -124,8 +124,8 @@ bool check_invalid_pos_nav_state(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 -int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, - bool force_report, vehicle_status_flags_s *status_flags, battery_status_s *battery, - uint8_t arm_requirements, hrt_abstime time_since_boot); +int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, + const bool force_report, vehicle_status_flags_s *status_flags, const battery_status_s& battery, + const uint8_t arm_requirements, const hrt_abstime& time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */