Merge pull request #886 from PX4/smooth_pos_hold

Smooth transitions to stabilized modes

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -105,6 +105,7 @@ public:
 	int		start();
 
 private:
+	const float alt_ctl_dz = 0.2f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
@@ -184,6 +185,8 @@ private:
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
 
 	/**
 	 * Update our local parameter cache.
@@ -216,6 +219,16 @@ private:
 	 */
 	void		reset_alt_sp();
 
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
@@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.z_vel_max, &v);
 		_params.vel_max(2) = v;
 		param_get(_params_handles.xy_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(0) = v;
 		_params.vel_ff(1) = v;
 		param_get(_params_handles.z_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(2) = v;
 
 		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
 {
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
-		_pos_sp(0) = _pos(0);
-		_pos_sp(1) = _pos(1);
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
@@ -508,11 +528,125 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_pos_sp(2) = _pos(2);
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
 	}
 }
 
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual.x;
+		_sp_move_rate(1) = _manual.y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
+	math::Matrix<3, 3> R_yaw_sp;
+	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	bool updated;
+	orb_check(_pos_sp_triplet_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+	}
+
+	if (_pos_sp_triplet.current.valid) {
+		if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet.current.x;
+			_pos_sp(1) = _pos_sp_triplet.current.y;
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
+		}
+
+		if (_control_mode.flag_control_altitude_enabled) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet.current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -551,13 +685,6 @@ MulticopterPositionControl::task_main()
 
 	hrt_abstime t_prev = 0;
 
-	const float alt_ctl_dz = 0.2f;
-
-	math::Vector<3> sp_move_rate;
-	sp_move_rate.zero();
-
-	float yaw_sp_move_rate;
-
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 	math::Matrix<3, 3> R;
@@ -616,138 +743,17 @@ MulticopterPositionControl::task_main()
 			_vel(1) = _local_pos.vy;
 			_vel(2) = _local_pos.vz;
 
-			sp_move_rate.zero();
+			_vel_ff.zero();
+			_sp_move_rate.zero();
 
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
-				if (_control_mode.flag_control_altitude_enabled) {
-					/* reset alt setpoint to current altitude if needed */
-					reset_alt_sp();
-
-					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
-				}
-
-				if (_control_mode.flag_control_position_enabled) {
-					/* reset position setpoint to current position if needed */
-					reset_pos_sp();
-
-					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.x;
-					sp_move_rate(1) = _manual.y;
-				}
-
-				/* limit setpoint move rate */
-				float sp_move_norm = sp_move_rate.length();
-
-				if (sp_move_norm > 1.0f) {
-					sp_move_rate /= sp_move_norm;
-				}
-
-				/* scale to max speed and rotate around yaw */
-				math::Matrix<3, 3> R_yaw_sp;
-				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-				/* move position setpoint */
-				_pos_sp += sp_move_rate * dt;
-
-				/* check if position setpoint is too far from actual position */
-				math::Vector<3> pos_sp_offs;
-				pos_sp_offs.zero();
-
-				if (_control_mode.flag_control_position_enabled) {
-					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-				}
-
-				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-				}
-
-				float pos_sp_offs_norm = pos_sp_offs.length();
-
-				if (pos_sp_offs_norm > 1.0f) {
-					pos_sp_offs /= pos_sp_offs_norm;
-					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-				}
+				control_manual(dt);
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
-				/* Offboard control */
-				bool updated;
-				orb_check(_pos_sp_triplet_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-				}
-
-				if (_pos_sp_triplet.current.valid) {
-
-					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
-
-						_pos_sp(0) = _pos_sp_triplet.current.x;
-						_pos_sp(1) = _pos_sp_triplet.current.y;
-						_pos_sp(2) = _pos_sp_triplet.current.z;
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-
-					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
-						/* reset position setpoint to current position if needed */
-						reset_pos_sp();
-						/* move position setpoint with roll/pitch stick */
-						sp_move_rate(0) = _pos_sp_triplet.current.vx;
-						sp_move_rate(1) = _pos_sp_triplet.current.vy;
-						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
-						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						/* reset alt setpoint to current altitude if needed */
-						reset_alt_sp();
-
-						/* move altitude setpoint with throttle stick */
-						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
-					}
-
-					/* limit setpoint move rate */
-					float sp_move_norm = sp_move_rate.length();
-
-					if (sp_move_norm > 1.0f) {
-						sp_move_rate /= sp_move_norm;
-					}
-
-					/* scale to max speed and rotate around yaw */
-					math::Matrix<3, 3> R_yaw_sp;
-					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-					/* move position setpoint */
-					_pos_sp += sp_move_rate * dt;
-
-					/* check if position setpoint is too far from actual position */
-					math::Vector<3> pos_sp_offs;
-					pos_sp_offs.zero();
-
-					if (_control_mode.flag_control_position_enabled) {
-						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-					}
-
-					float pos_sp_offs_norm = pos_sp_offs.length();
-
-					if (pos_sp_offs_norm > 1.0f) {
-						pos_sp_offs /= pos_sp_offs_norm;
-						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-					}
-
-				} else {
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				/* offboard control */
+				control_offboard(dt);
 
 			} else {
 				/* AUTO */
@@ -821,7 +827,7 @@ MulticopterPositionControl::task_main()
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err = _pos_sp - _pos;
 
-				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
 
 				if (!_control_mode.flag_control_altitude_enabled) {
 					_reset_alt_sp = true;
@@ -901,7 +907,7 @@ MulticopterPositionControl::task_main()
 					math::Vector<3> vel_err = _vel_sp - _vel;
 
 					/* derivative of velocity error, not includes setpoint acceleration */
-					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
 					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */

src/modules/uORB/topics/position_setpoint_triplet.h

@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
 };
 
 struct position_setpoint_s
@@ -71,9 +71,9 @@ struct position_setpoint_s
 	float y;			/**< local position setpoint in m in NED */
 	float z;			/**< local position setpoint in m in NED */
 	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m in NED */
-	float vy;			/**< local velocity setpoint in m in NED */
-	float vz;			/**< local velocity setpoint in m in NED */
+	float vx;			/**< local velocity setpoint in m/s in NED */
+	float vy;			/**< local velocity setpoint in m/s in NED */
+	float vz;			/**< local velocity setpoint in m/s in NED */
 	bool velocity_valid;		/**< true if local velocity setpoint valid */
 	double lat;			/**< latitude, in deg */
 	double lon;			/**< longitude, in deg */