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

commander: do AUTO_MISSION after takeoff

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-26 13:53:43 +02:00
parent bf9282c988
commit baa2cab69d
1 changed files with 28 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+28 -30
View File
@@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
case MAIN_STATE_AUTO: case MAIN_STATE_AUTO:
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* check if takeoff completed */ /* don't switch to other states until takeoff not completed */
if (local_pos->z < -5.0f && !status.condition_landed) { if (local_pos->z > -5.0f || status.condition_landed) {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
} else {
res = TRANSITION_NOT_CHANGED; res = TRANSITION_NOT_CHANGED;
break;
} }
}
/* check again, state can be changed */
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
} else { } else {
if (current_status->condition_landed) { /* if not landed: */
/* if landed: transitions only to AUTO_READY are allowed */ if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); /* act depending on switches when manual control enabled */
// TRANSITION_DENIED is not possible here if (current_status->return_switch == RETURN_SWITCH_RETURN) {
break; /* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
/* if not landed: */
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
} else { } else {
/* always switch to loiter in air when no RC control */ if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); /* MISSION */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
} }
} else {
/* switch to mission in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} }
} }
} else { } else {