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

Merge pull request #317 from DrTon/arm_safe_fix 

Arm/disarm and SAS modes order safety fixes
This commit is contained in:
Lorenz Meier 2013-07-10 22:53:45 -07:00
parent 897b541b12 2f1de6621b
commit 3a76162b08
1 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1389,6 +1389,7 @@ int commander_thread_main(int argc, char *argv[])
uint64_t start_time = hrt_absolute_time();
uint64_t failsave_ll_start_time = 0;
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
bool state_changed = true;
bool param_init_forced = true;
@ -1828,8 +1829,9 @@ int commander_thread_main(int argc, char *argv[])
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
/* bottom stick position, set default */
/* this MUST be mapped to extremal position to switch easy in case of emergency */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
@ -1837,8 +1839,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
} else {
/* center stick position, set default */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
/* center stick position, set altitude hold */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
}
if (current_status.manual_sas_mode != manual_sas_mode) {
/* publish SAS mode changes immediately */
manual_sas_mode = current_status.manual_sas_mode;
state_changed = true;
}
/*
@ -1849,8 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) &&
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
current_status.flag_control_manual_enabled &&
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
sp_man.yaw < -STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
@ -1862,7 +1872,10 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if left stick is in lower right position --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (current_status.flag_control_manual_enabled &&
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
sp_man.yaw > STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;