diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-08 14:49:55 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-20 09:14:12 +0200 |
| commit | 6f338eb1b205a124dab28b61a99b65e9950eb6a8 (patch) | |
| tree | ac9c1324377318fa710e8b0cccb3a0090d3ad07f | |
| parent | fde244f9032d5ce5bdac289699f06b4b3d800d48 (diff) | |
| download | px4-firmware-6f338eb1b205a124dab28b61a99b65e9950eb6a8.tar.gz px4-firmware-6f338eb1b205a124dab28b61a99b65e9950eb6a8.tar.bz2 px4-firmware-6f338eb1b205a124dab28b61a99b65e9950eb6a8.zip | |
Commander: Run preflight check on boot
| -rw-r--r-- | src/modules/commander/commander.cpp | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 10d49aa94..de2355e04 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); |