diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-22 17:04:07 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-22 17:04:07 +0100 |
| commit | 37e28e7fa82a965967abba6650ba24a31aba10d3 (patch) | |
| tree | 371d058923540f5428b3e9c2b36593a7ea0f8541 | |
| parent | 1dc9785083e1ed7a7db27466fbe9f61f6bb277f7 (diff) | |
| download | px4-firmware-37e28e7fa82a965967abba6650ba24a31aba10d3.tar.gz px4-firmware-37e28e7fa82a965967abba6650ba24a31aba10d3.tar.bz2 px4-firmware-37e28e7fa82a965967abba6650ba24a31aba10d3.zip | |
commander: OFFBOARD mode added, WIP
| -rw-r--r-- | src/modules/commander/commander.cpp | 80 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 6 |
2 files changed, 49 insertions, 37 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2b6ad1ca..dfcb1cc9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1429,56 +1429,62 @@ check_main_state_machine(struct vehicle_status_s *current_status) /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { - case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + /* offboard switch overrides main switch */ + res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + } else { + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - default: - break; + default: + break; + } } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..e5599e1c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -263,6 +263,12 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m } break; + + case MAIN_STATE_OFFBOARD: + + ret = TRANSITION_CHANGED; + + break; } if (ret == TRANSITION_CHANGED) { |