fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 01:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-23 14:56:12 +04:00
parent 55fd19f281
commit 7c1693a877
3 changed files with 33 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+13 -26
View File
@@ -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, &current_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, &current_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, &current_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
stick_on_counter = 0;
src/modules/commander/state_machine_helper.cpp
+19 -5
View File
@@ -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;
}
}
src/modules/uORB/topics/vehicle_control_mode.h
+1
View File
@@ -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 */