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

navigator: handle regaining global position lock while LANDing

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-29 13:12:57 +01:00
parent 23a87f5a52
commit 48cec50dd3
1 changed files with 14 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+14 -5
View File
@@ -178,6 +178,7 @@ private:
class Mission _mission; class Mission _mission;
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached; bool _waypoint_position_reached;
bool _waypoint_yaw_reached; bool _waypoint_yaw_reached;
@@ -393,6 +394,7 @@ Navigator::Navigator() :
_fence_valid(false), _fence_valid(false),
_inside_fence(true), _inside_fence(true),
_mission(), _mission(),
_global_pos_valid(false),
_reset_loiter_pos(true), _reset_loiter_pos(true),
_waypoint_position_reached(false), _waypoint_position_reached(false),
_waypoint_yaw_reached(false), _waypoint_yaw_reached(false),
@@ -819,12 +821,19 @@ Navigator::task_main()
/* publish position setpoint triplet on each position update if navigator active */ /* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true; _pos_sp_triplet_updated = true;
}
/* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { /* got global position when landing, update setpoint */
if (check_mission_item_reached()) { start_land();
on_mission_item_reached(); }
_global_pos_valid = _global_pos.global_valid;
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
on_mission_item_reached();
}
} }
} }