feat(mc_att_control): expose ref-model bandwidth and FF gain as parameters

Replace the hard-coded ref-model coefficients with two runtime parameters
and raise the default bandwidth based on SITL sweep data:

  MC_REF_W_N  [rad/s, default 100]
    Natural frequency of the 2nd-order critically-damped reference model.
    Was hardcoded to 10 rad/s, a conservative value pinned by the previous
    symplectic-Euler stability bound. The exact-discretisation introduced
    in the previous commit lifts that constraint; SITL maneuver sweeps
    show omega_n = 100 reduces small-step overshoot to zero, halves the
    AUTO_LAND-style snap-back undershoot, and improves ramp tracking
    by 2-3x with no measurable downside on the rate-loop side.

  MC_REF_FF  [0..1, default 1.0]
    Magnitude scaling for the angular-velocity feed-forward contribution
    to the rate setpoint. 1 = full FF (default, prior PR behaviour),
    0 = FF off (attitude P-loop targets q_d directly, legacy behaviour).
    Lets operators disable or attenuate the FF on airframes whose rate
    loop is not tuned to handle the FF demand, without recompiling.

Signed-off-by: gguidone <gennaroguido2002@gmail.com>

src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp

@@ -52,6 +52,13 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g
 	}
 }
 
+void AttitudeControl::setRefModelFrequency(float omega_n)
+{
+	_omega_n = math::max(omega_n, 0.1f);
+	_kq      = _omega_n * _omega_n;
+	_komega  = 2.f * _omega_n;
+}
+
 void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_setpoint, const float dt)
 {
 	Quatf qd_normalized = qd;
@@ -60,9 +67,9 @@ void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_
 	if (_ref_initialized && dt > 0.f) {
 		// Exact discretisation of the linearised 2nd-order critically damped
 		// ref model in body-frame coords:
-		//   d/dt[e; w] = [0 -1; kKq  -kKomega] [e; w] + [0; kKomega] w_known
+		//   d/dt[e; w] = [0 -1; w_n^2  -2 w_n] [e; w] + [0; 2 w_n] w_known
 		// Closed-form one-step update is unconditionally stable for any dt:
-		// the symplectic-Euler scheme requires kFFNaturalFreq*dt < 2 and blows
+		// the symplectic-Euler scheme requires _omega_n * dt < 2 and blows
 		// up when the setpoint stream pauses across a mode handoff (e.g. the
 		// ~500 ms gap PX4 produces when OFFBOARD hands off to AUTO_LAND).
 		const Vector3f w_known_in_ref = std::isfinite(yawspeed_setpoint)
@@ -73,12 +80,12 @@ void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_
 		q_err.canonicalize();
 		const Vector3f e = 2.f * q_err.imag();
 
-		// Discretisation coefficients (repeated eigenvalue at s = -kFFNaturalFreq).
-		const float w_dt   = kFFNaturalFreq * dt;
+		// Discretisation coefficients (repeated eigenvalue at s = -_omega_n).
+		const float w_dt   = _omega_n * dt;
 		const float emt    = expf(-w_dt);
 		const float a      = (1.f + w_dt) * emt;
 		const float b_neg  = dt * emt;
-		const float gamma  = kKq * dt * emt;
+		const float gamma  = _kq * dt * emt;
 		const float delta  = (1.f - w_dt) * emt;
 
 		const Vector3f w_offset     = _omega_ref - w_known_in_ref;
@@ -112,9 +119,10 @@ void AttitudeControl::adaptAttitudeSetpoint(const Quatf &q_delta)
 
 matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 {
-	// FF off (e.g. autotune): P targets the raw setpoint (legacy behavior).
-	// FF on: P targets the reference model's smoothed trajectory.
-	Quatf qd = _ff_enabled ? _q_ref : _attitude_setpoint_q;
+	// FF off (e.g. autotune, or MC_REF_FF=0): P targets the raw setpoint
+	// (legacy behaviour). FF on: P targets the ref model's smoothed trajectory.
+	const bool ff_active = _ff_enabled && (_ff_gain > 0.f);
+	Quatf qd = ff_active ? _q_ref : _attitude_setpoint_q;
 
 	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
 	const Vector3f e_z = q.dcm_z();
@@ -154,10 +162,11 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 	// calculate angular rates setpoint
 	Vector3f rate_setpoint = eq.emult(_proportional_gain);
 
-	if (_ff_enabled) {
-		// Feed forward the reference model's angular velocity. _omega_ref lives in
-		// q_ref's body frame; go through world to land in the current body frame.
-		rate_setpoint += q.rotateVectorInverse(_q_ref.rotateVector(_omega_ref));
+	if (ff_active) {
+		// Feed forward the reference model's angular velocity, scaled by _ff_gain.
+		// _omega_ref lives in q_ref's body frame; go through world to land in the
+		// current body frame.
+		rate_setpoint += _ff_gain * q.rotateVectorInverse(_q_ref.rotateVector(_omega_ref));
 	}
 
 	// limit rates

src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp

@@ -57,12 +57,12 @@ public:
 	AttitudeControl() = default;
 	~AttitudeControl() = default;
 
-	// Natural frequency of the attitude reference model (rad/s).
+	// Default natural frequency of the attitude reference model (rad/s).
 	// Damping ratio is fixed at 1.0 (critical damping, monotonic settling).
-	// Placeholder value; revisit after flight test if needed.
-	static constexpr float kFFNaturalFreq = 10.0f;
-	static constexpr float kKq            = kFFNaturalFreq * kFFNaturalFreq; ///< spring constant
-	static constexpr float kKomega        = 2.0f * kFFNaturalFreq;           ///< damping coeff (ζ=1)
+	// Runtime-tunable via setRefModelFrequency() / MC_REF_W_N.
+	static constexpr float kFFNaturalFreqDefault = 100.0f;
+	// Default feed-forward gain (0..1). 1 = full FF, 0 = FF off (legacy P-only).
+	static constexpr float kFFGainDefault = 1.0f;
 
 	/**
 	 * Set proportional attitude control gain
@@ -71,6 +71,23 @@ public:
 	 */
 	void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight);
 
+	/**
+	 * Set the reference-model natural frequency [rad/s]. The 2nd-order critically
+	 * damped (zeta=1) ref-model coefficients are recomputed accordingly.
+	 */
+	void setRefModelFrequency(float omega_n);
+
+	float getRefModelFrequency() const { return _omega_n; }
+
+	/**
+	 * Set the feed-forward magnitude scaling [0..1]. 0 disables the FF (the
+	 * proportional attitude law then targets q_d directly, like the pre-FF
+	 * behaviour); 1 is full FF.
+	 */
+	void setFeedForwardGain(float gain) { _ff_gain = math::constrain(gain, 0.f, 1.f); }
+
+	float getFeedForwardGain() const { return _ff_gain; }
+
 	/**
 	 * Set hard limit for output rate setpoints
 	 * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw
@@ -127,4 +144,12 @@ private:
 	bool _ref_initialized{false};          ///< false until first valid setpoint received
 
 	bool _ff_enabled{true};
+
+	// Reference-model coefficients. Runtime-tunable via setRefModelFrequency().
+	float _omega_n{kFFNaturalFreqDefault};
+	float _kq{kFFNaturalFreqDefault * kFFNaturalFreqDefault};
+	float _komega{2.f * kFFNaturalFreqDefault};
+
+	// FF magnitude scaling. 0..1; 0 = off, 1 = full.
+	float _ff_gain{kFFGainDefault};
 };

src/modules/mc_att_control/mc_att_control.hpp

@@ -161,6 +161,9 @@ private:
 		(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
 		(ParamFloat<px4::params::MC_YAWRATE_MAX>)   _param_mc_yawrate_max,
 
+		(ParamFloat<px4::params::MC_REF_W_N>)       _param_mc_ref_w_n,
+		(ParamFloat<px4::params::MC_REF_FF>)        _param_mc_ref_ff,
+
 		/* Stabilized mode params */
 		(ParamFloat<px4::params::MAN_DEADZONE>) _param_man_deadzone,
 		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max,

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -100,6 +100,9 @@ MulticopterAttitudeControl::parameters_updated()
 	_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()),
 						radians(_param_mc_yawrate_max.get())));
 
