39 #include <px4_platform_common/defines.h> 53 if (force || fabsf(_lp_filters_d.get_cutoff_freq() - cutoff) > 0.01
f) {
54 _lp_filters_d.set_cutoff_frequency(loop_rate, cutoff);
55 _lp_filters_d.reset(_rate_prev);
72 Vector3f rate_error = rate_sp - rate;
75 const Vector3f rate_filtered(_lp_filters_d.apply(rate));
79 rate_d = (rate_filtered - _rate_prev_filtered) / dt;
83 const Vector3f torque = _gain_p.
emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
86 _rate_prev_filtered = rate_filtered;
90 updateIntegral(rate_error, dt);
98 for (
int i = 0; i < 3; i++) {
100 if (_mixer_saturation_positive[i]) {
101 rate_error(i) =
math::min(rate_error(i), 0.
f);
105 if (_mixer_saturation_negative[i]) {
106 rate_error(i) =
math::max(rate_error(i), 0.
f);
116 i_factor =
math::max(0.0
f, 1.
f - i_factor * i_factor);
119 float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) *
dt;
122 if (PX4_ISFINITE(rate_i)) {
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
static struct vehicle_status_s status
PID 3 axis angular rate / angular velocity control.
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed)
Run one control loop cycle calculation.
void setSaturationStatus(const MultirotorMixer::saturation_status &status)
Set saturation status.
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
Set the rate control gains.
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
Set update frequency and low-pass filter cutoff that is applied to the derivative term...
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
Get status message of controller for logging/debugging.
constexpr _Tp min(_Tp a, _Tp b)
constexpr _Tp max(_Tp a, _Tp b)
void updateIntegral(matrix::Vector3f &rate_error, const float dt)
struct MultirotorMixer::saturation_status::@66 flags
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const