feat(mc_att_control): feedforward filtered q_d derivative on rate sp

Eliminates the steady-state ramp-tracking lag of the proportional
attitude law for camera-pointing / LOS use cases.

src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp

@@ -41,6 +41,25 @@
 
 using namespace matrix;
 
+namespace
+{
+// Rotation vector taking world-z onto q's thrust direction (zero yaw content).
+Vector3f extractTiltState(const Quatf &q)
+{
+	const Vector3f e_z_d = q.dcm_z();
+	const Vector3f e_z_world(0.f, 0.f, 1.f);
+	const Vector3f tilt_axis = e_z_world.cross(e_z_d);
+	const float tilt_axis_norm = tilt_axis.norm();
+
+	if (tilt_axis_norm > FLT_EPSILON) {
+		const float tilt_angle = acosf(math::constrain(e_z_world.dot(e_z_d), -1.f, 1.f));
+		return (tilt_axis / tilt_axis_norm) * tilt_angle;
+	}
+
+	return Vector3f{};
+}
+} // namespace
+
 void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
 {
 	_proportional_gain = proportional_gain;
@@ -52,6 +71,28 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g
 	}
 }
 
+void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_setpoint, const float dt)
+{
+	Quatf qd_normalized = qd;
+	qd_normalized.normalize();
+
+	// Tilt-only into the model; yaw is handled by _yawspeed_setpoint in update().
+	// The model auto-resets on dt <= 0 or dt > max_step.
+	_tilt_filter.update(dt, extractTiltState(qd_normalized));
+
+	_attitude_setpoint_q = qd_normalized;
+	_yawspeed_setpoint = yawspeed_setpoint;
+}
+
+void AttitudeControl::adaptAttitudeSetpoint(const Quatf &q_delta)
+{
+	_attitude_setpoint_q = q_delta * _attitude_setpoint_q;
+	_attitude_setpoint_q.normalize();
+	// Re-extract tilt and reseed the model so any tilt component of the heading
+	// reset isn't interpreted as real motion on the next update.
+	_tilt_filter.reset(extractTiltState(_attitude_setpoint_q));
+}
+
 matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 {
 	Quatf qd = _attitude_setpoint_q;
@@ -94,15 +135,20 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 	// calculate angular rates setpoint
 	Vector3f rate_setpoint = eq.emult(_proportional_gain);
 
-	// Feed forward the yaw setpoint rate.
-	// 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 (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.
-	if (std::isfinite(_yawspeed_setpoint)) {
-		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
+	if (_ff_enabled) {
+		// Tilt FF from the reference model (world frame); rotate to body.
+		rate_setpoint += q.rotateVectorInverse(_tilt_filter.getRate());
+
+		// Feed forward the yaw setpoint rate.
+		// _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 (_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.
+		if (std::isfinite(_yawspeed_setpoint)) {
+			rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
+		}
 	}
 
 	// limit rates

src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp

@@ -50,6 +50,7 @@
 
 #include <matrix/matrix/math.hpp>
 #include <mathlib/math/Limits.hpp>
+#include <mathlib/math/filter/second_order_reference_model.hpp>
 
 class AttitudeControl
 {
@@ -57,6 +58,11 @@ public:
 	AttitudeControl() = default;
 	~AttitudeControl() = default;
 
+	// Natural frequency of the tilt feedforward reference model.
+	// Damping ratio is fixed at 1.0 (critical damping, monotonic settling).
+	// Placeholder value; revisit after flight test if needed.
+	static constexpr float kTiltFFNaturalFreq = 10.0f;
+
 	/**
 	 * Set proportional attitude control gain
 	 * @param proportional_gain 3D vector containing gains for roll, pitch, yaw
@@ -74,24 +80,22 @@ public:
 	 * 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
+	 * @param dt [s] time since previous setpoint; <= 0 skips the FF derivative update
 	 */
-	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint)
-	{
-		_attitude_setpoint_q = qd;
-		_attitude_setpoint_q.normalize();
-		_yawspeed_setpoint = yawspeed_setpoint;
-	}
+	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint, const float dt = -1.f);
 
 	/**
 	 * Adjust last known attitude setpoint by a delta rotation
 	 * Optional use to avoid glitches when attitude estimate reference e.g. heading changes.
 	 * @param q_delta delta rotation to apply
 	 */
-	void adaptAttitudeSetpoint(const matrix::Quatf &q_delta)
-	{
-		_attitude_setpoint_q = q_delta * _attitude_setpoint_q;
-		_attitude_setpoint_q.normalize();
-	}
+	void adaptAttitudeSetpoint(const matrix::Quatf &q_delta);
+
+	/**
+	 * Gate the setpoint-derivative feedforward (the filter keeps running, only
+	 * its addition to the rate setpoint is suppressed). Used during autotune.
+	 */
+	void setFeedForwardEnabled(bool enabled) { _ff_enabled = enabled; }
 
 	/**
 	 * Run one control loop cycle calculation
@@ -107,4 +111,7 @@ private:
 
 	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
+
+	math::SecondOrderReferenceModel<matrix::Vector3f> _tilt_filter{kTiltFFNaturalFreq, 1.0f};
+	bool _ff_enabled{true};
 };

src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -138,3 +138,117 @@ TEST(AttitudeControlTest, YawWeightScaling)
 	// THEN: no actuation (also no NAN)
 	EXPECT_EQ(rate_setpoint, Vector3f());
 }
+
+class AttitudeControlFeedforwardTest : public ::testing::Test
+{
+public:
+	AttitudeControlFeedforwardTest()
+	{
+		_attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, 2.8f), 0.4f);
+		_attitude_control.setRateLimit(Vector3f(10.f, 10.f, 10.f));
+	}
+
+	// Push a constant-rate ramp around the given body axis until the reference model is settled.
+	// First call uses dt < 0 to reset the model to the current sample (matches the wrapper's
+	// behaviour on the very first setpoint after boot).
+	Quatf rampSetpoint(const Vector3f &body_rate, float yawspeed_sp, int steps)
+	{
+		Quatf q_d;
+
+		for (int i = 0; i < steps; i++) {
+			q_d = q_d * Quatf(AxisAnglef(body_rate * kDt));
+			_attitude_control.setAttitudeSetpoint(q_d, yawspeed_sp, (i == 0) ? -1.f : kDt);
+		}
+
+		return q_d;
+	}
+
+	AttitudeControl _attitude_control;
+
+	static constexpr float kDt = 0.004f;                                    // 250 Hz setpoint rate
+	static constexpr float kOmegaN = AttitudeControl::kTiltFFNaturalFreq;   // 10 rad/s
+	static constexpr int kSettleSteps = 200;                                // ~ 8 / omega_n, well-settled
+};
+
+TEST_F(AttitudeControlFeedforwardTest, ConstantSetpointGivesNoFeedforward)
+{
+	// GIVEN: a constant tilted setpoint repeated with valid dt
+	const Quatf q_d(AxisAnglef(Vector3f(0.1f, 0.f, 0.f)));
+
+	for (int i = 0; i < kSettleSteps; i++) {
+		_attitude_control.setAttitudeSetpoint(q_d, 0.f, (i == 0) ? -1.f : kDt);
+	}
+
+	// WHEN: vehicle is at the setpoint (no error)
+	const Vector3f rate_setpoint = _attitude_control.update(q_d);
+
+	// THEN: rate setpoint is zero — non-moving SP gives a zero model rate output
+	EXPECT_NEAR(rate_setpoint.norm(), 0.f, 1e-3f);
+}
+
+TEST_F(AttitudeControlFeedforwardTest, RollRampProducesRollFeedforward)
+{
+	// GIVEN: a steady roll ramp, vehicle perfectly tracking
+	const float omega = 0.5f;
+	const Quatf q_d = rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps);
+
+	// WHEN: vehicle is at the setpoint (no error → P term = 0)
+	const Vector3f rate_setpoint = _attitude_control.update(q_d);
+
+	// THEN: the entire rate setpoint comes from the tilt FF and lies on the roll axis
+	EXPECT_NEAR(rate_setpoint(0), omega, 0.05f * omega);
+	EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
+	EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
+}
+
+TEST_F(AttitudeControlFeedforwardTest, PitchRampProducesPitchFeedforward)
+{
+	// GIVEN: a steady pitch ramp, vehicle perfectly tracking
+	const float omega = 0.5f;
+	const Quatf q_d = rampSetpoint(Vector3f(0.f, omega, 0.f), 0.f, kSettleSteps);
+
+	const Vector3f rate_setpoint = _attitude_control.update(q_d);
+
+	EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
+	EXPECT_NEAR(rate_setpoint(1), omega, 0.05f * omega);
+	EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
+}
+
+TEST_F(AttitudeControlFeedforwardTest, YawRampOnlyAnalyticalFeedforwardContributes)
+{
+	// GIVEN: a yaw ramp with the analytical yawspeed FF set to the same rate.
+	// Yaw rotation does not change the desired thrust direction, so the tilt
+	// reference model sees a constant input and produces zero output. The yaw FF
+	// thus comes solely from the analytical yaw_sp_move_rate path — no projection
+	// or double-counting needed because yaw never enters the model.
+	const float omega = 0.5f;
+	const Quatf q_d = rampSetpoint(Vector3f(0.f, 0.f, omega), omega, kSettleSteps);
+
+	const Vector3f rate_setpoint = _attitude_control.update(q_d);
+
+	EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
+	EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
+	EXPECT_NEAR(rate_setpoint(2), omega, 0.05f * omega);
+}
+
+TEST_F(AttitudeControlFeedforwardTest, FeedForwardDisabledSuppressesContribution)
+{
+	// GIVEN: a settled roll-ramp FF
+	const float omega = 0.5f;
+	const Quatf q_d = rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps);
+
+	// WHEN: FF is disabled (e.g. autotune active)
+	_attitude_control.setFeedForwardEnabled(false);
+	Vector3f rate_setpoint = _attitude_control.update(q_d);
+
+	// THEN: no FF applied even though the model still holds the ramp rate
+	EXPECT_NEAR(rate_setpoint.norm(), 0.f, 1e-3f);
+
+	// AND WHEN: re-enabled, the FF returns immediately (model state preserved)
+	_attitude_control.setFeedForwardEnabled(true);
+	rate_setpoint = _attitude_control.update(q_d);
+
+	EXPECT_NEAR(rate_setpoint(0), omega, 0.05f * omega);
+	EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
+	EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
+}

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -315,7 +315,11 @@ MulticopterAttitudeControl::Run()
 				if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
 				    && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
 
-					_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
+					const float setpoint_dt = (_last_attitude_setpoint > 0)
+								  ? (vehicle_attitude_setpoint.timestamp - _last_attitude_setpoint) * 1e-6f
+								  : -1.f;
+					_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d),
+									      vehicle_attitude_setpoint.yaw_sp_move_rate, setpoint_dt);
 					_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
 					_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
 				}
@@ -341,19 +345,23 @@ MulticopterAttitudeControl::Run()
 				_quat_reset_counter = v_att.quat_reset_counter;
 			}
 
-			Vector3f rates_sp = _attitude_control.update(q);
-
+			// While autotune is identifying, suppress the FF and add its injected rate.
 			const hrt_abstime now = hrt_absolute_time();
 			autotune_attitude_control_status_s pid_autotune;
+			const bool autotune_status_fresh = _autotune_attitude_control_status_sub.copy(&pid_autotune)
+							   && ((now - pid_autotune.timestamp) < 1_s);
+			const bool autotune_active = autotune_status_fresh
+						     && (pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
+							 || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
+							 || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
+							 || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST);
 
-			if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
-				if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
-				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
-				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
-				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
-				    && ((now - pid_autotune.timestamp) < 1_s)) {
-					rates_sp += Vector3f(pid_autotune.rate_sp);
-				}
+			_attitude_control.setFeedForwardEnabled(!autotune_active);
+
+			Vector3f rates_sp = _attitude_control.update(q);
+
+			if (autotune_active) {
+				rates_sp += Vector3f(pid_autotune.rate_sp);
 			}
 
 			// publish rate setpoint