+	_attitude_control.setRefModelFrequency(_param_mc_ref_w_n.get());
+	_attitude_control.setFeedForwardGain(_param_mc_ref_ff.get());
+
 	// Update from hover thrust parameter if there's no valid estimate in use
 	if (!PX4_ISFINITE(_hover_thrust_estimate)) {
 		_hover_thrust_slew_rate.setForcedValue(_param_mpc_thr_hover.get());

src/modules/mc_att_control/mc_att_control_params.yaml

@@ -95,6 +95,40 @@ parameters:
       max: 1800.0
       decimal: 1
       increment: 5
+    MC_REF_W_N:
+      description:
+        short: Attitude reference-model natural frequency
+        long: |-
+          Natural frequency of the 2nd-order critically-damped reference model
+          that smooths the attitude setpoint and produces the feed-forward
+          angular rate. Higher values track the raw setpoint more aggressively
+          (less lag, smaller steady-state ramp tracking error) but demand more
+          peak rate from the inner rate loop. The closed-form discretisation
+          makes any value here unconditionally stable; the practical upper
+          limit is set by the rate-loop bandwidth.
+      type: float
+      default: 100.0
+      unit: rad/s
+      min: 1.0
+      max: 200.0
+      decimal: 1
+      increment: 5
+    MC_REF_FF:
+      description:
+        short: Attitude reference-model feed-forward gain
+        long: |-
+          Scaling applied to the ref-model's angular-velocity feed-forward
+          contribution to the rate setpoint. 1 = full FF (default), 0 = FF off
+          (the attitude P-loop targets q_d directly, legacy pre-FF behaviour).
+          Intermediate values blend.
+
+          Set to 0 if the inner rate loop is not tuned to handle the FF demand.
+      type: float
+      default: 1.0
+      min: 0.0
+      max: 1.0
+      decimal: 2
+      increment: 0.1
 - group: Multicopter Position Control
   definitions:
     MC_MAN_TILT_TAU: