From 5c4494b1c9e9be2c6e88d4078c32f08ef2f12124 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Apr 2015 00:38:03 +0200 Subject: commander: Fixing HIL operation with failing preflight checks --- src/modules/commander/commander.cpp | 7 ++++--- src/modules/commander/state_machine_helper.cpp | 12 +++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65fc8f90e..b787a9d4d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -553,7 +553,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s else { //Refuse to arm if preflight checks have failed - if(!status.condition_system_sensors_initialized) { + if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); cmd_result = VEHICLE_CMD_RESULT_DENIED; break; @@ -1579,7 +1579,9 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + if (armed.armed) { + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -1587,7 +1589,6 @@ int commander_thread_main(int argc, char *argv[]) && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (!armed.armed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7410baca3..9aba61e14 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -137,6 +137,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; + prearm_ret = OK; + status->condition_system_sensors_initialized = true; } else { armed->lockdown = false; @@ -174,7 +176,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } @@ -184,7 +186,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { @@ -211,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; @@ -221,9 +223,9 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); } else { - mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } -- cgit v1.2.3