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

More syntax, etc. fixes. Compiles OK now.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@932 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-11-26 01:53:12 +00:00
parent dc40ee00ae
commit 52abef58d5
3 changed files with 23 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/PID/PID.cpp
View File

@ -3,6 +3,7 @@
/// @file PID.cpp
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#include <math.h>
#include "PID.h"
long
@ -37,7 +38,11 @@ PID::get_pid(long error, long dt, float scaler)
// Compute integral component if time has elapsed
if (_ki && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time;
_integrator = constrain(_integrator, -_imax, _imax);
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
output += _integrator;
}
@ -45,11 +50,9 @@ PID::get_pid(long error, long dt, float scaler)
}
void
PID::reset_I(void)
PID::imax(float v)
{
_integrator = 0;
_last_error = 0;
_last_error_derivative = 0;
_imax = fabs(v);
}
void

17
libraries/PID/PID.h
View File

@ -24,7 +24,7 @@ public:
_address(address),
_integrator(0),
_last_error(0),
_last_error_derivative(0)
_last_derivative(0)
{}
/// Constructor
@ -38,7 +38,7 @@ public:
_address(0),
_integrator(0),
_last_error(0),
_last_error_derivative(0)
_last_derivative(0)
{}
/// Iterate the PID, return the new control value
@ -55,7 +55,11 @@ public:
/// Reset the PID integrator
///
void reset_I() { _integrator = 0; _last_error = 0; }
void reset_I() {
_integrator = 0;
_last_error = 0;
_last_derivative = 0;
}
/// Load gain properties
///
@ -75,11 +79,11 @@ public:
void kP(float v) { _kp = v; }
void kI(float v) { _ki = v; }
void kD(float v) { _kd = v; }
void imax(float v) { _imax = v; }
void imax(float v);
//@}
private:
uint16_t _address ///< EEPROM address for save/restore of P/I/D
uint16_t _address; ///< EEPROM address for save/restore of P/I/D
float *_gain_array; ///< pointer to the gains for this pid
float _kp; ///< proportional gain
@ -89,13 +93,14 @@ private:
float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter
/// low pass filter cut frequency
/// for derivative calculation,
/// set to 20 Hz becasue anything over that
/// is probably noise, see
/// http://en.wikipedia.org/wiki/Low-pass_filter
static uint8_t _RC = 20;
static const uint8_t _RC = 20;
};
#endif

8
libraries/PID/examples/pid/pid.pde
View File

@ -20,10 +20,10 @@ void setup()
//rc.trim();
radio_trim = APM_RC.InputCh(0);
pid.kP() = 1;
pid.kI() = 0;
pid.kD() = 0.5;
pid.imax() = 50;
pid.kP(1);
pid.kI(0);
pid.kD(0.5);
pid.imax(50);
}
void loop()