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

Commander: Fix single channel mode switching

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-18 18:29:07 +02:00
parent b90f6dc157
commit 0adb4d8e8c
2 changed files with 63 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+57 -59
View File
@@ -2988,71 +2988,69 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
/* enable the use of break */
do {
/* fallback strategies, give the user the closest mode to what he wanted */
if (res == TRANSITION_DENIED) {
/* fallback strategies, give the user the closest mode to what he wanted */
while (res == TRANSITION_DENIED) {
if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
if (res != TRANSITION_DENIED) {
break;
} else {
/* fall back to loiter */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO MISSION");
}
}
/* fall back to loiter */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO MISSION");
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
} else {
/* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_POSCTL;
print_reject_mode(status_local, "AUTO PAUSE");
}
}
if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
} else {
/* fall back to altitude control */
new_mode = commander_state_s::MAIN_STATE_ALTCTL;
print_reject_mode(status_local, "POSITION CONTROL");
}
}
if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
} else {
/* fall back to stabilized */
new_mode = commander_state_s::MAIN_STATE_STAB;
print_reject_mode(status_local, "ALTITUDE CONTROL");
}
}
if (new_mode == commander_state_s::MAIN_STATE_STAB) {
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
} else {
/* there is no decent fallback any more, stay in mode and emit a warning */
print_reject_mode(status_local, "STABILIZED");
}
if (res != TRANSITION_DENIED) {
break;
}
}
} while (0);
if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
/* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_POSCTL;
print_reject_mode(status_local, "AUTO PAUSE");
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
}
}
if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
/* fall back to altitude control */
new_mode = commander_state_s::MAIN_STATE_ALTCTL;
print_reject_mode(status_local, "POSITION CONTROL");
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
}
}
if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
/* fall back to stabilized */
new_mode = commander_state_s::MAIN_STATE_STAB;
print_reject_mode(status_local, "ALTITUDE CONTROL");
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
}
}
if (new_mode == commander_state_s::MAIN_STATE_STAB) {
/* fall back to manual */
new_mode = commander_state_s::MAIN_STATE_MANUAL;
print_reject_mode(status_local, "STABILIZED");
res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
break;
}
}
}
}
return res;