diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 1c6965b58d..a21e87bb22 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -52,6 +52,13 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } +void AttitudeControl::setRefModelFrequency(float omega_n) +{ + _omega_n = math::max(omega_n, 0.1f); + _kq = _omega_n * _omega_n; + _komega = 2.f * _omega_n; +} + void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_setpoint, const float dt) { Quatf qd_normalized = qd; @@ -60,9 +67,9 @@ void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_ if (_ref_initialized && dt > 0.f) { // Exact discretisation of the linearised 2nd-order critically damped // ref model in body-frame coords: - // d/dt[e; w] = [0 -1; kKq -kKomega] [e; w] + [0; kKomega] w_known + // d/dt[e; w] = [0 -1; w_n^2 -2 w_n] [e; w] + [0; 2 w_n] w_known // Closed-form one-step update is unconditionally stable for any dt: - // the symplectic-Euler scheme requires kFFNaturalFreq*dt < 2 and blows + // the symplectic-Euler scheme requires _omega_n * dt < 2 and blows // up when the setpoint stream pauses across a mode handoff (e.g. the // ~500 ms gap PX4 produces when OFFBOARD hands off to AUTO_LAND). const Vector3f w_known_in_ref = std::isfinite(yawspeed_setpoint) @@ -73,12 +80,12 @@ void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_ q_err.canonicalize(); const Vector3f e = 2.f * q_err.imag(); - // Discretisation coefficients (repeated eigenvalue at s = -kFFNaturalFreq). - const float w_dt = kFFNaturalFreq * dt; + // Discretisation coefficients (repeated eigenvalue at s = -_omega_n). + const float w_dt = _omega_n * dt; const float emt = expf(-w_dt); const float a = (1.f + w_dt) * emt; const float b_neg = dt * emt; - const float gamma = kKq * dt * emt; + const float gamma = _kq * dt * emt; const float delta = (1.f - w_dt) * emt; const Vector3f w_offset = _omega_ref - w_known_in_ref; @@ -112,9 +119,10 @@ void AttitudeControl::adaptAttitudeSetpoint(const Quatf &q_delta) matrix::Vector3f AttitudeControl::update(const Quatf &q) const { - // FF off (e.g. autotune): P targets the raw setpoint (legacy behavior). - // FF on: P targets the reference model's smoothed trajectory. - Quatf qd = _ff_enabled ? _q_ref : _attitude_setpoint_q; + // FF off (e.g. autotune, or MC_REF_FF=0): P targets the raw setpoint + // (legacy behaviour). FF on: P targets the ref model's smoothed trajectory. + const bool ff_active = _ff_enabled && (_ff_gain > 0.f); + Quatf qd = ff_active ? _q_ref : _attitude_setpoint_q; // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch const Vector3f e_z = q.dcm_z(); @@ -154,10 +162,11 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // calculate angular rates setpoint Vector3f rate_setpoint = eq.emult(_proportional_gain); - if (_ff_enabled) { - // Feed forward the reference model's angular velocity. _omega_ref lives in - // q_ref's body frame; go through world to land in the current body frame. - rate_setpoint += q.rotateVectorInverse(_q_ref.rotateVector(_omega_ref)); + if (ff_active) { + // Feed forward the reference model's angular velocity, scaled by _ff_gain. + // _omega_ref lives in q_ref's body frame; go through world to land in the + // current body frame. + rate_setpoint += _ff_gain * q.rotateVectorInverse(_q_ref.rotateVector(_omega_ref)); } // limit rates diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 0954624053..ae2a923350 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -57,12 +57,12 @@ public: AttitudeControl() = default; ~AttitudeControl() = default; - // Natural frequency of the attitude reference model (rad/s). + // Default natural frequency of the attitude reference model (rad/s). // Damping ratio is fixed at 1.0 (critical damping, monotonic settling). - // Placeholder value; revisit after flight test if needed. - static constexpr float kFFNaturalFreq = 10.0f; - static constexpr float kKq = kFFNaturalFreq * kFFNaturalFreq; ///< spring constant - static constexpr float kKomega = 2.0f * kFFNaturalFreq; ///< damping coeff (ΞΆ=1) + // Runtime-tunable via setRefModelFrequency() / MC_REF_W_N. + static constexpr float kFFNaturalFreqDefault = 100.0f; + // Default feed-forward gain (0..1). 1 = full FF, 0 = FF off (legacy P-only). + static constexpr float kFFGainDefault = 1.0f; /** * Set proportional attitude control gain @@ -71,6 +71,23 @@ public: */ void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight); + /** + * Set the reference-model natural frequency [rad/s]. The 2nd-order critically + * damped (zeta=1) ref-model coefficients are recomputed accordingly. + */ + void setRefModelFrequency(float omega_n); + + float getRefModelFrequency() const { return _omega_n; } + + /** + * Set the feed-forward magnitude scaling [0..1]. 0 disables the FF (the + * proportional attitude law then targets q_d directly, like the pre-FF + * behaviour); 1 is full FF. + */ + void setFeedForwardGain(float gain) { _ff_gain = math::constrain(gain, 0.f, 1.f); } + + float getFeedForwardGain() const { return _ff_gain; } + /** * Set hard limit for output rate setpoints * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw @@ -127,4 +144,12 @@ private: bool _ref_initialized{false}; ///< false until first valid setpoint received bool _ff_enabled{true}; + + // Reference-model coefficients. Runtime-tunable via setRefModelFrequency(). + float _omega_n{kFFNaturalFreqDefault}; + float _kq{kFFNaturalFreqDefault * kFFNaturalFreqDefault}; + float _komega{2.f * kFFNaturalFreqDefault}; + + // FF magnitude scaling. 0..1; 0 = off, 1 = full. + float _ff_gain{kFFGainDefault}; }; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 3f5e6e17f8..d2dd27a7d0 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -161,6 +161,9 @@ private: (ParamFloat) _param_mc_pitchrate_max, (ParamFloat) _param_mc_yawrate_max, + (ParamFloat) _param_mc_ref_w_n, + (ParamFloat) _param_mc_ref_ff, + /* 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 36844b499e..1c51fbc7fb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -100,6 +100,9 @@ MulticopterAttitudeControl::parameters_updated() _attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get()))); + _attitude_control.setRefModelFrequency(_param_mc_ref_w_n.get()); + _attitude_control.setFeedForwardGain(_param_mc_ref_ff.get()); + // Update from hover thrust parameter if there's no valid estimate in use if (!PX4_ISFINITE(_hover_thrust_estimate)) { _hover_thrust_slew_rate.setForcedValue(_param_mpc_thr_hover.get()); 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..03d92931e1 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,40 @@ parameters: max: 1800.0 decimal: 1 increment: 5 + MC_REF_W_N: + description: + short: Attitude reference-model natural frequency + long: |- + Natural frequency of the 2nd-order critically-damped reference model + that smooths the attitude setpoint and produces the feed-forward + angular rate. Higher values track the raw setpoint more aggressively + (less lag, smaller steady-state ramp tracking error) but demand more + peak rate from the inner rate loop. The closed-form discretisation + makes any value here unconditionally stable; the practical upper + limit is set by the rate-loop bandwidth. + type: float + default: 100.0 + unit: rad/s + min: 1.0 + max: 200.0 + decimal: 1 + increment: 5 + MC_REF_FF: + description: + short: Attitude reference-model feed-forward gain + long: |- + Scaling applied to the ref-model's angular-velocity feed-forward + contribution to the rate setpoint. 1 = full FF (default), 0 = FF off + (the attitude P-loop targets q_d directly, legacy pre-FF behaviour). + Intermediate values blend. + + Set to 0 if the inner rate loop is not tuned to handle the FF demand. + type: float + default: 1.0 + min: 0.0 + max: 1.0 + decimal: 2 + increment: 0.1 - group: Multicopter Position Control definitions: MC_MAN_TILT_TAU: