diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 13:12:57 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 13:12:57 +0100 |
commit | 48cec50dd30cd2b3163aedbeb11ae52866e2601b (patch) | |
tree | 4ca4552a960798b67f2a6c4be187ec55c3adc0da /src/modules/navigator/navigator_main.cpp | |
parent | 23a87f5a5204580b4fad435a75b38efc9d15c05c (diff) | |
download | px4-firmware-48cec50dd30cd2b3163aedbeb11ae52866e2601b.tar.gz px4-firmware-48cec50dd30cd2b3163aedbeb11ae52866e2601b.tar.bz2 px4-firmware-48cec50dd30cd2b3163aedbeb11ae52866e2601b.zip |
navigator: handle regaining global position lock while LANDing
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 19 |
1 files changed, 14 insertions, 5 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73b7c13d0..80bb1b752 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -178,6 +178,7 @@ private: 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 _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -393,6 +394,7 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), @@ -819,12 +821,19 @@ Navigator::task_main() /* 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 */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (check_mission_item_reached()) { - on_mission_item_reached(); + if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + /* got global position when landing, update setpoint */ + start_land(); + } + + _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(); + } } } |