From 90b4afebb55627a28d62e412d252a518a7733909 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Dec 2017 16:11:13 +0100 Subject: [PATCH] Commander: properly separate preflight check and prearm checks We were running pre-arm checks before when not arming, which led to annoying error messages on vehicles that were on the bench or serviced on the ground. Now we really only run them when trying to arm. --- src/modules/commander/commander.cpp | 4 +- .../commander/state_machine_helper.cpp | 57 +++++++++++-------- src/modules/commander/state_machine_helper.h | 2 +- 3 files changed, 35 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ded6e6a9d9..56a3a86e2a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -447,10 +447,10 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = prearm_check(&status, &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 = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = prearm_check(&status, &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; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index aa4b7d82c2..763335f74f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -148,14 +148,32 @@ transition_result_t arming_state_transition(vehicle_status_s *status, * Get sensing state if necessary */ int prearm_ret = OK; + bool checkAirspeed = false; + bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); + + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { + checkAirspeed = true; + } /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, + bool preflight_check = 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, 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); + + if (!preflight_check) { + prearm_ret = false; + } + } /* re-run the pre-flight check as long as sensors are failing */ @@ -165,9 +183,11 @@ transition_result_t arming_state_transition(vehicle_status_s *status, && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { 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, arm_requirements, - time_since_boot); + + prearm_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); last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -556,7 +576,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason) { - if (!old_failsafe) { + if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason); } @@ -1081,29 +1101,16 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail } } -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, +int prearm_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, uint8_t arm_requirements, hrt_abstime time_since_boot) { bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && status_flags->condition_system_hotplug_timeout); - - bool checkAirspeed = false; - - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { - checkAirspeed = true; - } - - bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); - - bool preflight_ok = 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, reportFailures, prearm, time_since_boot); + bool prearm_ok = true; if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) { - preflight_ok = false; + prearm_ok = false; if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe"); @@ -1111,7 +1118,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } if (!status_flags->circuit_breaker_engaged_power_check && battery->warning >= battery_status_s::BATTERY_WARNING_LOW) { - preflight_ok = false; + prearm_ok = false; if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: LOW BATTERY"); @@ -1124,7 +1131,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p !status_flags->condition_home_position_valid || !status_flags->condition_global_position_valid)) { - preflight_ok = false; + prearm_ok = false; if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: valid mission required"); @@ -1132,9 +1139,9 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } /* report once, then set the flag */ - if (reportFailures && !preflight_ok) { + if (reportFailures && !prearm_ok) { status_flags->condition_system_prearm_error_reported = true; } - return !preflight_ok; + return !prearm_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 54ca219b52..e1f09d5f78 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -162,7 +162,7 @@ void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, st 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, +int prearm_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, uint8_t arm_requirements, hrt_abstime time_since_boot);