Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: use new defualt pattern

This commit is contained in:
Iampete1 2023-01-03 17:22:36 +00:00 committed by Andrew Tridgell
parent 46e560f3f6
commit 18d0dbcd8a
14 changed files with 89 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_PID/AC_P.cpp
View File

@ -8,7 +8,7 @@ const AP_Param::GroupInfo AC_P::var_info[] = {
// @Param: P
// @DisplayName: PI Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_P, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P, _kp, default_kp),
AP_GROUPEND
};

8
libraries/AC_PID/AC_P.h
View File

@ -20,10 +20,10 @@ public:
///
/// @param initial_p Initial value for the P term.
///
AC_P(const float &initial_p = 0.0f)
AC_P(const float &initial_p = 0.0f) :
default_kp(initial_p)
{
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_p);
AP_Param::setup_object_defaults(this, var_info);
}
CLASS_NO_COPY(AC_P);
@ -65,4 +65,6 @@ public:
private:
AP_Float _kp;
const float default_kp;
};

9
libraries/AC_PID/AC_PI.cpp
View File

@ -25,14 +25,13 @@ const AP_Param::GroupInfo AC_PI::var_info[] = {
};
// Constructor
AC_PI::AC_PI(float initial_p, float initial_i, float initial_imax)
AC_PI::AC_PI(float initial_p, float initial_i, float initial_imax) :
default_kp(initial_p),
default_ki(initial_i),
default_imax(initial_imax)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
kP.set_and_default(initial_p);
kI.set_and_default(initial_i);
imax.set_and_default(initial_imax);
}
float AC_PI::update(float measurement, float target, float dt)

6
libraries/AC_PID/AC_PI.h
View File

@ -33,4 +33,10 @@ protected:
AP_Float imax;
float integrator;
float output_P;
private:
const float default_kp;
const float default_ki;
const float default_imax;
};

33
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -4,65 +4,62 @@
#include <AP_Math/AP_Math.h>
#include "AC_PID_2D.h"
#define AC_PID_2D_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PID_2D_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency
#define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_PID_2D, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_2D, _kp, default_kp),
// @Param: I
// @DisplayName: PID Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
AP_GROUPINFO("I", 1, AC_PID_2D, _ki, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_2D, _ki, default_ki),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 2, AC_PID_2D, _kimax, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_2D, _kimax, default_kimax),
// @Param: FLTE
// @DisplayName: PID Input filter frequency in Hz
// @Description: Input filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTE", 3, AC_PID_2D, _filt_E_hz, AC_PID_2D_FILT_E_HZ_DEFAULT),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_2D, _filt_E_hz, default_filt_E_hz),
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
AP_GROUPINFO("D", 4, AC_PID_2D, _kd, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_2D, _kd, default_kd),
// @Param: FLTD
// @DisplayName: D term filter frequency in Hz
// @Description: D term filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTD", 5, AC_PID_2D, _filt_D_hz, AC_PID_2D_FILT_D_HZ_DEFAULT),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_2D, _filt_D_hz, default_filt_D_hz),
// @Param: FF
// @DisplayName: PID Feed Forward Gain
// @Description: FF Gain which produces an output that is proportional to the magnitude of the target
AP_GROUPINFO("FF", 6, AC_PID_2D, _kff, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_2D, _kff, default_kff),
AP_GROUPEND
};
// Constructor
AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) :
default_kp(initial_kP),
default_ki(initial_kI),
default_kd(initial_kD),
default_kff(initial_kFF),
default_kimax(initial_imax),
default_filt_E_hz(initial_filt_E_hz),
default_filt_D_hz(initial_filt_D_hz)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_kP);
_ki.set_and_default(initial_kI);
_kd.set_and_default(initial_kD);
_kff.set_and_default(initial_kFF);
_kimax.set_and_default(initial_imax);
_filt_E_hz.set_and_default(initial_filt_E_hz);
_filt_D_hz.set_and_default(initial_filt_D_hz);
// reset input filter to first value received
_reset_filter = true;
}

9
libraries/AC_PID/AC_PID_2D.h
View File

@ -98,4 +98,13 @@ protected:
AP_PIDInfo _pid_info_x;
AP_PIDInfo _pid_info_y;
private:
const float default_kp;
const float default_ki;
const float default_kd;
const float default_kff;
const float default_kimax;
const float default_filt_E_hz;
const float default_filt_D_hz;
};

33
libraries/AC_PID/AC_PID_Basic.cpp
View File

