diff --git a/docs/assets/diagrams/mc_angle_diagram.jpg b/docs/assets/diagrams/mc_angle_diagram.jpg index ac0f27d109..6636999e05 100644 Binary files a/docs/assets/diagrams/mc_angle_diagram.jpg and b/docs/assets/diagrams/mc_angle_diagram.jpg differ diff --git a/docs/assets/diagrams/mc_control_arch_tikz.tex b/docs/assets/diagrams/mc_control_arch_tikz.tex index 548b88078e..9206a09137 100644 --- a/docs/assets/diagrams/mc_control_arch_tikz.tex +++ b/docs/assets/diagrams/mc_control_arch_tikz.tex @@ -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} diff --git a/docs/en/flight_stack/controller_diagrams.md b/docs/en/flight_stack/controller_diagrams.md index 6c3822f4bf..b8a52bd044 100644 --- a/docs/en/flight_stack/controller_diagrams.md +++ b/docs/en/flight_stack/controller_diagrams.md @@ -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 diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 02f3894f0e..54e7605038 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -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)); diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index d90c623081..4124719435 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -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}; }; diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp index 2803686c12..103e45b167 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -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); +} diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 3f5e6e17f8..b6e9b066d9 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -161,6 +161,10 @@ private: (ParamFloat) _param_mc_pitchrate_max, (ParamFloat) _param_mc_yawrate_max, + (ParamFloat) _param_mc_ref_w_n, + (ParamFloat) _param_mc_ref_ff, + (ParamFloat) _param_mc_ref_ff_max, + /* Stabilized mode params */ (ParamFloat) _param_man_deadzone, (ParamFloat) _param_mpc_man_tilt_max, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 1b7b2ec2fa..b573de9d3a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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; } diff --git a/src/modules/mc_att_control/mc_att_control_params.yaml b/src/modules/mc_att_control/mc_att_control_params.yaml index 00def08bc9..e9e43018a6 100644 --- a/src/modules/mc_att_control/mc_att_control_params.yaml +++ b/src/modules/mc_att_control/mc_att_control_params.yaml @@ -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: