diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 88fbaca21dc4..0c7c1c03f7fe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -401,12 +401,8 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int checkres = 0; - 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(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); - warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); + bool checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED"); return 0; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5d1094eca42d..84f52d93d933 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -85,7 +85,6 @@ const char *const arming_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(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, @@ -118,7 +117,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt /* * Get sensing state if necessary */ - int prearm_ret = OK; + bool prearm_check_ret = true; + bool checkAirspeed = false; bool sensor_checks = !hil_enabled; @@ -136,11 +136,11 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, true, true, time_since_boot); - prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, status_flags, battery, + prearm_check_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; + prearm_check_ret = false; } } @@ -153,16 +153,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { - prearm_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed, + prearm_check_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, false, false, time_since_boot); - status_flags->condition_system_sensors_initialized = (prearm_ret == OK); + status_flags->condition_system_sensors_initialized = prearm_check_ret; last_preflight_check = hrt_absolute_time(); - last_prearm_ret = prearm_ret; - - } else { - prearm_ret = last_prearm_ret; } } @@ -176,7 +172,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt /* enforce lockdown in HIL */ if (hil_enabled) { armed->lockdown = true; - prearm_ret = OK; + prearm_check_ret = true; status_flags->condition_system_sensors_initialized = true; /* recover from a prearm fail */ @@ -201,7 +197,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt !hil_enabled) { // Fail transition if pre-arm check fails - if (prearm_ret) { + if (!prearm_check_ret) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; @@ -1035,7 +1031,7 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c } } -int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report, +bool 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) { @@ -1075,5 +1071,5 @@ int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool fo status_flags->condition_system_prearm_error_reported = true; } - return !prearm_ok; + return prearm_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e40642def11e..4acec95e70cc 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -106,7 +106,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ 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); -int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report, +bool 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);