mc_pos_control: Use arming state to prevent unsafe reference point changes

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;
-
 	}
 }