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

commander: modes fallback and reject messages fixed

This commit is contained in:
Anton Babushkin 2014-06-25 17:56:11 +02:00
parent 9d3d5a30af
commit bdf1b9274c
1 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/commander/commander.cpp
View File

@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL"); print_reject_mode(status, "POSCTL");
} }
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL); res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
print_reject_mode(status, "ALTCTL"); print_reject_mode(status, "ALTCTL");
} }
// else fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL); res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) { } else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_LOITER");
} else { } else {
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status, "AUTO_MISSION");
} }
// else fallback to ALTCTL (POSCTL likely will not work too) // fallback to POSCTL
res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL); res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// else fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL); res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;