Advertisement
Guest User

PID for pos-attitude-est

a guest
Apr 24th, 2015
237
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.26 KB | None | 0 0
  1. /*
  2.  * Attitude rates controller.
  3.  * Input: '_rates_sp' vector, '_thrust_sp'
  4.  * Output: '_att_control' vector
  5.  */
  6. void
  7. MulticopterAttitudeControl::control_attitude_rates(float dt)
  8. {
  9.     /* reset integral if disarmed */
  10.     if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
  11.         _rates_int.zero();
  12.     }
  13.  
  14.     /* current body angular rates */
  15.     math::Vector<3> rates;
  16.     rates(0) = _v_att.rollspeed;
  17.     rates(1) = _v_att.pitchspeed;
  18.     rates(2) = _v_att.yawspeed;
  19.  
  20.     /* angular rates error */
  21.     math::Vector<3> rates_err = _rates_sp - rates;
  22.     _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
  23.     _rates_prev = rates;
  24.  
  25.     /* update integral only if not saturated on low limit and if motor commands are not saturated */
  26.     if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
  27.         for (int i = 0; i < 3; i++) {
  28.             if (fabsf(_att_control(i)) < _thrust_sp) {
  29.                 float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
  30.  
  31.                 if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
  32.                     _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
  33.                     _rates_int(i) = rate_i;
  34.                 }
  35.             }
  36.         }
  37.     }
  38. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement