Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Merge pull request #399 from PX4/arm_alarm

Browse Source 
Beep and print message when arming is not allowed
This commit is contained in:
Lorenz Meier
2013-09-14 02:10:54 -07:00
parent 7ad2654b2d 8a70efdf43
commit d25a4caebb
1 changed files with 65 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+64 -38
View File
@@ -369,7 +369,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
transition_result_t arming_res = TRANSITION_NOT_CHANGED; transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); if (safety->safety_switch_available && !safety->safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
@@ -1083,10 +1089,18 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */ /* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY && if (status.arming_state == ARMING_STATE_STANDBY &&
status.main_state == MAIN_STATE_MANUAL &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
@@ -1106,15 +1120,8 @@ int commander_thread_main(int argc, char *argv[])
} }
} else if (res == TRANSITION_DENIED) { } else if (res == TRANSITION_DENIED) {
/* DENIED here indicates safety switch not pressed */ 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);
if (!(!safety.safety_switch_available || safety.safety_off)) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else {
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);
}
} }
/* fill current_status according to mode switches */ /* fill current_status according to mode switches */
@@ -1228,6 +1235,7 @@ int commander_thread_main(int argc, char *argv[])
/* wait for threads to complete */ /* wait for threads to complete */
ret = pthread_join(commander_low_prio_thread, NULL); ret = pthread_join(commander_low_prio_thread, NULL);
if (ret) { if (ret) {
warn("join failed", ret); warn("join failed", ret);
} }
@@ -1295,8 +1303,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (armed->armed) { if (armed->armed) {
rgbled_set_mode(RGBLED_MODE_ON); rgbled_set_mode(RGBLED_MODE_ON);
} else if (armed->ready_to_arm) { } else if (armed->ready_to_arm) {
rgbled_set_mode(RGBLED_MODE_BREATHE); rgbled_set_mode(RGBLED_MODE_BREATHE);
} else { } else {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
} }
@@ -1304,29 +1314,35 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
switch (status->battery_warning) { switch (status->battery_warning) {
case VEHICLE_BATTERY_WARNING_LOW: case VEHICLE_BATTERY_WARNING_LOW:
rgbled_set_color(RGBLED_COLOR_YELLOW); rgbled_set_color(RGBLED_COLOR_YELLOW);
break; break;
case VEHICLE_BATTERY_WARNING_CRITICAL:
rgbled_set_color(RGBLED_COLOR_AMBER); case VEHICLE_BATTERY_WARNING_CRITICAL:
break; rgbled_set_color(RGBLED_COLOR_AMBER);
default: break;
break;
default:
break;
} }
} else { } else {
switch (status->main_state) { switch (status->main_state) {
case MAIN_STATE_MANUAL: case MAIN_STATE_MANUAL:
rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_color(RGBLED_COLOR_WHITE);
break; break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_EASY: case MAIN_STATE_SEATBELT:
rgbled_set_color(RGBLED_COLOR_GREEN); case MAIN_STATE_EASY:
break; rgbled_set_color(RGBLED_COLOR_GREEN);
case MAIN_STATE_AUTO: break;
rgbled_set_color(RGBLED_COLOR_BLUE);
break; case MAIN_STATE_AUTO:
default: rgbled_set_color(RGBLED_COLOR_BLUE);
break; break;
default:
break;
} }
} }
@@ -1497,16 +1513,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
return TRANSITION_NOT_CHANGED; return TRANSITION_NOT_CHANGED;
} }
} }
if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */ /* possibly on ground, switch to TAKEOFF if needed */
if (local_pos->z > -takeoff_alt || status->condition_landed) { if (local_pos->z > -takeoff_alt || status->condition_landed) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res; return res;
} }
} }
/* switch to AUTO mode */ /* switch to AUTO mode */
if (status->rc_signal_found_once && !status->rc_signal_lost) { if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */ /* act depending on switches when manual control enabled */
@@ -1524,18 +1542,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
} }
} }
} else { } else {
/* switch to MISSION when no RC control and first time in some AUTO mode */ /* switch to MISSION when no RC control and first time in some AUTO mode */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL || status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED; res = TRANSITION_NOT_CHANGED;
} else { } else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} }
} }
} else { } else {
/* disarmed, always switch to AUTO_READY */ /* disarmed, always switch to AUTO_READY */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1762,35 +1782,41 @@ void *commander_low_prio_loop(void *arg)
if (((int)(cmd.param1)) == 0) { if (((int)(cmd.param1)) == 0) {
int ret = param_load_default(); int ret = param_load_default();
if (ret == OK) { if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */ /* convenience as many parts of NuttX use negative errno */
if (ret < 0) if (ret < 0)
ret = -ret; ret = -ret;
if (ret < 1000) if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
} }
} else if (((int)(cmd.param1)) == 1) { } else if (((int)(cmd.param1)) == 1) {
int ret = param_save_default(); int ret = param_save_default();
if (ret == OK) { if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
/* convenience as many parts of NuttX use negative errno */ /* convenience as many parts of NuttX use negative errno */
if (ret < 0) if (ret < 0)
ret = -ret; ret = -ret;
if (ret < 1000) if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
} }
} }