diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-23 14:56:12 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-23 14:56:12 +0400 |
commit | 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 (patch) | |
tree | 5b056cacc16acd33a4a9fbce5c7057190a9142eb /src | |
parent | 55fd19f2813110d14d536943503851255c997b6f (diff) | |
download | px4-firmware-7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492.tar.gz px4-firmware-7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492.tar.bz2 px4-firmware-7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492.zip |
commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 39 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 24 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 1 |
3 files changed, 33 insertions, 31 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea..1978d8782 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 60ab01536..4a7aa66b7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = false; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = false; } break; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } } @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } break; @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } break; @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 8481a624f..d83eb45d9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ |