From 52abef58d5c538ad4e2e7917e64de579723ea173 Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Fri, 26 Nov 2010 01:53:12 +0000 Subject: [PATCH] More syntax, etc. fixes. Compiles OK now. git-svn-id: https://arducopter.googlecode.com/svn/trunk@932 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/PID/PID.cpp | 13 ++++++++----- libraries/PID/PID.h | 17 +++++++++++------ libraries/PID/examples/pid/pid.pde | 8 ++++---- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 330e950049..152ee174a8 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -3,6 +3,7 @@ /// @file PID.cpp /// @brief Generic PID algorithm, with EEPROM-backed storage of constants. +#include #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 diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 82298c9d03..99c25f75ca 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -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 diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index 249c05d653..a86d8fcad1 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -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()