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

navigator: switch to READY instead of LOITER if landed

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-16 14:45:18 +01:00
parent 5bb004a711
commit a6af669399
1 changed files with 35 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+35 -31
View File
@@ -305,6 +305,12 @@ private:
void start_land(); void start_land();
void start_land_home(); void start_land_home();
/**
* Fork for state transitions
*/
void request_loiter_or_ready();
void request_mission_if_available();
/** /**
* Guards offboard mission * Guards offboard mission
*/ */
@@ -699,24 +705,17 @@ Navigator::task_main()
} else { } else {
/* MISSION switch */ /* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED); request_loiter_or_ready();
stick_mode = true; stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */ request_mission_if_available();
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true; stick_mode = true;
} }
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED); request_mission_if_available();
stick_mode = true; stick_mode = true;
} }
} }
@@ -733,17 +732,11 @@ Navigator::task_main()
break; break;
case NAV_STATE_LOITER: case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED); request_loiter_or_ready();
break; break;
case NAV_STATE_MISSION: case NAV_STATE_MISSION:
if (_mission.current_mission_available()) { request_mission_if_available();
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break; break;
case NAV_STATE_RTL: case NAV_STATE_RTL:
@@ -770,12 +763,7 @@ Navigator::task_main()
} else { } else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */ /* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) { if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) { request_mission_if_available();
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
} }
} }
} }
@@ -1397,6 +1385,28 @@ Navigator::set_rtl_item()
_pos_sp_triplet_updated = true; _pos_sp_triplet_updated = true;
} }
void
Navigator::request_loiter_or_ready()
{
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
void
Navigator::request_mission_if_available()
{
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
request_loiter_or_ready();
}
}
void void
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
{ {
@@ -1555,13 +1565,7 @@ Navigator::on_mission_item_reached()
/* loiter at last waypoint */ /* loiter at last waypoint */
_reset_loiter_pos = false; _reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
request_loiter_or_ready();
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
} }
} else if (myState == NAV_STATE_RTL) { } else if (myState == NAV_STATE_RTL) {