diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-10 22:53:45 -0700 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-10 22:53:45 -0700 |
| commit | 3a76162b08ba6d8a8403aeac1889066964ac244b (patch) | |
| tree | 2ad3bf8ac94a51ba888d4f67d672399df3febebe /src/modules/commander | |
| parent | 897b541b12d5782af51ce0a78658cc153bd13544 (diff) | |
| parent | 2f1de6621b34f76ddf3a0ff00ac8e9fcb8e60bea (diff) | |
| download | px4-firmware-3a76162b08ba6d8a8403aeac1889066964ac244b.tar.gz px4-firmware-3a76162b08ba6d8a8403aeac1889066964ac244b.tar.bz2 px4-firmware-3a76162b08ba6d8a8403aeac1889066964ac244b.zip | |
Merge pull request #317 from DrTon/arm_safe_fix
Arm/disarm and SAS modes order safety fixes
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/commander.c | 27 |
1 files changed, 20 insertions, 7 deletions
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 67f053e22..928d9b85e 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -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, ¤t_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, ¤t_status, mavlink_fd); stick_on_counter = 0; |