diff options
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 206 |
1 files changed, 112 insertions, 94 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17db0f9c8..66b9272de 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } } /* Main state machine */ @@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v return res; } +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + default: + break; + } +} + void *commander_low_prio_loop(void *arg) { /* Set thread name */ @@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) continue; - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle low-priority commands here */ switch (cmd.command) { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { - - if (((int)(cmd.param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd.param1)) == 3) { - /* reboot to bootloader */ - systemreset(true); - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); } else { - result = VEHICLE_CMD_RESULT_DENIED; + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; - /* try to go to INIT/PREFLIGHT arming state */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - result = VEHICLE_CMD_RESULT_DENIED; - break; - } + int calib_ret = ERROR; - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* try to go to INIT/PREFLIGHT arming state */ - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - do_accel_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - - break; } - - default: break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - } + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ |