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.cpp112
1 files changed, 59 insertions, 53 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2ef509980..44a144296 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -369,15 +369,17 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+
} else {
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
// TODO remove debug code
- //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
+ //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
arming_res = TRANSITION_NOT_CHANGED;
@@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
- case VEHICLE_CMD_COMPONENT_ARM_DISARM:
- {
- transition_result_t arming_res = TRANSITION_NOT_CHANGED;
- if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
- if (safety->safety_switch_available && !safety->safety_off) {
- print_reject_arm("NOT ARMING: Press safety switch first.");
- arming_res = TRANSITION_DENIED;
+ case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+ transition_result_t arming_res = TRANSITION_NOT_CHANGED;
- } else {
- arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
- }
+ if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+ if (safety->safety_switch_available && !safety->safety_off) {
+ print_reject_arm("NOT ARMING: Press safety switch first.");
+ arming_res = TRANSITION_DENIED;
- if (arming_res == TRANSITION_CHANGED) {
- mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- } else {
- mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ } else {
+ arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+ }
+
+ if (arming_res == TRANSITION_CHANGED) {
+ mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
}
}
- }
break;
default:
@@ -518,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
tune_negative();
if (result == VEHICLE_CMD_RESULT_DENIED) {
- mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+ mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
- mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+ mavlink_log_critical(mavlink_fd, "#audio: 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);
+ mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
}
}
@@ -687,7 +690,7 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
- bool rc_calibration_ok = (OK == rc_calibration_check());
+ bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -802,7 +805,7 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
/* re-check RC calibration */
- rc_calibration_ok = (OK == rc_calibration_check());
+ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
@@ -871,10 +874,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.condition_landed) {
- mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+ mavlink_log_critical(mavlink_fd, "#audio: LANDED");
} else {
- mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+ mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
}
@@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
- struct stat statbuf;
+ //struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
@@ -952,7 +955,7 @@ int commander_thread_main(int argc, char *argv[])
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+ mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
@@ -964,12 +967,13 @@ int commander_thread_main(int argc, char *argv[])
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+ mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
@@ -1065,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[])
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
- mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+ mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
status_changed = true;
} else {
if (status.rc_signal_lost) {
- mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+ mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
}
}
@@ -1139,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (res == TRANSITION_DENIED) {
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
- mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
/* fill current_status according to mode switches */
@@ -1155,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[])
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
- mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
} else {
if (!status.rc_signal_lost) {
- mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+ mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
@@ -1185,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[])
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
- mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+ mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
}
/* check which state machines for changes, clear "changed" flag */
@@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
counter++;
int blink_state = blink_msg_state();
+
if (blink_state > 0) {
/* blinking LED message, don't touch LEDs */
if (blink_state == 2) {
/* blinking LED message completed, restore normal state */
control_status_leds(&status, &armed, true);
}
+
} else {
/* normal state */
control_status_leds(&status, &armed, status_changed);
@@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
ret = pthread_join(commander_low_prio_thread, NULL);
if (ret) {
- warn("join failed", ret);
+ warn("join failed: %d", ret);
}
rgbled_set_mode(RGBLED_MODE_OFF);
@@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
/* driving rgbled */
if (changed) {
bool set_normal_color = false;
+
/* set mode */
if (status->arming_state == ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
@@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER);
}
+
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else {
@@ -1498,7 +1506,7 @@ print_reject_mode(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
- sprintf(s, "[cmd] WARNING: reject %s", msg);
+ sprintf(s, "#audio: warning: reject %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
}
@@ -1512,7 +1520,7 @@ print_reject_arm(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
- sprintf(s, "[cmd] %s", msg);
+ sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
}
@@ -1609,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
if (res == TRANSITION_CHANGED) {
if (control_mode->flag_control_position_enabled) {
- mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+ mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
} else {
if (status->condition_landed) {
- mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+ mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
} else {
- mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+ mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
}
}
}
@@ -1652,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break;
case VEHICLE_CMD_RESULT_DENIED:
- mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+ mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
- mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+ mavlink_log_critical(mavlink_fd, "#audio: 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);
+ mavlink_log_critical(mavlink_fd, "#audio: 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);
+ mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
tune_negative();
break;
@@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
fds[0].events = POLLIN;
while (!thread_should_exit) {
-
- /* wait for up to 100ms for data */
+ /* wait for up to 200ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
- /* timed out - periodic check for _task_should_exit, etc. */
+ /* timed out - periodic check for thread_should_exit, etc. */
if (pret == 0)
continue;
@@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
- answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_rc_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
@@ -1807,14 +1814,14 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
- mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+ mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
- mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+ mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1827,14 +1834,14 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
- mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+ mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
- mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+ mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
/* send acknowledge command */
// XXX TODO
}
-
}
close(cmd_sub);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-30 08:05:26 +0000