mc_attitude_control: keep last attitude setpoint as member

The last attitude setpoint that is known from the position controller
is now kept inside the AttitudeControl class such that we don't
keep the whole vehicle_attitude_setpoint struct and always copy over from
there to run an update step.

src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp

@@ -52,8 +52,10 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g
 	}
 }
 
-matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, const float yawspeed_feedforward)
+matrix::Vector3f AttitudeControl::update(matrix::Quatf q) const
 {
+	Quatf qd = _attitude_setpoint_q;
+
 	// ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
 	q.normalize();
 	qd.normalize();
@@ -93,13 +95,13 @@ matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, cons
 	matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);
 
 	// Feed forward the yaw setpoint rate.
-	// yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
+	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
 	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
 	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
-	// and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
+	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
 	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
 	// such that it can be added to the rates setpoint.
-	rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;
+	rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
 
 	// limit rates
 	for (int i = 0; i < 3; i++) {

src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp

@@ -70,17 +70,25 @@ public:
 	 */
 	void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }
 
+	/**
+	 * Set a new attitude setpoint replacing the one tracked before
+	 * @param qd desired vehicle attitude setpoint
+	 * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame
+	 */
+	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; }
+
 	/**
 	 * Run one control loop cycle calculation
 	 * @param q estimation of the current vehicle attitude unit quaternion
-	 * @param qd desired vehicle attitude setpoint
-	 * @param yawspeed_feedforward [rad/s] yaw feed forward angular rate in world frame
 	 * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller
 	 */
-	matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward);
+	matrix::Vector3f update(matrix::Quatf q) const;
 
 private:
 	matrix::Vector3f _proportional_gain;
 	matrix::Vector3f _rate_limit;
-	float _yaw_w{0.f}; /**< yaw weight [0,1] to prioritize roll and pitch */
+	float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch
+
+	matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control
+	float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint
 };

src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -40,7 +40,7 @@ using namespace matrix;
 TEST(AttitudeControlTest, AllZeroCase)
 {
 	AttitudeControl attitude_control;
-	Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(), 0.f);
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
 	EXPECT_EQ(rate_setpoint, Vector3f());
 }
 
@@ -58,9 +58,11 @@ public:
 		int i; // need function scope to check how many steps
 		Vector3f rate_setpoint(1000.f, 1000.f, 1000.f);
 
+		_attitude_control.setAttitudeSetpoint(_quat_goal, 0.f);
+
 		for (i = 100; i > 0; i--) {
 			// run attitude control to get rate setpoints
-			const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f);
+			const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state);
 			// rotate the simulated state quaternion according to the rate setpoint
 			_quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new));
 			_quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem
@@ -112,25 +114,27 @@ TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
 
 TEST(AttitudeControlTest, YawWeightScaling)
 {
-	// GIVEN: default tuning
+	// GIVEN: default tuning and pure yaw turn command
 	AttitudeControl attitude_control;
 	const float yaw_gain = 2.8f;
 	const float yaw_sp = .1f;
+	Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f));
 	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f);
 	attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f));
+	attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f);
 
-	// WHEN: we command a pure yaw turn
-	Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
+	// WHEN: we run one iteration of the controller
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
 
 	// THEN: no actuation in roll, pitch
 	EXPECT_EQ(Vector2f(rate_setpoint), Vector2f());
 	// THEN: actuation error * gain in yaw
 	EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f);
 
-	// GIVEN: corner case of zero yaw weight
+	// GIVEN: additional corner case of zero yaw weight
 	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f);
-	// WHEN: we command a pure yaw turn
-	rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
+	// WHEN: we run one iteration of the controller
+	rate_setpoint = attitude_control.update(Quatf());
 	// THEN: no actuation (also no NAN)
 	EXPECT_EQ(rate_setpoint, Vector3f());
 }

src/modules/mc_att_control/mc_att_control.hpp

@@ -105,7 +105,7 @@ private:
 
 	AttitudeControl _attitude_control; ///< class for attitude control calculations
 
-	uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};		/**< vehicle attitude setpoint subscription */
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint subscription */
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
@@ -119,7 +119,6 @@ private:
 	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
 
 	struct vehicle_attitude_s		_v_att {};		/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_v_att_sp {};		/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s		_v_rates_sp {};		/**< vehicle rates setpoint */
 	struct manual_control_setpoint_s	_manual_control_sp {};	/**< manual control setpoint */
 	struct vehicle_control_mode_s		_v_control_mode {};	/**< vehicle control mode */
@@ -128,7 +127,8 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
-	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
+	matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
+	matrix::Vector3f _rates_sp; ///< angular rates setpoint
 
 	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
 	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -69,7 +69,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
 
 	/* initialize quaternions in messages to be valid */
 	_v_att.q[0] = 1.f;
-	_v_att_sp.q_d[0] = 1.f;
 
 	parameters_updated();
 }
@@ -234,8 +233,14 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 void
 MulticopterAttitudeControl::control_attitude()
 {
-	_v_att_sp_sub.update(&_v_att_sp);
-	_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
+	if (_vehicle_attitude_setpoint_sub.updated()) {
+		vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+		_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
+		_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
+		_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
+	}
+
+	_rates_sp = _attitude_control.update(Quatf(_v_att.q));
 }
 
 void
@@ -246,9 +251,7 @@ MulticopterAttitudeControl::publish_rates_setpoint()
 	v_rates_sp.roll = _rates_sp(0);
 	v_rates_sp.pitch = _rates_sp(1);
 	v_rates_sp.yaw = _rates_sp(2);
-	v_rates_sp.thrust_body[0] = _v_att_sp.thrust_body[0];
-	v_rates_sp.thrust_body[1] = _v_att_sp.thrust_body[1];
-	v_rates_sp.thrust_body[2] = _v_att_sp.thrust_body[2];
+	_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
 	v_rates_sp.timestamp = hrt_absolute_time();
 
 	_v_rates_sp_pub.publish(v_rates_sp);