123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176 |
- #include <AP_Math/AP_Math.h>
- #include "AC_PI_2D.h"
- const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
-
-
-
- AP_GROUPINFO("P", 0, AC_PI_2D, _kp, 0),
-
-
-
- AP_GROUPINFO("I", 1, AC_PI_2D, _ki, 0),
-
-
-
- AP_GROUPINFO("IMAX", 2, AC_PI_2D, _imax, 0),
-
-
-
-
- AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT),
- AP_GROUPEND
- };
- AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) :
- _dt(dt)
- {
-
- AP_Param::setup_object_defaults(this, var_info);
- _kp = initial_p;
- _ki = initial_i;
- _imax = fabsf(initial_imax);
- filt_hz(initial_filt_hz);
-
- _flags._reset_filter = true;
- }
- void AC_PI_2D::set_dt(float dt)
- {
-
- _dt = dt;
- calc_filt_alpha();
- }
- void AC_PI_2D::filt_hz(float hz)
- {
- _filt_hz.set(fabsf(hz));
-
- _filt_hz = MAX(_filt_hz, AC_PI_2D_FILT_HZ_MIN);
-
- calc_filt_alpha();
- }
- void AC_PI_2D::set_input(const Vector2f &input)
- {
-
- if (!isfinite(input.x) || !isfinite(input.y)) {
- return;
- }
-
- if (_flags._reset_filter) {
- _flags._reset_filter = false;
- _input = input;
- }
-
- Vector2f input_filt_change = (input - _input) * _filt_alpha;
- _input = _input + input_filt_change;
- }
- Vector2f AC_PI_2D::get_p() const
- {
- return (_input * _kp);
- }
- Vector2f AC_PI_2D::get_i()
- {
- if (!is_zero(_ki) && !is_zero(_dt)) {
- _integrator += (_input * _ki) * _dt;
- const float integrator_length = _integrator.length();
- if ((integrator_length > _imax) && (is_positive(integrator_length))) {
- _integrator *= (_imax / integrator_length);
- }
- return _integrator;
- }
- return Vector2f();
- }
- Vector2f AC_PI_2D::get_i_shrink()
- {
- if (!is_zero(_ki) && !is_zero(_dt)) {
- const float integrator_length_orig = MIN(_integrator.length(),_imax);
- _integrator += (_input * _ki) * _dt;
- const float integrator_length_new = _integrator.length();
- if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) {
- _integrator *= (integrator_length_orig / integrator_length_new);
- }
- return _integrator;
- }
- return Vector2f();
- }
- Vector2f AC_PI_2D::get_pi()
- {
- return get_p() + get_i();
- }
- void AC_PI_2D::reset_I()
- {
- _integrator.zero();
- }
- void AC_PI_2D::load_gains()
- {
- _kp.load();
- _ki.load();
- _imax.load();
- _imax = fabsf(_imax);
- _filt_hz.load();
-
- calc_filt_alpha();
- }
- void AC_PI_2D::save_gains()
- {
- _kp.save();
- _ki.save();
- _imax.save();
- _filt_hz.save();
- }
- void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz, float dt)
- {
- _kp = p;
- _ki = i;
- _imax = fabsf(imaxval);
- _filt_hz = input_filt_hz;
- _dt = dt;
-
- calc_filt_alpha();
- }
- void AC_PI_2D::calc_filt_alpha()
- {
- if (is_zero(_filt_hz)) {
- _filt_alpha = 1.0f;
- return;
- }
-
-
- const float rc = 1/(M_2PI*_filt_hz);
- _filt_alpha = _dt / (_dt + rc);
- }
|