From e556649f2ff6922a7a3b7751b68cdedd0d6254aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:48:41 +0200 Subject: Beep when mode is not possible --- src/modules/commander/commander.c | 3 --- src/modules/commander/state_machine_helper.c | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 6812fb1fb..1d3f90807 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[]) // printf("checking\n"); 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) { - - - printf("System Type: %d\n", current_status.system_type); if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 4f2fbc984..dcaf775b9 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed first */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); } else { ret = OK; 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 */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); } else { ret = OK; 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 */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); } else { ret = OK; 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 */ if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); } else { ret = OK; 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 */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); } else { ret = OK; 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 */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; -- cgit v1.2.3