aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PID: replace set_filt_hz method with filt_hz

Browse Source 
Thanks to Jonathan Challinger for spotting this bug
Also add sanity check to filt_hz
This commit is contained in:
Randy Mackay 2015-03-11 14:16:26 +09:00
parent 88ec13b10d
commit c10b0b34ca
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_PID/AC_PID.cpp
View File

@ -5,6 +5,7 @@
#include <AP_Math.h>
#include "AC_PID.h"
#include <stdio.h>
const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = {
// @Param: P
@ -53,13 +54,10 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
_ki = initial_i;
_kd = initial_d;
_imax = fabs(initial_imax);
_filt_hz = initial_filt_hz;
filt_hz(initial_filt_hz);
// reset input filter to first value received
_flags._reset_filter = true;
// calculate the input filter alpha
calc_filt_alpha();
}
// set_dt - set time step in seconds
@ -70,10 +68,14 @@ void AC_PID::set_dt(float dt)
calc_filt_alpha();
}
// set_filt_hz - set input filter hz
void AC_PID::set_filt_hz(float hz)
// filt_hz - set input filter hz
void AC_PID::filt_hz(float hz)
{
_filt_hz.set(hz);
_filt_hz.set(fabs(hz));
// sanity check _filt_hz
_filt_hz = max(_filt_hz, AC_PID_FILT_HZ_MIN);
// calculate the input filter alpha
calc_filt_alpha();
}
@ -198,7 +200,7 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f
// calc_filt_alpha - recalculate the input filter alpha
void AC_PID::calc_filt_alpha()
{
{
// calculate alpha
float rc = 1/(2*PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);

6
libraries/AC_PID/AC_PID.h
View File

@ -12,6 +12,7 @@
#include <math.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
@ -24,9 +25,6 @@ public:
// set_dt - set time step in seconds
void set_dt(float dt);
// set_filt_hz - set input filter hz
void set_filt_hz(float hz);
// 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
@ -72,7 +70,7 @@ public:
void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); }
void imax(const float v) { _imax.set(fabs(v)); }
void filt_hz(const float v) { _filt_hz.set(fabs(v)); }
void filt_hz(const float v);
float get_integrator() const { return _integrator; }
void set_integrator(float i) { _integrator = i; }