Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -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 "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[] = {
// @Param: P
// @DisplayName: PID Proportional Gain
@ -109,7 +114,7 @@ void AC_PID_2D::set_input(const Vector2f &input)
// update filter and calculate derivative
const Vector2f input_delta = (input - _input) * _filt_alpha;
_input = _input + input_delta;
_input += 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
// 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 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
if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) {
return;
}
// reset input filter to value received
// reset input filter
if (_flags._reset_filter) {
_flags._reset_filter = false;
_derivative.x = 0.0f;
@ -133,8 +138,8 @@ void AC_PID_2D::set_input_filter_d(Vector2f input_delta)
// update filter and calculate derivative
if (is_positive(_dt)) {
Vector2f derivative = input_delta / _dt;
Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d;
const Vector2f derivative = input_delta / _dt;
const Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d;
_derivative += delta_derivative;
}
}
@ -214,21 +219,6 @@ void AC_PID_2D::save_gains()
_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
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 <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
/// @brief Copter PID control class
class AC_PID_2D {
@ -49,9 +44,6 @@ public:
// save gain to eeprom
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
AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; }
@ -81,7 +73,7 @@ protected:
// 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(Vector2f input_delta);
void set_input_filter_d(const Vector2f& input_delta);
// calc_filt_alpha - recalculate the input filter alpha
void calc_filt_alpha();