47 _proportional_gain = proportional_gain;
50 const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.
f;
53 _proportional_gain(2) = roll_pitch_gain;
65 Quatf qd_red(e_z, e_z_d);
67 if (fabsf(qd_red(1)) > (1.
f - 1e-5
f) || fabsf(qd_red(2)) > (1.
f - 1e-5
f)) {
84 qd = qd_red *
Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));
103 rate_setpoint += q.
inversed().
dcm_z() * yawspeed_feedforward;
106 for (
int i = 0; i < 3; i++) {
107 rate_setpoint(i) =
math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
110 return rate_setpoint;
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
Run one control loop cycle calculation.
Limiting / constrain helper functions.
collection of rather simple mathematical functions that get used over and over again ...
A quaternion based attitude controller.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void setProportionalGain(const matrix::Vector3f &proportional_gain)
Set proportional attitude control gain.
Vector3< Type > imag() const
Imaginary components of quaternion.
Quaternion inversed() const
Invert quaternion.
Quaternion< float > Quatf
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const