diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-26 13:52:55 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-26 13:53:43 +0200 |
| commit | baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a (patch) | |
| tree | d666a7eda45c2057be7f0a5f1f7a704f79023bf2 /src/modules/commander/commander.cpp | |
| parent | bf9282c9882882a5fc4adf680a6ecc5e655bb0e8 (diff) | |
| download | px4-firmware-baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a.tar.gz px4-firmware-baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a.tar.bz2 px4-firmware-baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a.zip | |
commander: do AUTO_MISSION after takeoff
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -rw-r--r-- | src/modules/commander/commander.cpp | 54 |
1 files changed, 26 insertions, 28 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37a..d90008633 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { |