diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-19 16:20:07 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-21 06:54:15 +0200 |
| commit | b9d17241a38166aa7e346787cf5f382d277a0e60 (patch) | |
| tree | 1067f86a84463b0f366548d581a9982a7badba89 | |
| parent | 2f5239c17a1604acada0d3ba2748fc1e5cb5676f (diff) | |
| download | px4-firmware-b9d17241a38166aa7e346787cf5f382d277a0e60.tar.gz px4-firmware-b9d17241a38166aa7e346787cf5f382d277a0e60.tar.bz2 px4-firmware-b9d17241a38166aa7e346787cf5f382d277a0e60.zip | |
RTL mode switching: Allow to flick to RTL in any mode.
| -rw-r--r-- | src/modules/commander/commander.cpp | 41 |
1 files changed, 23 insertions, 18 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb1ed7f5d..3fecd9f58 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2220,13 +2220,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + /* mode rejected, continue to evaluate the main system mode */ } else { + /* changed successfully or already in this state */ return res; } } - /* offboard switched off or denied, check main mode switch */ + /* RTL switch overrides main switch */ + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); + + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "AUTO_RTL"); + + /* fallback to LOITER if home position not set */ + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* mode rejected, continue to evaluate the main system mode */ + } + } + + /* offboard and RTL switches off or denied, check main mode switch */ switch (sp_man->mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -2271,23 +2292,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode(status_local, "AUTO_RTL"); - - // fallback to LOITER if home position not set - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { |