ardupilot/libraries/AC_PID/AC_PI_2D.h

94 lines
2.9 KiB
C
Raw Normal View History

#pragma once
2015-01-29 02:49:30 -04:00
/// @file AC_PI_2D.h
/// @brief 2-axis PI controller with EEPROM-backed storage of constants.
2015-01-29 02:49:30 -04:00
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
2015-01-29 02:49:30 -04:00
#include <stdlib.h>
#include <cmath>
2015-01-29 02:49:30 -04:00
#define AC_PI_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency
2015-01-29 02:49:30 -04:00
/// @class AC_PI_2D
/// @brief 2-axis PI controller
2015-01-29 02:49:30 -04:00
class AC_PI_2D {
public:
// constructor
2015-01-29 02:49:30 -04:00
AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt);
2020-01-04 02:25:34 -04:00
// set time step in seconds
void set_dt(float dt);
2015-01-29 02:49:30 -04:00
// set_input - set input to PI controller
// input is filtered before the PI controllers are run
2015-01-29 02:49:30 -04:00
// this should be called before any other calls to get_p, get_i or get_d
2020-01-04 02:25:34 -04:00
void set_input(const Vector2f &input);
2021-05-12 00:51:35 -03:00
void set_input(const Vector3f &input) { set_input(Vector2f{input.x, input.y}); }
2015-01-29 02:49:30 -04:00
// get_pi - get results from pid controller
2020-01-04 02:25:34 -04:00
Vector2f get_pi();
Vector2f get_p() const;
Vector2f get_i();
Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink)
2015-01-29 02:49:30 -04:00
// reset_I - reset the integrator
2020-01-04 02:25:34 -04:00
void reset_I();
2015-01-29 02:49:30 -04:00
// reset_filter - input filter will be reset to the next value provided to set_input()
2020-01-04 02:25:34 -04:00
void reset_filter();
2015-01-29 02:49:30 -04:00
// load gain from eeprom
2020-01-04 02:25:34 -04:00
void load_gains();
2015-01-29 02:49:30 -04:00
// save gain to eeprom
2020-01-04 02:25:34 -04:00
void save_gains();
2015-01-29 02:49:30 -04:00
/// operator function call for easy initialisation
2015-04-28 03:04:54 -03:00
void operator() (float p, float i, float imaxval, float input_filt_hz, float dt);
2015-01-29 02:49:30 -04:00
// get accessors
2020-01-04 02:25:34 -04:00
AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; }
float imax() const { return _imax.get(); }
float filt_hz() const { return _filt_hz.get(); }
float get_filt_alpha() const { return _filt_alpha; }
2015-01-29 02:49:30 -04:00
// set accessors
2020-01-04 02:25:34 -04:00
void kP(float v) { _kp.set(v); }
void kI(float v) { _ki.set(v); }
void imax(float v) { _imax.set(fabsf(v)); }
void filt_hz(float hz);
2015-01-29 02:49:30 -04:00
2020-01-04 02:25:34 -04:00
Vector2f get_integrator() const { return _integrator; }
void set_integrator(const Vector2f &i) { _integrator = i; }
void set_integrator(const Vector3f &i) { _integrator.x = i.x; _integrator.y = i.y; }
2015-01-29 02:49:30 -04:00
// parameter var table
2020-01-04 02:25:34 -04:00
static const struct AP_Param::GroupInfo var_info[];
2015-01-29 02:49:30 -04:00
2020-01-04 02:25:34 -04:00
private:
2015-01-29 02:49:30 -04:00
// calc_filt_alpha - recalculate the input filter alpha
2020-01-04 02:25:34 -04:00
void calc_filt_alpha();
2015-01-29 02:49:30 -04:00
// parameters
2020-01-04 02:25:34 -04:00
AP_Float _kp;
AP_Float _ki;
AP_Float _imax;
AP_Float _filt_hz; // PI Input filter frequency in Hz
2015-01-29 02:49:30 -04:00
// flags
struct ac_pid_flags {
2020-01-04 02:25:34 -04:00
bool _reset_filter : 1; // true when input filter should be reset during next call to set_input
2015-01-29 02:49:30 -04:00
} _flags;
// internal variables
2020-01-04 02:25:34 -04:00
float _dt; // timestep in seconds
Vector2f _integrator; // integrator value
Vector2f _input; // last input for derivative
float _filt_alpha; // input filter alpha
2015-01-29 02:49:30 -04:00
};