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

PID: Added PID_Info to class for PID logging

This commit is contained in:
Grant Morphett 2016-05-30 16:48:40 +10:00 committed by Andrew Tridgell
parent 35a132f74e
commit 944541b287
2 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/PID/PID.cpp
View File

@ -57,7 +57,8 @@ float PID::get_pid(float error, float scaler)
delta_time = (float)dt / 1000.0f; delta_time = (float)dt / 1000.0f;
// Compute proportional component // Compute proportional component
output += error * _kp; _pid_info.P = error * _kp;
output += _pid_info.P;
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if ((fabsf(_kd) > 0) && (dt > 0)) { if ((fabsf(_kd) > 0) && (dt > 0)) {
@ -85,11 +86,14 @@ float PID::get_pid(float error, float scaler)
_last_derivative = derivative; _last_derivative = derivative;
// add in derivative component // add in derivative component
output += _kd * derivative; _pid_info.D = _kd * derivative;
output += _pid_info.D;
} }
// scale the P and D components // scale the P and D components
output *= scaler; output *= scaler;
_pid_info.D *= scaler;
_pid_info.P *= scaler;
// Compute integral component if time has elapsed // Compute integral component if time has elapsed
if ((fabsf(_ki) > 0) && (dt > 0)) { if ((fabsf(_ki) > 0) && (dt > 0)) {
@ -99,9 +103,11 @@ float PID::get_pid(float error, float scaler)
} else if (_integrator > _imax) { } else if (_integrator > _imax) {
_integrator = _imax; _integrator = _imax;
} }
_pid_info.I = _integrator;
output += _integrator; output += _integrator;
} }
_pid_info.desired = output;
return output; return output;
} }
@ -118,6 +124,7 @@ PID::reset_I()
// we use NAN (Not A Number) to indicate that the last // we use NAN (Not A Number) to indicate that the last
// derivative value is not valid // derivative value is not valid
_last_derivative = NAN; _last_derivative = NAN;
_pid_info.I = 0;
} }
void void

5
libraries/PID/PID.h
View File

@ -6,6 +6,7 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <DataFlash/DataFlash.h>
#include <stdlib.h> #include <stdlib.h>
#include <cmath> #include <cmath>
@ -98,6 +99,8 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
private: private:
AP_Float _kp; AP_Float _kp;
AP_Float _ki; AP_Float _ki;
@ -111,6 +114,8 @@ private:
float _get_pid(float error, uint16_t dt, float scaler); float _get_pid(float error, uint16_t dt, float scaler);
DataFlash_Class::PID_Info _pid_info {};
/// Low pass filter cut frequency for derivative calculation. /// Low pass filter cut frequency for derivative calculation.
/// ///
/// 20 Hz because anything over that is probably noise, see /// 20 Hz because anything over that is probably noise, see