diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-06-25 17:56:11 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-06-25 17:56:11 +0200 |
| commit | bdf1b9274c369ddfd52bcb92952f2467abd7c26f (patch) | |
| tree | 9975f2af6656df6a61e4b47f9282a334fcf0551d /src/modules/commander/commander.cpp | |
| parent | 9d3d5a30af0781b975d17c5d73796f593c78b6e4 (diff) | |
| download | px4-firmware-bdf1b9274c369ddfd52bcb92952f2467abd7c26f.tar.gz px4-firmware-bdf1b9274c369ddfd52bcb92952f2467abd7c26f.tar.bz2 px4-firmware-bdf1b9274c369ddfd52bcb92952f2467abd7c26f.zip | |
commander: modes fallback and reject messages fixed
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -rw-r--r-- | src/modules/commander/commander.cpp | 30 |
1 files changed, 26 insertions, 4 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..ec4273fc3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_LOITER"); + } else { res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; |