made position controllers handle estimator state resets

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -248,6 +248,10 @@ private:
 	float _asp_after_transition;
 	bool _was_in_transition;
 
+	// estimator reset counters
+	uint8_t _pos_reset_counter;		// captures the number of times the estimator has reset the horizontal position
+	uint8_t _alt_reset_counter;		// captures the number of times the estimator has reset the altitude state
+
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	enum FW_POSCTRL_MODE {
@@ -613,6 +617,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_last_tecs_update(0.0f),
 	_asp_after_transition(0.0f),
 	_was_in_transition(false),
+	_pos_reset_counter(0),
+	_alt_reset_counter(0),
 	_l1_control(),
 	_tecs(),
 	_control_mode_current(FW_POSCTRL_MODE_OTHER),
@@ -2283,6 +2289,22 @@ FixedwingPositionControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 
+			// handle estimator reset events. we only adjust setpoins for manual modes
+			if (_control_mode.flag_control_manual_enabled) {
+				if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
+					_hold_alt += _global_pos.delta_alt;
+				}
+
+				// adjust navigation waypoints in position control mode
+				if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
+					_hdg_hold_prev_wp.lat += _global_pos.delta_lat_lon[0];
+					_hdg_hold_prev_wp.lon += _global_pos.delta_lat_lon[1];
+					_hdg_hold_curr_wp.lat += _global_pos.delta_lat_lon[0];
+					_hdg_hold_curr_wp.lon += _global_pos.delta_lat_lon[1];
+					_pos_reset_counter = _global_pos.lat_lon_reset_counter;
+				}
+			}
+
 			// XXX add timestamp check
 			_global_pos_valid = true;
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -269,6 +269,14 @@ private:
 	float _takeoff_thrust_sp;
 	bool control_vel_enabled_prev;	/**< previous loop was in velocity controlled mode (control_state.flag_control_velocity_enabled) */
 
+	// counters for reset events on position and velocity states
+	// they are used to identify a reset event
+	uint8_t _z_reset_counter;
+	uint8_t _xy_reset_counter;
+	uint8_t _vz_reset_counter;
+	uint8_t _vxy_reset_counter;
+	uint8_t _heading_reset_counter;
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -407,7 +415,12 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_z_lp(0),
 	_acc_z_lp(0),
 	_takeoff_thrust_sp(0.0f),
-	control_vel_enabled_prev(false)
+	control_vel_enabled_prev(false),
+	_z_reset_counter(0),
+	_xy_reset_counter(0),
+	_vz_reset_counter(0),
+	_vxy_reset_counter(0),
+	_heading_reset_counter(0)
 {
 	// Make the quaternion valid for control state
 	_ctrl_state.q[0] = 1.0f;
@@ -660,6 +673,18 @@ MulticopterPositionControl::poll_subscriptions()
 		math::Vector<3> euler_angles;
 		euler_angles = _R.to_euler();
 		_yaw = euler_angles(2);
+
+		if (_control_mode.flag_control_manual_enabled) {
+			if (_heading_reset_counter != _ctrl_state.quat_reset_counter) {
+				_heading_reset_counter = _ctrl_state.quat_reset_counter;
+				math::Quaternion delta_q(_ctrl_state.delta_q_reset[0], _ctrl_state.delta_q_reset[1], _ctrl_state.delta_q_reset[2], _ctrl_state.delta_q_reset[3]);
+
+				// we only extract the heading change from the delta quaternion
+				math::Vector<3> delta_euler = delta_q.to_euler();
+				_att_sp.yaw_body += delta_euler(2);
+			}
+		}
+
 	}
 
 	orb_check(_att_sp_sub, &updated);
@@ -690,6 +715,37 @@ MulticopterPositionControl::poll_subscriptions()
 
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+
+		// check if a reset event has happened
+		// if the vehicle is in manual mode we will shift the setpoints of the
+		// states which were reset. In auto mode we do not shift the setpoints
+		// since we want the vehicle to track the original state.
+		if (_control_mode.flag_control_manual_enabled) {
+			if (_z_reset_counter != _local_pos.z_reset_counter) {
+				_pos_sp(2) += _local_pos.delta_z;
+				_z_reset_counter = _local_pos.z_reset_counter;
+			}
+
+			if (_xy_reset_counter != _local_pos.xy_reset_counter) {
+				_pos_sp(0) += _local_pos.delta_xy[0];
+				_pos_sp(1) += _local_pos.delta_xy[1];
+				_xy_reset_counter = _local_pos.xy_reset_counter;
+			}
+
+			if (_vz_reset_counter != _local_pos.vz_reset_counter) {
+				_vel_sp(2) += _local_pos.delta_vz;
+				_vel_sp_prev(2) +=  _local_pos.delta_vz;
+				_vz_reset_counter = _local_pos.vz_reset_counter;
+			}
+
+			if (_vxy_reset_counter != _local_pos.vxy_reset_counter) {
+				_vel_sp(0) += _local_pos.delta_vxy[0];
+				_vel_sp(1) += _local_pos.delta_vxy[1];
+				_vel_sp_prev(0) += _local_pos.delta_vxy[0];
+				_vel_sp_prev(1) += _local_pos.delta_vxy[1];
+				_vxy_reset_counter = _local_pos.vxy_reset_counter;
+			}
+		}
 	}
 }