aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/commander.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r--src/modules/commander/commander.cpp206
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 */
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 23:17:35 +0000