diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 4582504c96..b8cf5b5dd3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -127,7 +127,7 @@ bool in_transition_mode # True if VTOL is doing a transition bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_sensors_initialized -bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_prearm_error_reported # true if errors have already been reported bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available @@ -186,8 +186,3 @@ uint16 errors_count2 uint16 errors_count3 uint16 errors_count4 -bool circuit_breaker_engaged_power_check -bool circuit_breaker_engaged_airspd_check -bool circuit_breaker_engaged_enginefailure_check -bool circuit_breaker_engaged_gpsfailure_check -bool cb_usb diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 26383f058c..346809a408 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,6 +210,12 @@ static manual_control_setpoint_s _last_sp_man = {}; static struct vtol_vehicle_status_s vtol_status = {}; +static bool circuit_breaker_engaged_power_check; +static bool circuit_breaker_engaged_airspd_check; +static bool circuit_breaker_engaged_enginefailure_check; +static bool circuit_breaker_engaged_gpsfailure_check; +static bool cb_usb; + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -388,9 +394,15 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + cb_usb); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + cb_usb); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -612,9 +624,17 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co return TRANSITION_DENIED; } - // Transition the armed state. - arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, mavlink_log_pub_local); + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, + arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, + &armed, + true /* fRunPreArmChecks */, + mavlink_log_pub_local, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1244,10 +1264,10 @@ int commander_thread_main(int argc, char *argv[]) status.usb_connected = false; // CIRCUIT BREAKERS - status.circuit_breaker_engaged_power_check = false; - status.circuit_breaker_engaged_airspd_check = false; - status.circuit_breaker_engaged_enginefailure_check = false; - status.circuit_breaker_engaged_gpsfailure_check = false; + circuit_breaker_engaged_power_check = false; + circuit_breaker_engaged_airspd_check = false; + circuit_breaker_engaged_enginefailure_check = false; + circuit_breaker_engaged_gpsfailure_check = false; get_circuit_breaker_params(); /* publish initial state */ @@ -1455,7 +1475,7 @@ int commander_thread_main(int argc, char *argv[]) bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { checkAirspeed = true; } @@ -1472,7 +1492,7 @@ int commander_thread_main(int argc, char *argv[]) } else { // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !circuit_breaker_engaged_gpsfailure_check, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1655,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[]) /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { chAirspeed = true; } @@ -1667,7 +1687,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* check sensors also */ (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -1764,9 +1784,17 @@ int commander_thread_main(int argc, char *argv[]) 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, &safety, new_arming_state, &armed, - true /* fRunPreArmChecks */, &mavlink_log_pub)) { - mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, + &safety, + new_arming_state, + &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb)) { + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -2030,8 +2058,16 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { - arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub); + arming_ret = arming_state_transition(&status, + &safety, + vehicle_status_s::ARMING_STATE_STANDBY, + &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2069,7 +2105,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS is ok */ - if (!status.circuit_breaker_engaged_gpsfailure_check) { + if (!circuit_breaker_engaged_gpsfailure_check) { bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; //Check if GPS receiver is too noisy while we are disarmed @@ -2281,8 +2317,15 @@ int commander_thread_main(int argc, char *argv[]) /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ 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); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub); + arming_ret = arming_state_transition(&status, &safety, + new_arming_state, + &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2318,8 +2361,16 @@ int commander_thread_main(int argc, char *argv[]) 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, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub); + arming_ret = arming_state_transition(&status, + &safety, + vehicle_status_s::ARMING_STATE_ARMED, + &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2461,7 +2512,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!status.circuit_breaker_engaged_enginefailure_check && + if (!circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && @@ -2744,15 +2795,15 @@ int commander_thread_main(int argc, char *argv[]) void get_circuit_breaker_params() { - status.circuit_breaker_engaged_power_check = + circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.cb_usb = + cb_usb = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = + circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); - status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); - status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); } @@ -3486,8 +3537,16 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - false /* fRunPreArmChecks */, &mavlink_log_pub)) { + if (TRANSITION_DENIED == arming_state_transition(&status, + &safety, + vehicle_status_s::ARMING_STATE_INIT, + &armed, + false /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb)) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3566,14 +3625,23 @@ void *commander_low_prio_loop(void *arg) bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { checkAirspeed = true; } status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub); + arming_state_transition(&status, + &safety, + vehicle_status_s::ARMING_STATE_STANDBY, + &armed, + false /* fRunPreArmChecks */, + &mavlink_log_pub, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + circuit_breaker_engaged_power_check, + cb_usb); } else { tune_negative(true); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 737ad037ad..67b57d8139 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -116,6 +116,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s struct actuator_armed_s *armed, ///< current armed status bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing orb_advert_t *mavlink_log_pub) ///< uORB handle for mavlink log + bool circuit_breaker_engaged_airspd_check, + bool circuit_breaker_engaged_gpsfailure_check, + bool circuit_breaker_engaged_power_check, + bool cb_usb) { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); @@ -140,7 +144,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s 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 */ ); + prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + cb_usb); } /* re-run the pre-flight check as long as sensors are failing */ if (!status->condition_system_sensors_initialized @@ -149,7 +156,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s && 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 */); + prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, + circuit_breaker_engaged_airspd_check, + circuit_breaker_engaged_gpsfailure_check, + cb_usb); status->condition_system_sensors_initialized = !prearm_ret; last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -208,7 +218,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Perform power checks only if circuit breaker is not // engaged for these checks - if (!status->circuit_breaker_engaged_power_check) { + if (!circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { @@ -859,7 +869,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report) +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, + bool circuit_breaker_engaged_airspd_check, + bool circuit_breaker_engaged_gpsfailure_check, + bool cb_usb) { /* */ @@ -869,15 +882,15 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { + if (!circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; } bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + !circuit_breaker_engaged_gpsfailure_check, true, reportFailures); - if (!status->cb_usb && status->usb_connected && prearm) { + if (!cb_usb && status->usb_connected && prearm) { preflight_ok = false; if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f7da9860b9..805154f537 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,8 +56,16 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, + const struct safety_s *safety, + arming_state_t new_arming_state, + struct actuator_armed_s *armed, + bool fRunPreArmChecks, + orb_advert_t *mavlink_log_pub, + bool circuit_breaker_engaged_airspd_check, + bool circuit_breaker_engaged_gpsfailure_check, + bool circuit_breaker_engaged_power_check, + bool cb_usb); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); @@ -65,6 +73,9 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report=false); +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, + bool circuit_breaker_engaged_airspd_check, + bool circuit_breaker_engaged_gpsfailure_check, + bool cb_usb); #endif /* STATE_MACHINE_HELPER_H_ */