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

Commander: Run preflight check on boot

This commit is contained in:
Johan Jansen 2015-03-08 14:49:55 +01:00 committed by Lorenz Meier
parent fde244f903
commit 6f338eb1b2
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -536,9 +536,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
//Refuse to arm if preflight checks have failed
if(!status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED;
break;
}
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
@ -1115,6 +1122,8 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = true;
thread_running = true;
//Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
transition_result_t arming_ret;
@ -2104,7 +2113,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);