From f0e8abe783fa8fe9f7c73be89155f4b1ccb19f81 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
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;
-
}
}