dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: Fix single channel mode switching

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
src/modules/commander/commander.cpp
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;

6
src/modules/commander/commander_params.c
View File

@ -279,6 +279,7 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
@ -301,6 +302,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
@ -323,6 +325,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
@ -345,6 +348,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
@ -367,6 +371,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
@ -389,5 +394,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
* @value 7 Offboard
* @value 8 Stabilized
* @value 9 Rattitude
* @value 12 Follow Me
*/
PARAM_DEFINE_INT32(COM_FLTMODE6, -1);