mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
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 |
@@ -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}
|
||||
|
||||
@@ -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:
|
||||
|
||||
Reference in new issue
Block a user