@ -5,66 +5,63 @@
#include <AP_InternalError/AP_InternalError.h>
#include "AC_PID_Basic.h"
#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PID_Basic_FILT_E_HZ_MIN 0.01f // minimum input filter frequency
#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency
#define AC_PID_Basic_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
const AP_Param::GroupInfo AC_PID_Basic::var_info[] = {
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_PID_Basic, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_Basic, _kp, default_kp),
// @Param: I
// @DisplayName: PID Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
AP_GROUPINFO("I", 1, AC_PID_Basic, _ki, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_Basic, _ki, default_ki),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 2, AC_PID_Basic, _kimax, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_Basic, _kimax, default_kimax),
// @Param: FLTE
// @DisplayName: PID Error filter frequency in Hz
// @Description: Error filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTE", 3, AC_PID_Basic, _filt_E_hz, AC_PID_Basic_FILT_E_HZ_DEFAULT),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_Basic, _filt_E_hz, default_filt_E_hz),
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
AP_GROUPINFO("D", 4, AC_PID_Basic, _kd, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_Basic, _kd, default_kd),
// @Param: FLTD
// @DisplayName: D term filter frequency in Hz
// @Description: D term filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTD", 5, AC_PID_Basic, _filt_D_hz, AC_PID_Basic_FILT_D_HZ_DEFAULT),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_Basic, _filt_D_hz, default_filt_D_hz),
// @Param: FF
// @DisplayName: PID Feed Forward Gain
// @Description: FF Gain which produces an output that is proportional to the magnitude of the target
AP_GROUPINFO("FF", 6, AC_PID_Basic, _kff, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_Basic, _kff, default_kff),
AP_GROUPEND
};
// Constructor
AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) :
default_kp(initial_p),
default_ki(initial_i),
default_kd(initial_d),
default_kff(initial_ff),
default_kimax(initial_imax),
default_filt_E_hz(initial_filt_E_hz),
default_filt_D_hz(initial_filt_D_hz)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_p);
_ki.set_and_default(initial_i);
_kd.set_and_default(initial_d);
_kff.set_and_default(initial_ff);
_kimax.set_and_default(initial_imax);
_filt_E_hz.set_and_default(initial_filt_E_hz);
_filt_D_hz.set_and_default(initial_filt_D_hz);
// reset input filter to first value received
_reset_filter = true;
}

9
libraries/AC_PID/AC_PID_Basic.h
View File

@ -91,4 +91,13 @@ protected:
bool _reset_filter; // true when input filter should be reset during next call to set_input
AP_PIDInfo _pid_info;
private:
const float default_kp;
const float default_ki;
const float default_kd;
const float default_kff;
const float default_kimax;
const float default_filt_E_hz;
const float default_filt_D_hz;
};

18
libraries/AC_PID/AC_PI_2D.cpp
View File

@ -8,38 +8,38 @@ const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
// @Param: P
// @DisplayName: PI Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_PI_2D, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PI_2D, _kp, default_kp),
// @Param: I
// @DisplayName: PI Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
AP_GROUPINFO("I", 1, AC_PI_2D, _ki, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PI_2D, _ki, default_ki),
// @Param: IMAX
// @DisplayName: PI Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 2, AC_PI_2D, _imax, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PI_2D, _imax, default_imax),
// @Param: FILT_HZ
// @DisplayName: PI Input filter frequency in Hz
// @Description: Input filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FILT_HZ", 3, AC_PI_2D, _filt_hz, default_filt_hz),
AP_GROUPEND
};
// Constructor
AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) :
_dt(dt)
_dt(dt),
default_kp(initial_p),
default_ki(initial_i),
default_imax(initial_imax),
default_filt_hz(initial_filt_hz)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_p);
_ki.set_and_default(initial_i);
_imax.set_and_default(initial_imax);
_filt_hz.set_and_default(initial_filt_hz);
filt_hz(initial_filt_hz);
// reset input filter to first value received

7
libraries/AC_PID/AC_PI_2D.h
View File

@ -8,7 +8,6 @@
#include <stdlib.h>
#include <cmath>
#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
/// @class AC_PI_2D
@ -92,4 +91,10 @@ private:
Vector2f _integrator; // integrator value
Vector2f _input; // last input for derivative
float _filt_alpha; // input filter alpha
const float default_kp;
const float default_ki;
const float default_imax;
const float default_filt_hz;
};

7
libraries/AC_PID/AC_P_1D.cpp
View File

@ -8,17 +8,16 @@ const AP_Param::GroupInfo AC_P_1D::var_info[] = {
// @Param: P
// @DisplayName: P Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_P_1D, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P_1D, _kp, default_kp),
AP_GROUPEND
};
// Constructor
AC_P_1D::AC_P_1D(float initial_p)
AC_P_1D::AC_P_1D(float initial_p) :
default_kp(initial_p)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_p);
}
// update_all - set target and measured inputs to P controller and calculate outputs

2
libraries/AC_PID/AC_P_1D.h
View File

@ -57,4 +57,6 @@ private:
float _error_min; // error limit in negative direction
float _error_max; // error limit in positive direction
float _D1_max; // maximum first derivative of output
const float default_kp;
};

7
libraries/AC_PID/AC_P_2D.cpp
View File

@ -8,17 +8,16 @@ const AP_Param::GroupInfo AC_P_2D::var_info[] = {
// @Param: P
// @DisplayName: Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_P_2D, _kp, 0),
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P_2D, _kp, default_kp),
AP_GROUPEND
};
// Constructor
AC_P_2D::AC_P_2D(float initial_p)
AC_P_2D::AC_P_2D(float initial_p) :
default_kp(initial_p)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp.set_and_default(initial_p);
}
// update_all - set target and measured inputs to P controller and calculate outputs

2
libraries/AC_PID/AC_P_2D.h
View File

@ -58,4 +58,6 @@ private:
Vector2f _error; // time step in seconds
float _error_max; // error limit in positive direction
float _D1_max; // maximum first derivative of output
const float default_kp;
};