From ef0e47ee63eaa32e3ca8cd12d6f72d3f13f7e865 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 19 Sep 2017 15:57:21 +1000 Subject: [PATCH] mc_pos_control: Fix bug causing flyaway when GPS gained after takeoff The change in origin when GPS was gained after takeoff was being used to shift the set point despite the previous origin being invalid. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1b2225bd8a..c44ec11481 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -266,6 +266,7 @@ private: struct map_projection_reference_s _ref_pos; float _ref_alt; hrt_abstime _ref_timestamp; + bool _first_origin_set; hrt_abstime _last_warn; math::Vector<3> _thrust_int; @@ -455,6 +456,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _user_intention_z(brake), _ref_alt(0.0f), _ref_timestamp(0), + _first_origin_set{false}, _last_warn(0), _yaw(0.0f), _yaw_takeoff(0.0f), @@ -858,7 +860,7 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) void MulticopterPositionControl::update_ref() { - if (_local_pos.ref_timestamp != _ref_timestamp) { + if (_local_pos.ref_timestamp != _ref_timestamp && _first_origin_set) { double lat_sp, lon_sp; float alt_sp = 0.0f; @@ -884,6 +886,13 @@ MulticopterPositionControl::update_ref() } _ref_timestamp = _local_pos.ref_timestamp; + + } else if (_local_pos.xy_global && _local_pos.z_global) { + // Ignore the origin change for the first time the origin is set. + // This allows for GPS use to commence after takeoff + _first_origin_set = true; + _ref_timestamp = _local_pos.ref_timestamp; + } }