From f0e8abe783fa8fe9f7c73be89155f4b1ccb19f81 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Sep 2017 08:38:52 +1000 Subject: [PATCH] mc_pos_control: Use arming state to prevent unsafe reference point changes --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) 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 c44ec11481..084db316c0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -266,7 +266,6 @@ 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; @@ -456,7 +455,6 @@ 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), @@ -860,7 +858,11 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) void MulticopterPositionControl::update_ref() { - if (_local_pos.ref_timestamp != _ref_timestamp && _first_origin_set) { + // The reference point is only allowed to change when the vehicle is in standby state which is the + // normal state when the estimator origin is set. Changing reference point in flight causes large controller + // setpoint changes. Changing reference point in other arming states is untested and shoud not be performed. + if ((_local_pos.ref_timestamp != _ref_timestamp) + && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { double lat_sp, lon_sp; float alt_sp = 0.0f; @@ -887,12 +889,6 @@ 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; - } }