Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: fixes after peer review of AC_PID_2D

Browse Source
This commit is contained in:
Randy Mackay 2018-01-22 11:41:46 +09:00
parent efbd8eb386
commit 04822152da
2 changed files with 11 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -4,6 +4,11 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include "AC_PID_2D.h" #include "AC_PID_2D.h"
#define AC_PID_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PID_2D_FILT_HZ_MIN 0.01f // minimum 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[] = { const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
// @Param: P // @Param: P
// @DisplayName: PID Proportional Gain // @DisplayName: PID Proportional Gain
@ -109,7 +114,7 @@ void AC_PID_2D::set_input(const Vector2f &input)
// update filter and calculate derivative // update filter and calculate derivative
const Vector2f input_delta = (input - _input) * _filt_alpha; const Vector2f input_delta = (input - _input) * _filt_alpha;
_input = _input + input_delta; _input += input_delta;
set_input_filter_d(input_delta); set_input_filter_d(input_delta);
} }
@ -117,14 +122,14 @@ void AC_PID_2D::set_input(const Vector2f &input)
// set_input_filter_d - set input to PID controller // set_input_filter_d - set input to PID controller
// only input to the D portion of the controller is filtered // 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 // this should be called before any other calls to get_p, get_i or get_d
void AC_PID_2D::set_input_filter_d(Vector2f input_delta) void AC_PID_2D::set_input_filter_d(const Vector2f& input_delta)
{ {
// don't process inf or NaN // don't process inf or NaN
if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) { if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) {
return; return;
} }
// reset input filter to value received // reset input filter
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;
_derivative.x = 0.0f; _derivative.x = 0.0f;
@ -133,8 +138,8 @@ void AC_PID_2D::set_input_filter_d(Vector2f input_delta)
// update filter and calculate derivative // update filter and calculate derivative
if (is_positive(_dt)) { if (is_positive(_dt)) {
Vector2f derivative = input_delta / _dt; const Vector2f derivative = input_delta / _dt;
Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d; const Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d;
_derivative += delta_derivative; _derivative += delta_derivative;
} }
} }
@ -214,21 +219,6 @@ void AC_PID_2D::save_gains()
_filt_d_hz.save(); _filt_d_hz.save();
} }
/// Overload the function call operator to permit easy initialisation
void AC_PID_2D::operator() (float p, float i, float imaxval, float input_filt_hz, float input_filt_d_hz, float dt)
{
_kp = p;
_ki = i;
_kd = i;
_imax = fabsf(imaxval);
_filt_hz = input_filt_hz;
_filt_d_hz = input_filt_d_hz;
_dt = dt;
// calculate the input filter alpha
calc_filt_alpha();
calc_filt_alpha_d();
}
// calc_filt_alpha - recalculate the input filter alpha // calc_filt_alpha - recalculate the input filter alpha
void AC_PID_2D::calc_filt_alpha() void AC_PID_2D::calc_filt_alpha()
{ {

10
libraries/AC_PID/AC_PID_2D.h
View File

@ -8,11 +8,6 @@
#include <stdlib.h> #include <stdlib.h>
#include <cmath> #include <cmath>
#define AC_PID_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency
#define AC_PID_2D_FILT_HZ_MIN 0.01f // minimum 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
/// @class AC_PID_2D /// @class AC_PID_2D
/// @brief Copter PID control class /// @brief Copter PID control class
class AC_PID_2D { class AC_PID_2D {
@ -49,9 +44,6 @@ public:
// save gain to eeprom // save gain to eeprom
void save_gains(); void save_gains();
/// operator function call for easy initialisation
void operator() (float p, float i, float imaxval, float input_filt_hz, float input_filt_D_hz, float dt);
// get accessors // get accessors
AP_Float &kP() { return _kp; } AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; } AP_Float &kI() { return _ki; }
@ -81,7 +73,7 @@ protected:
// set_input_filter_d - set input to PID controller // set_input_filter_d - set input to PID controller
// only input to the D portion of the controller is filtered // 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 // this should be called before any other calls to get_p, get_i or get_d
void set_input_filter_d(Vector2f input_delta); void set_input_filter_d(const Vector2f& input_delta);
// calc_filt_alpha - recalculate the input filter alpha // calc_filt_alpha - recalculate the input filter alpha
void calc_filt_alpha(); void calc_filt_alpha();