diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 02f3894f0e..b6f3b4d307 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -41,6 +41,25 @@ using namespace matrix; +namespace +{ +// Rotation vector taking world-z onto q's thrust direction (zero yaw content). +Vector3f extractTiltState(const Quatf &q) +{ + const Vector3f e_z_d = q.dcm_z(); + const Vector3f e_z_world(0.f, 0.f, 1.f); + const Vector3f tilt_axis = e_z_world.cross(e_z_d); + const float tilt_axis_norm = tilt_axis.norm(); + + if (tilt_axis_norm > FLT_EPSILON) { + const float tilt_angle = acosf(math::constrain(e_z_world.dot(e_z_d), -1.f, 1.f)); + return (tilt_axis / tilt_axis_norm) * tilt_angle; + } + + return Vector3f{}; +} +} // namespace + void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight) { _proportional_gain = proportional_gain; @@ -52,6 +71,28 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } +void AttitudeControl::setAttitudeSetpoint(const Quatf &qd, const float yawspeed_setpoint, const float dt) +{ + Quatf qd_normalized = qd; + qd_normalized.normalize(); + + // Tilt-only into the model; yaw is handled by _yawspeed_setpoint in update(). + // The model auto-resets on dt <= 0 or dt > max_step. + _tilt_filter.update(dt, extractTiltState(qd_normalized)); + + _attitude_setpoint_q = qd_normalized; + _yawspeed_setpoint = yawspeed_setpoint; +} + +void AttitudeControl::adaptAttitudeSetpoint(const Quatf &q_delta) +{ + _attitude_setpoint_q = q_delta * _attitude_setpoint_q; + _attitude_setpoint_q.normalize(); + // Re-extract tilt and reseed the model so any tilt component of the heading + // reset isn't interpreted as real motion on the next update. + _tilt_filter.reset(extractTiltState(_attitude_setpoint_q)); +} + matrix::Vector3f AttitudeControl::update(const Quatf &q) const { Quatf qd = _attitude_setpoint_q; @@ -94,15 +135,20 @@ 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; + if (_ff_enabled) { + // Tilt FF from the reference model (world frame); rotate to body. + rate_setpoint += q.rotateVectorInverse(_tilt_filter.getRate()); + + // 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; + } } // limit rates diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index d90c623081..c720627075 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -50,6 +50,7 @@ #include #include +#include class AttitudeControl { @@ -57,6 +58,11 @@ public: AttitudeControl() = default; ~AttitudeControl() = default; + // Natural frequency of the tilt feedforward reference model. + // Damping ratio is fixed at 1.0 (critical damping, monotonic settling). + // Placeholder value; revisit after flight test if needed. + static constexpr float kTiltFFNaturalFreq = 10.0f; + /** * Set proportional attitude control gain * @param proportional_gain 3D vector containing gains for roll, pitch, yaw @@ -74,24 +80,22 @@ 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; <= 0 skips the FF derivative update */ - 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); + + /** + * Gate the setpoint-derivative feedforward (the filter keeps running, only + * its addition to the rate setpoint is suppressed). Used during autotune. + */ + void setFeedForwardEnabled(bool enabled) { _ff_enabled = enabled; } /** * Run one control loop cycle calculation @@ -107,4 +111,7 @@ private: 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 + + math::SecondOrderReferenceModel _tilt_filter{kTiltFFNaturalFreq, 1.0f}; + bool _ff_enabled{true}; }; diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp index 2803686c12..d45abb478a 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -138,3 +138,117 @@ 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)); + } + + // 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 float kOmegaN = AttitudeControl::kTiltFFNaturalFreq; // 10 rad/s + static constexpr int kSettleSteps = 200; // ~ 8 / omega_n, well-settled +}; + +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, vehicle perfectly tracking + const float omega = 0.5f; + const Quatf q_d = rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps); + + // WHEN: vehicle is at the setpoint (no error → P term = 0) + const Vector3f rate_setpoint = _attitude_control.update(q_d); + + // THEN: the entire rate setpoint comes from the tilt FF and lies on the roll axis + EXPECT_NEAR(rate_setpoint(0), omega, 0.05f * omega); + 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, vehicle perfectly tracking + const float omega = 0.5f; + const Quatf q_d = rampSetpoint(Vector3f(0.f, omega, 0.f), 0.f, kSettleSteps); + + const Vector3f rate_setpoint = _attitude_control.update(q_d); + + EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f); + EXPECT_NEAR(rate_setpoint(1), omega, 0.05f * omega); + EXPECT_NEAR(rate_setpoint(2), 0.f, 1e-3f); +} + +TEST_F(AttitudeControlFeedforwardTest, YawRampOnlyAnalyticalFeedforwardContributes) +{ + // GIVEN: a yaw ramp with the analytical yawspeed FF set to the same rate. + // Yaw rotation does not change the desired thrust direction, so the tilt + // reference model sees a constant input and produces zero output. The yaw FF + // thus comes solely from the analytical yaw_sp_move_rate path — no projection + // or double-counting needed because yaw never enters the model. + const float omega = 0.5f; + const Quatf q_d = rampSetpoint(Vector3f(0.f, 0.f, omega), omega, kSettleSteps); + + const Vector3f rate_setpoint = _attitude_control.update(q_d); + + EXPECT_NEAR(rate_setpoint(0), 0.f, 1e-3f); + EXPECT_NEAR(rate_setpoint(1), 0.f, 1e-3f); + EXPECT_NEAR(rate_setpoint(2), omega, 0.05f * omega); +} + +TEST_F(AttitudeControlFeedforwardTest, FeedForwardDisabledSuppressesContribution) +{ + // GIVEN: a settled roll-ramp FF + const float omega = 0.5f; + const Quatf q_d = rampSetpoint(Vector3f(omega, 0.f, 0.f), 0.f, kSettleSteps); + + // WHEN: FF is disabled (e.g. autotune active) + _attitude_control.setFeedForwardEnabled(false); + Vector3f rate_setpoint = _attitude_control.update(q_d); + + // THEN: no FF applied even though the model still holds the ramp rate + EXPECT_NEAR(rate_setpoint.norm(), 0.f, 1e-3f); + + // AND WHEN: re-enabled, the FF returns immediately (model state preserved) + _attitude_control.setFeedForwardEnabled(true); + rate_setpoint = _attitude_control.update(q_d); + + EXPECT_NEAR(rate_setpoint(0), omega, 0.05f * omega); + 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_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 99f270f78a..36844b499e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -315,7 +315,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; } @@ -341,19 +345,23 @@ MulticopterAttitudeControl::Run() _quat_reset_counter = v_att.quat_reset_counter; } - Vector3f rates_sp = _attitude_control.update(q); - + // While autotune is identifying, suppress the FF and add its injected rate. const hrt_abstime now = hrt_absolute_time(); autotune_attitude_control_status_s pid_autotune; + const bool autotune_status_fresh = _autotune_attitude_control_status_sub.copy(&pid_autotune) + && ((now - pid_autotune.timestamp) < 1_s); + const bool autotune_active = autotune_status_fresh + && (pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL + || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH + || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW + || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST); - if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) { - if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL - || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH - || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW - || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST) - && ((now - pid_autotune.timestamp) < 1_s)) { - rates_sp += Vector3f(pid_autotune.rate_sp); - } + _attitude_control.setFeedForwardEnabled(!autotune_active); + + Vector3f rates_sp = _attitude_control.update(q); + + if (autotune_active) { + rates_sp += Vector3f(pid_autotune.rate_sp); } // publish rate setpoint