mirror of https://github.com/ArduPilot/ardupilot
111 lines
3.8 KiB
C++
111 lines
3.8 KiB
C++
#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>
|
|
#include <stdlib.h>
|
|
#include <cmath>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency
|
|
#define AC_PID_FILT_HZ_MIN 0.01f // minimum input filter frequency
|
|
|
|
/// @class AC_PID
|
|
/// @brief Copter PID control class
|
|
class AC_PID {
|
|
public:
|
|
|
|
// Constructor for PID
|
|
AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff = 0);
|
|
|
|
// set_dt - set time step in seconds
|
|
void set_dt(float dt);
|
|
|
|
// set_input_filter_all - set input to PID controller
|
|
// input is filtered before the PID controllers are run
|
|
// this should be called before any other calls to get_p, get_i or get_d
|
|
void set_input_filter_all(float input);
|
|
|
|
// set_input_filter_d - set input to PID controller
|
|
// only input to the D portion of the controller is filtered
|
|
// this should be called before any other calls to get_p, get_i or get_d
|
|
void set_input_filter_d(float input);
|
|
|
|
// get_pid - get results from pid controller
|
|
float get_pid();
|
|
float get_pi();
|
|
float get_p();
|
|
float get_i();
|
|
float get_d();
|
|
float get_ff(float requested_rate);
|
|
|
|
// reset_I - reset the integrator
|
|
void reset_I();
|
|
|
|
// reset_filter - input filter will be reset to the next value provided to set_input()
|
|
void reset_filter() { _flags._reset_filter = true; }
|
|
|
|
// load gain from eeprom
|
|
void load_gains();
|
|
|
|
// save gain to eeprom
|
|
void save_gains();
|
|
|
|
/// operator function call for easy initialisation
|
|
void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval = 0);
|
|
|
|
// get accessors
|
|
AP_Float &kP() { return _kp; }
|
|
AP_Float &kI() { return _ki; }
|
|
AP_Float &kD() { return _kd; }
|
|
AP_Float &filt_hz() { return _filt_hz; }
|
|
float imax() const { return _imax.get(); }
|
|
float get_filt_alpha() const;
|
|
float ff() const { return _ff.get(); }
|
|
|
|
// set accessors
|
|
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 imax(const float v) { _imax.set(fabsf(v)); }
|
|
void filt_hz(const float v);
|
|
void ff(const float v) { _ff.set(v); }
|
|
|
|
float get_integrator() const { return _integrator; }
|
|
void set_integrator(float i) { _integrator = i; }
|
|
|
|
// set the desired and actual rates (for logging purposes)
|
|
void set_desired_rate(float desired) { _pid_info.desired = desired; }
|
|
void set_actual_rate(float actual) { _pid_info.actual = actual; }
|
|
|
|
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
|
|
|
// parameter var table
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
protected:
|
|
|
|
// parameters
|
|
AP_Float _kp;
|
|
AP_Float _ki;
|
|
AP_Float _kd;
|
|
AP_Float _imax;
|
|
AP_Float _filt_hz; // PID Input filter frequency in Hz
|
|
AP_Float _ff;
|
|
|
|
// flags
|
|
struct ac_pid_flags {
|
|
bool _reset_filter : 1; // true when input filter should be reset during next call to set_input
|
|
} _flags;
|
|
|
|
// internal variables
|
|
float _dt; // timestep in seconds
|
|
float _integrator; // integrator value
|
|
float _input; // last input for derivative
|
|
float _derivative; // last derivative for low-pass filter
|
|
|
|
AP_Logger::PID_Info _pid_info;
|
|
};
|