Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * Attitude rates controller.
- * Input: '_rates_sp' vector, '_thrust_sp'
- * Output: '_att_control' vector
- */
- void
- MulticopterAttitudeControl::control_attitude_rates(float dt)
- {
- /* reset integral if disarmed */
- if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
- _rates_int.zero();
- }
- /* current body angular rates */
- math::Vector<3> rates;
- rates(0) = _v_att.rollspeed;
- rates(1) = _v_att.pitchspeed;
- rates(2) = _v_att.yawspeed;
- /* angular rates error */
- math::Vector<3> rates_err = _rates_sp - rates;
- _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
- _rates_prev = rates;
- /* update integral only if not saturated on low limit and if motor commands are not saturated */
- if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
- for (int i = 0; i < 3; i++) {
- if (fabsf(_att_control(i)) < _thrust_sp) {
- float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
- if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
- _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
- _rates_int(i) = rate_i;
- }
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement