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

Add a 2nd-order critically-damped attitude reference model whose angular-velocity output is fed forward onto the rate setpoint, removing the steady-state ramp-tracking lag of the pure-P attitude law. The model uses an exact closed-form discretisation, so it is unconditionally stable.

Parameters:
- MC_REF_W_N     reference-model bandwidth [rad/s]
- MC_REF_FF      feedforward gain [0..1]; 0 disables the anticipation
- MC_REF_FF_MAX  per-axis feedforward saturation [deg/s]

The commanded yaw rate is always fed forward at unity (it is a setpoint, not a model prediction), so MC_REF_FF scales only the error-driven anticipation and MC_REF_FF=0 is the exact legacy pure-P law. Shipped disabled by default to avoid changing behaviour on existing platforms; enable per-airframe. Includes docs and the updated controller diagram.
This commit is contained in:
Gennaro Guidone authored and GitHub committed 2026-06-26 10:57:48 +02:00
1 parent 171f0f38cf
commit 2a0e048171
9 files changed
+479 -36

No files matched your search

Binary file not shown.

Before

Width:  |  Height:  |  Size: 19 KiB

After

Width:  |  Height:  |  Size: 110 KiB

+24 -7
View File
@@ -248,29 +248,46 @@
\hspace{-0.7cm}
%===============================================================================
\node[text centered, yshift=0.25*\blockheight] (ref) {};
\node[text centered] (ref) {};
% Reference model: 2nd-order critically-damped model (natural frequency MC_REF_W_N)
% that smooths the setpoint q_sp into the reference attitude q_ref tracked by the
% P law, and outputs the reference body rate omega_ref used as feedforward.
\path(ref)+(0.9*\nodesep,0) node (ref_model) [text_block] {Reference model};
% Controller
\path(ref)+(0.75*\nodesep,-0.25*\blockheight) node (err_quat) [text_block] {Error quaternion};
\path(ref_model)+(1.5*\nodesep,0) node (err_quat) [text_block] {Error quaternion};
\path(err_quat)+(1.5*\nodesep,0) node (select_axis) [text_block] {Extract component};
\path(select_axis)+(0, 1.5*\blockheight) node (select_mag) [text_block] {Extract magnitude};
\path(select_mag)+(1.1*\nodesep,0) node (sgn) [simple_block] {sgn};
\path(select_axis)+(1.9*\nodesep, 0.25*\blockheight) node (mult) [simple_block] {$\times$};
\path(mult)+(0.65*\nodesep, 0) node (p_gain) [gain] {\scriptsize $2 P$};
\path(p_gain)+(0.75*\nodesep, 0) node (sat_ctrl) [sat_block] {};
\path(sat_ctrl)+(0.75*\nodesep, 0) node (output) [] {};
% Summing junction: P-law rate + reference-model rate feedforward, BEFORE the final rate saturation
\path(p_gain)+(0.7*\nodesep, 0) node (sum) [draw, circle, inner sep=1.5pt] {$+$};
\path(sum)+(0.7*\nodesep, 0) node (sat_ctrl) [sat_block] {};
\path(sat_ctrl)+(0.7*\nodesep, 0) node (output) [] {};
% omega_ref feedforward chain: gain (MC_REF_FF) then per-axis saturation (MC_REF_FF_MAX)
\path(sum)+(-0.7*\nodesep, -2.7*\blockheight) node (ff_sat) [sat_block] {};
\path(ff_sat)+(-1.1*\nodesep, 0) node (ff_gain) [gain] {\scriptsize $k_\text{ff}$};
%===============================================================================
\path[draw,->] (ref.east) -- node[text centered, anchor=south, pos=0.1] {$\bm{q}_\text{sp}$} ([yshift=0.25*\blockheight]err_quat.west);
\path[draw,->] ([yshift=-0.25*\blockheight]err_quat.west -| ref.east) -- node[text centered, anchor=north, pos=0.1] {$\bm{q}$} ([yshift=-0.25*\blockheight]err_quat.west);
\path[draw,->] (ref.east) -- node[text centered, anchor=south, pos=0.3] {$\bm{q}_\text{sp}$} (ref_model.west);
\path[draw,->] (ref_model.east) -- node[text centered, anchor=south] {$\bm{q}_\text{ref}$} (err_quat.west);
\path[draw,->] ([yshift=-0.75*\blockheight]err_quat.south) -- node[text centered, anchor=east, pos=0] {$\bm{q}$} (err_quat.south);
\path[draw,->] (err_quat.east) -- node[text centered, anchor=south,pos=0.4] {$\bm{q}_\text{e}$} (select_axis.west);
\path[draw,->]([xshift=0.3*\nodesep]err_quat.east) |- (select_mag.west);
\path[draw,->] (select_mag.east) -- node[text centered, anchor=south] {$q_{0_\text{e}}$} (sgn.west);
\path[draw,->] (sgn.east) -- ([xshift=0.15*\nodesep]sgn.east) |- ([yshift=0.25*\blockheight]mult.west);
\path[draw,->] (select_axis.east) -- node[text centered, anchor=south, pos=0.45]{$q_{j_\text{e}}$} ([yshift=-0.25*\blockheight]mult.west);
\path[draw,->] (mult.east) -- (p_gain.west);
\path[draw,->] (p_gain.east) -- (sat_ctrl.west);
\path[draw,->] (p_gain.east) -- (sum.west);
\path[draw,->] (sum.east) -- (sat_ctrl.west);
\path[draw,->] (sat_ctrl.east) -- node[text centered, anchor=south] {$\Omega_\text{sp}$} (output.west);
% omega_ref feedforward: reference model -> gain -> saturation -> summing junction
\path[draw,->] (ref_model.south) |- (ff_gain.west);
\node[text centered, anchor=south] at (select_axis |- ff_gain) {$\bm{\omega}_\text{ref}$};
\path[draw,->] (ff_gain.east) -- (ff_sat.west);
\path[draw,->] (ff_sat.east) -| (sum.south);
\end{tikzpicture}
\end{figure}
+3 -1
View File
@@ -43,7 +43,9 @@ The diagrams use the standard [PX4 notation](../contribute/notation.md) (and eac
- The attitude controller makes use of [quaternions](https://en.wikipedia.org/wiki/Quaternion).
- The controller is implemented from this [article](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf).
- When tuning this controller, the only parameter of concern is the P gain.
- The attitude setpoint is first smoothed by a 2nd-order critically-damped reference model ([MC_REF_W_N](../advanced_config/parameter_reference#MC_REF_W_N)); the P-law tracks its reference attitude, and the model's reference body rate is fed forward to the rate setpoint, removing the pure-P law's steady-state tracking lag.
- The feedforward is scaled by `MC_REF_FF` (`0` disables it), clipped per axis by `MC_REF_FF_MAX`, and suppressed during autotuning.
- Higher `MC_REF_W_N` or `MC_REF_FF` tracks more aggressively but demands more peak rate; the P gain remains the main tuning parameter.
- The rate command is saturated.
### Multicopter Acceleration to Thrust and Attitude Setpoint Conversion
@@ -41,6 +41,10 @@
using namespace matrix;
static __attribute__((noinline)) Quatf qmul(const Quatf &a, const Quatf &b) { return a * b; }
static __attribute__((noinline)) Quatf qinv(const Quatf &q) { return q.inversed(); }
static __attribute__((noinline)) Vector3f qzaxis(const Quatf &q) { return q.dcm_z(); }
void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
{
_proportional_gain = proportional_gain;
@@ -52,13 +56,94 @@ 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;
}
void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_setpoint, const float dt)
{
Quatf qd_normalized = qd;
qd_normalized.normalize();
if (_ref_initialized && dt > 0.f) {
propagateReferenceModel(qd_normalized, yawspeed_setpoint, dt);
} else {
// First call (or dt out of range): snap reference to the current setpoint.
_q_ref = qd_normalized;
_omega_correction.zero();
_omega_command.zero();
_ref_initialized = true;
}
}
void AttitudeControl::propagateReferenceModel(const Quatf &qd, const float yawspeed_setpoint, const float dt)
{
// 2nd-order critically damped ref model with exact (ZOH) discretisation.
// Repeated eigenvalue at s = -_omega_n; unconditionally stable for any dt.
// Tangent-space inputs: rotate the analytical yaw rate into q_ref's body
// frame, and form the small-angle error vector from q_ref to q_d.
const Quatf q_ref_inv = qinv(_q_ref);
const Vector3f yaw_axis_body = qzaxis(q_ref_inv); // world yaw axis expressed in q_ref's body frame
const Vector3f omega_command = PX4_ISFINITE(yawspeed_setpoint)
? yaw_axis_body * yawspeed_setpoint
: Vector3f{};
Quatf q_err = qmul(q_ref_inv, qd);
q_err.canonicalize();
const Vector3f e = 2.f * q_err.imag();
// Entries of exp(A*dt) for A = [0 -1; _kq -2*_omega_n]. A has the repeated eigenvalue lambda = -_omega_n.
// The matrix N = A - lambda*I is then nilpotent (a matrix is nilpotent when
// N^k = 0 for some k, and here N^2 = 0). Writing exp(A*dt) = e^(lambda*dt) * exp(N*dt) and expanding the
// series for exp(N*dt) = I + N*dt + (N*dt)^2/2! + ... , every term from (N*dt)^2
// onward vanishes, so the exponential truncates to the exact closed form
// exp(A*dt) = e^(-_omega_n*dt) * [ (1 + _omega_n*dt) I + dt*A ]
// = emt * [ a -b ; gamma delta ].
const float w_dt = _omega_n * dt;
const float emt = expf(-w_dt);
const float a = (1.f + w_dt) * emt;
const float b = dt * emt;
const float gamma = _kq * dt * emt;
const float delta = (1.f - w_dt) * emt;
// Propagate the error-driven correction in tangent space (the 2nd-order state). delta_phi is the integral
// of omega over [0, dt]; the correction part collapses to e(0) - e(dt) since e_dot = -correction.
const Vector3f delta_phi = (1.f - a) * e + b * _omega_correction + omega_command * dt;
_omega_correction = gamma * e + delta * _omega_correction;
// Yaw-rate command: the heading setpoint just follows the measured yaw, so feeding the error-driven
// rate forward closes a positive-feedback loop. Keep only the commanded rate (omega_command) on the yaw axis.
if (PX4_ISFINITE(yawspeed_setpoint) && (fabsf(yawspeed_setpoint) > FLT_EPSILON)) {
_omega_correction -= _omega_correction.dot(yaw_axis_body) * yaw_axis_body;
}
// Commanded (analytical) reference rate, kept separate so update() can exempt it from the feedforward limit.
_omega_command = omega_command;
_q_ref = qmul(_q_ref, Quatf(AxisAnglef(delta_phi)));
_q_ref.normalize();
}
void AttitudeControl::adaptAttitudeSetpoint(const Quatf &q_delta)
{
// Apply the world-frame delta to the reference attitude. _omega_correction and _omega_command are
// in the reference body frame and physically invariant under a world relabeling.
_q_ref = qmul(q_delta, _q_ref);
_q_ref.normalize();
}
matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
Quatf qd = _attitude_setpoint_q;
// The P controller always tracks the reference-model attitude.
Quatf qd = _q_ref;
// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
const Vector3f e_z = q.dcm_z();
const Vector3f e_z_d = qd.dcm_z();
const Vector3f e_z = qzaxis(q);
const Vector3f e_z_d = qzaxis(qd);
Quatf qd_red(e_z, e_z_d);
if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
@@ -75,7 +160,7 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
// With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component.
// By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2))
Quatf qd_dyaw = qd_red.inversed() * qd;
Quatf qd_dyaw = qmul(qinv(qd_red), qd);
qd_dyaw.canonicalize();
// catch numerical problems with the domain of acosf and asinf
qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f);
@@ -85,7 +170,7 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3))));
// quaternion attitude control law, qe is rotation from q to qd
const Quatf qe = q.inversed() * qd;
const Quatf qe = qmul(qinv(q), qd);
// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
// also taking care of the antipodal unit quaternion ambiguity
@@ -94,17 +179,24 @@ 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;
// Map reference-frame rates into the current body frame.
const Quatf q_rel = qmul(qinv(q), _q_ref);
// The commanded reference rate (e.g. manual/auto yaw rate) is a setpoint, not a model prediction, so it
// bypasses the reference model: it is fed forward at unity regardless of the feedforward gain and limit.
rate_setpoint += q_rel.rotateVector(_omega_command);
// the gain scales and the limit caps the model's error-driven anticipation (zero at gain 0)
Vector3f omega_ff = _ff_gain * q_rel.rotateVector(_omega_correction);
if (_ff_max > 0.f) {
for (int i = 0; i < 3; i++) {
omega_ff(i) = math::constrain(omega_ff(i), -_ff_max, _ff_max);
}
}
rate_setpoint += omega_ff;
// limit rates
for (int i = 0; i < 3; i++) {
rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
@@ -64,6 +64,15 @@ public:
*/
void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight);
/// Set the critically damped reference-model natural frequency [rad/s].
void setRefModelFrequency(float omega_n);
/// Set the FF magnitude scaling [0..1]
void setFeedForwardGain(float gain) { _ff_gain = math::constrain(gain, 0.f, 1.f); }
/// Set per-axis saturation on the FF angular-velocity contribution [rad/s]; 0 = disabled.
void setFeedForwardLimit(float limit) { _ff_max = math::max(limit, 0.f); }
/**
* Set hard limit for output rate setpoints
* @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw
@@ -74,24 +83,16 @@ 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
*/
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);
/**
* Run one control loop cycle calculation
@@ -100,11 +101,32 @@ public:
*/
matrix::Vector3f update(const matrix::Quatf &q) const;
/**
* Attitude state of the reference model
*/
const matrix::Quatf &getReferenceAttitude() const { return _q_ref; }
private:
/**
* Advance the 2nd-order reference model by one step toward the desired attitude
* @param qd normalized desired attitude setpoint
* @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame
* @param dt [s] time since previous setpoint (> 0)
*/
void propagateReferenceModel(const matrix::Quatf &qd, const float yawspeed_setpoint, const float dt);
matrix::Vector3f _proportional_gain;
matrix::Vector3f _rate_limit;
float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize compared 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
matrix::Quatf _q_ref; ///< reference attitude tracked by the 2nd-order ref model
matrix::Vector3f _omega_correction; ///< error-driven correction (2nd-order state); reference rate = _omega_correction + _omega_command
matrix::Vector3f _omega_command; ///< commanded (analytical) reference rate; exempt from the feedforward limit
bool _ref_initialized{false};
float _omega_n{50.f}; ///< ref-model natural frequency [rad/s]
float _kq{_omega_n * _omega_n}; ///< stiffness coefficient, kept in sync with _omega_n
float _ff_gain{1.f};
float _ff_max{0.f};
};
@@ -138,3 +138,264 @@ 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));
_attitude_control.setRefModelFrequency(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 int kSettleSteps = 500; // generous settling window for any default omega_n
};
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, reference model settled
const float omega = 0.5f;
rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps);
// WHEN: vehicle is at the reference (no P error → P term = 0)
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: rate setpoint is the FF from omega_ref, which has converged to the ramp rate
EXPECT_NEAR(rate_setpoint(0), omega, 1e-3f);
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, reference model settled
const float omega = 0.5f;
rampSetpoint(Vector3f(0.f, omega, 0.f), 0.f, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), omega, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, HighRateRampStillExact)
{
// GIVEN: a high-rate pitch ramp (~90 dps). Reference model steady-state holds at
// the ramp rate independent of magnitude (the model's equilibrium tracking is
// rate-invariant for any constant-rate command).
const float omega = 1.5708f; // ~90 dps
rampSetpoint(Vector3f(0.f, omega, 0.f), 0.f, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), omega, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, YawRampOnlyAnalyticalFeedforwardContributes)
{
// GIVEN: a yaw ramp with the analytical yawspeed setpoint matching. The reference
// model's damping is biased toward this known rate, so omega_ref settles to
// (0,0,omega) in q_ref's body frame and the FF reads out the body-z component.
const float omega = 0.5f;
rampSetpoint(Vector3f(0.f, 0.f, omega), omega, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), omega, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, TiltedYawDoesNotDoubleCount)
{
// GIVEN: body locked at constant tilt, yawing around world-z at constant rate.
// Truth body angular velocity: ω_body = R_BW · (0, 0, yaw_rate)
// = (-sin(tilt)·yaw_rate, 0, cos(tilt)·yaw_rate)
// The reference model bakes yaw_sp_move_rate into omega_ref via its damping bias,
// so the FF reproduces the body-frame projection of the world-z rotation —
// no separate analytical path, no double-count possible.
const float tilt = 0.5f; // ~28.6° pitch
const float yaw_rate = 0.5f; // ~28.6 dps
const Quatf q_pitch(AxisAnglef(Vector3f(0.f, tilt, 0.f)));
Quatf q_d;
for (int i = 0; i < kSettleSteps; i++) {
const Quatf q_yaw(AxisAnglef(Vector3f(0.f, 0.f, yaw_rate * kDt * i)));
q_d = q_yaw * q_pitch;
_attitude_control.setAttitudeSetpoint(q_d, yaw_rate, (i == 0) ? -1.f : kDt);
}
// WHEN: vehicle is at the reference attitude
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: rate setpoint matches the analytical world-z rotation in body frame
EXPECT_NEAR(rate_setpoint(0), -sinf(tilt) * yaw_rate, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), cosf(tilt) * yaw_rate, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, FeedForwardDisabledSuppressesContribution)
{
// GIVEN: a settled roll-ramp reference
const float omega = 0.5f;
rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps);
// WHEN: the feedforward gain is 0, evaluated at the reference attitude so the P term is zero
_attitude_control.setFeedForwardGain(0.f);
Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: the anticipation feedforward is fully suppressed
EXPECT_NEAR(rate_setpoint.norm(), 0.f, 1e-3f);
// AND WHEN: the gain is restored, the anticipation returns (reference model preserved)
_attitude_control.setFeedForwardGain(1.f);
rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
EXPECT_NEAR(rate_setpoint(0), omega, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, UnlockedYawDoesNotFeedBackSlavedHeading)
{
// GIVEN: the manual yaw-rate regime. StickYaw slaves the setpoint heading to the measured yaw,
// so q_d.yaw ramps at the (large) vehicle yaw rate while the commanded analytical rate is only
// the small decaying filter tail. Differentiating the slaved heading would latch the FF onto the
// measured rate (the uncommanded-yaw runaway). Reproduce: q_d.yaw ramps fast, yawspeed_sp small.
const float ramp_rate = 0.8f; // slaved-heading rate (≈ measured yaw rate)
const float commanded = 0.05f; // small but > FLT_EPSILON: yaw stays "unlocked"
rampSetpoint(Vector3f(0.f, 0.f, ramp_rate), commanded, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: yaw FF tracks only the analytical commanded rate, NOT the slaved-heading ramp (no latch)
EXPECT_NEAR(rate_setpoint(2), commanded, 1e-3f);
EXPECT_LT(fabsf(rate_setpoint(2)), 0.2f);
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, LockedYawRampStillFeedsForward)
{
// GIVEN: a genuine yaw-angle target slewed with no commanded rate (heading-hold / auto yaw).
// yawspeed_sp = 0 → the heading is exogenous, not slaved, so the full reference-model FF must
// still differentiate it. This guards against over-gating killing the legitimate yaw FF.
const float omega = 0.5f;
rampSetpoint(Vector3f(0.f, 0.f, omega), 0.f, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
EXPECT_NEAR(rate_setpoint(2), omega, 1e-3f);
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, TiltedUnlockedYawUsesCommandedRateOnly)
{
// GIVEN: tilted while the slaved heading ramps fast (q_d.yaw := measured) but the commanded
// analytical rate is small — the tilted form of the uncommanded-yaw runaway.
const float tilt = 0.5f;
const float slaved_rate = 0.8f; // q_d.yaw ramp (≈ measured yaw rate)
const float commanded = 0.05f; // small but > FLT_EPSILON: yaw stays "unlocked"
const Quatf q_pitch(AxisAnglef(Vector3f(0.f, tilt, 0.f)));
Quatf q_d;
for (int i = 0; i < kSettleSteps; i++) {
const Quatf q_yaw(AxisAnglef(Vector3f(0.f, 0.f, slaved_rate * kDt * i)));
q_d = q_yaw * q_pitch;
_attitude_control.setAttitudeSetpoint(q_d, commanded, (i == 0) ? -1.f : kDt);
}
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: FF is the commanded world-z rate projected into the body frame (not the slaved ramp),
// with no roll/pitch leak from the stripped heading error.
EXPECT_NEAR(rate_setpoint(0), -sinf(tilt) * commanded, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), cosf(tilt) * commanded, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, CommandedYawRateFedForwardWhenFFDisabled)
{
// GIVEN: a settled manual yaw-rate command (heading slaved to measurement, small commanded rate)
const float commanded = 0.05f;
rampSetpoint(Vector3f(0.f, 0.f, 0.8f), commanded, kSettleSteps);
// WHEN: the feedforward gain is 0, evaluated at the reference attitude so the P term is zero
_attitude_control.setFeedForwardGain(0.f);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: the commanded yaw rate still bypasses the ref model and is fed forward at unity
EXPECT_NEAR(rate_setpoint(2), commanded, 1e-3f);
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, FractionalGainDoesNotWeakenCommandedYaw)
{
// GIVEN: a manual yaw-rate command with the anticipation gain detuned to 0.1
const float commanded = 0.05f;
_attitude_control.setFeedForwardGain(0.1f);
rampSetpoint(Vector3f(0.f, 0.f, 0.8f), commanded, kSettleSteps);
// Evaluate at the reference attitude so the P term is zero; the only yaw contribution is the FF.
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: commanded yaw authority is preserved at full strength (NOT scaled to 0.1 x commanded).
EXPECT_NEAR(rate_setpoint(2), commanded, 1e-3f);
EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
}
TEST_F(AttitudeControlFeedforwardTest, FractionalGainScalesAnticipation)
{
// GIVEN: a settled roll-ramp reference (pure error-driven anticipation, no commanded rate), gain 0.1
const float omega = 0.5f;
_attitude_control.setFeedForwardGain(0.1f);
rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps);
const Vector3f rate_setpoint = _attitude_control.update(_attitude_control.getReferenceAttitude());
// THEN: the anticipation IS scaled by the gain (0.1 x the settled reference rate), unlike the command.
EXPECT_NEAR(rate_setpoint(0), 0.1f * omega, 1e-3f);
EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f);
EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f);
}
@@ -161,6 +161,10 @@ 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,
(ParamFloat<px4::params::MC_REF_FF_MAX>) _param_mc_ref_ff_max,
/* Stabilized mode params */
(ParamFloat<px4::params::MAN_DEADZONE>) _param_man_deadzone,
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max,
@@ -101,6 +101,10 @@ 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());
_attitude_control.setFeedForwardLimit(math::radians(_param_mc_ref_ff_max.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());
@@ -316,7 +320,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;
}
@@ -95,6 +95,43 @@ parameters:
max: 1800.0
decimal: 1
increment: 5
MC_REF_W_N:
description:
short: Attitude reference-model natural frequency
long: |-
Bandwidth of the reference model that smooths the attitude setpoint
and generates the rate feed-forward. Higher = less lag, more peak
rate demand.
type: float
default: 50.0
unit: rad/s
min: 1.0
max: 200.0
decimal: 0
increment: 1
MC_REF_FF:
description:
short: Attitude reference-model feed-forward gain
long: |-
Scale on the reference-model rate feed-forward. 0 disables it.
type: float
default: 0.0
min: 0.0
max: 1.0
decimal: 2
increment: 0.01
MC_REF_FF_MAX:
description:
short: Feed-forward angular-rate cap; 0 = disabled
long: |-
Per-axis cap on the rate feed-forward.
type: float
default: 100.0
unit: deg/s
min: 0.0
max: 1800.0
decimal: 0
increment: 5
- group: Multicopter Position Control
definitions:
MC_MAN_TILT_TAU: