ardupilot/libraries/AC_PID/AC_PID.h

137 lines
4.8 KiB
C
Raw Normal View History

#pragma once
/// @file AC_PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
2012-10-09 18:01:58 -03:00
#include <stdlib.h>
#include <cmath>
#include <AP_Logger/AP_Logger.h>
2019-06-27 06:33:15 -03:00
#define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency
2014-02-14 02:52:24 -04:00
/// @class AC_PID
/// @brief Copter PID control class
class AC_PID {
public:
// Constructor for PID
2019-06-27 06:33:15 -03:00
AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
// set_dt - set time step in seconds
2019-06-27 06:33:15 -03:00
void set_dt(float dt);
// update_all - set target and measured inputs to PID controller and calculate outputs
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
float update_all(float target, float measurement, bool limit = false);
// update_error - set error input to PID controller and calculate outputs
// target is set to zero and error is set and filtered
// the derivative then is calculated and filtered
// the integral is then updated based on the setting of the limit flag
// Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
float update_error(float error, bool limit = false);
// update_i - update the integral
// if the limit flag is set the integral is only allowed to shrink
void update_i(bool limit);
// get_pid - get results from pid controller
2019-06-27 06:33:15 -03:00
float get_pid() const;
float get_pi() const;
float get_p() const;
float get_i() const;
float get_d() const;
float get_ff();
// todo: remove function when it is no longer used.
float get_ff(float target);
2019-04-15 07:34:39 -03:00
// reset_I - reset the integrator
2019-06-27 06:33:15 -03:00
void reset_I();
2012-08-17 02:37:26 -03:00
// reset_filter - input filter will be reset to the next value provided to set_input()
2019-06-27 06:33:15 -03:00
void reset_filter() {
_flags._reset_filter = true;
}
// load gain from eeprom
2019-06-27 06:33:15 -03:00
void load_gains();
2012-08-17 02:37:26 -03:00
// save gain to eeprom
2019-06-27 06:33:15 -03:00
void save_gains();
/// operator function call for easy initialisation
2019-06-27 06:33:15 -03:00
void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt);
// get accessors
2019-06-27 06:33:15 -03:00
AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; }
AP_Float &kD() { return _kd; }
AP_Float &ff() { return _kff;}
AP_Float &filt_T_hz() { return _filt_T_hz; }
AP_Float &filt_E_hz() { return _filt_E_hz; }
AP_Float &filt_D_hz() { return _filt_D_hz; }
float imax() const { return _kimax.get(); }
float get_filt_alpha(float filt_hz) const;
float get_filt_T_alpha() const;
float get_filt_E_alpha() const;
float get_filt_D_alpha() const;
// set accessors
2019-06-27 06:33:15 -03:00
void kP(const float v) { _kp.set(v); }
void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); }
void ff(const float v) { _kff.set(v); }
void imax(const float v) { _kimax.set(fabsf(v)); }
void filt_T_hz(const float v);
void filt_E_hz(const float v);
void filt_D_hz(const float v);
2012-08-17 02:37:26 -03:00
2018-08-08 01:11:59 -03:00
// set the desired and actual rates (for logging purposes)
2019-06-27 06:33:15 -03:00
void set_target_rate(float target) { _pid_info.target = target; }
void set_actual_rate(float actual) { _pid_info.actual = actual; }
// integrator setting functions
void set_integrator(float target, float measurement, float i);
void set_integrator(float error, float i);
void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); }
2019-06-27 06:33:15 -03:00
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
// parameter var table
2019-06-27 06:33:15 -03:00
static const struct AP_Param::GroupInfo var_info[];
2012-02-12 07:25:50 -04:00
protected:
// parameters
2019-06-27 06:33:15 -03:00
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Float _kff;
AP_Float _kimax;
AP_Float _filt_T_hz; // PID target filter frequency in Hz
AP_Float _filt_E_hz; // PID error filter frequency in Hz
AP_Float _filt_D_hz; // PID derivative filter frequency in Hz
// flags
struct ac_pid_flags {
2019-06-27 06:33:15 -03:00
bool _reset_filter :1; // true when input filter should be reset during next call to set_input
} _flags;
// internal variables
2019-06-27 06:33:15 -03:00
float _dt; // timestep in seconds
float _integrator; // integrator value
float _target; // target value to enable filtering
float _error; // error value to enable filtering
float _derivative; // derivative value to enable filtering
2019-06-27 06:33:15 -03:00
AP_Logger::PID_Info _pid_info;
};