dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Fixing HIL operation with failing preflight checks

This commit is contained in:
Lorenz Meier 2015-04-22 00:38:03 +02:00
parent 9c56aa386b
commit 5c4494b1c9
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/commander.cpp
View File

@ -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) {

12
src/modules/commander/state_machine_helper.cpp
View File

@ -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;
}