fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 12:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: OFFBOARD mode added, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-22 17:04:07 +01:00
parent 1dc9785083
commit 37e28e7fa8
2 changed files with 56 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+50 -44
View File
@@ -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;
case MODE_SWITCH_ASSISTED:
if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
res = main_state_transition(current_status, MAIN_STATE_EASY);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to SEATBELT
print_reject_mode("EASY");
}
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this mode
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);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to SEATBELT
print_reject_mode("EASY");
// 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
// else fallback to MANUAL
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
default:
break;
}
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this mode
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);
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);
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;
default:
break;
}
return res;