dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Beep when mode is not possible

This commit is contained in:
Julian Oes 2013-06-14 16:48:41 +02:00
parent 5b21362e1f
commit e556649f2f
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/commander.c
View File

@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[])
// printf("checking\n"); // printf("checking\n");
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
printf("System Type: %d\n", current_status.system_type);
if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||

19
src/modules/commander/state_machine_helper.c
View File

@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be disarmed first */ /* need to be disarmed first */
if (current_state->arming_state != ARMING_STATE_STANDBY) { if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be armed first */ /* need to be armed first */
if (current_state->arming_state != ARMING_STATE_ARMED) { if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -236,8 +238,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be disarmed and have a position estimate */ /* need to be disarmed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_STANDBY) { if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_local_position_valid) { } else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -265,8 +269,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be armed and have a position estimate */ /* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) { if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) { } else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -293,8 +299,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be armed and have a position estimate */ /* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) { if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) { } else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -317,10 +325,13 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be disarmed and have a position and home lock */ /* need to be disarmed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_STANDBY) { if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_global_position_valid) { } else if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) { } else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -343,6 +354,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to be armed and have a position and home lock */ /* need to be armed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_ARMED) { if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -380,8 +392,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to have a position and home lock */ /* need to have a position and home lock */
if (!current_state->condition_global_position_valid) { if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) { } else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -405,6 +419,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to have a mission ready */ /* need to have a mission ready */
if (!current_state-> condition_auto_mission_available) { if (!current_state-> condition_auto_mission_available) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -428,8 +443,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to have a position and home lock */ /* need to have a position and home lock */
if (!current_state->condition_global_position_valid) { if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) { } else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;
@ -450,8 +467,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* need to have a position and home lock */ /* need to have a position and home lock */
if (!current_state->condition_global_position_valid) { if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) { } else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos");
tune_negative();
} else { } else {
ret = OK; ret = OK;
current_state->flag_control_rates_enabled = true; current_state->flag_control_rates_enabled = true;