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

navigator: add home position check for RTL state

This commit is contained in:
Anton Babushkin 2014-01-28 21:44:46 +01:00
parent 6a1a29f77e
commit 23a87f5a52
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/navigator/navigator_main.cpp
View File

@ -685,7 +685,8 @@ Navigator::task_main()
/* RC signal available, use control switches to set mode */ /* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */ /* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { /* switch to RTL if not already landed after RTL and home position set */
if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED); dispatch(EVENT_RTL_REQUESTED);
} }
@ -742,7 +743,7 @@ Navigator::task_main()
break; break;
case NAV_STATE_RTL: case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED); dispatch(EVENT_RTL_REQUESTED);
} }
@ -815,6 +816,11 @@ Navigator::task_main()
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
global_position_update(); global_position_update();
/* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true;
}
/* only check if waypoint has been reached in MISSION and RTL modes */ /* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) { if (check_mission_item_reached()) {