mirror of https://github.com/ArduPilot/ardupilot
More syntax, etc. fixes. Compiles OK now.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@932 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
dc40ee00ae
commit
52abef58d5
|
@ -3,6 +3,7 @@
|
||||||
/// @file PID.cpp
|
/// @file PID.cpp
|
||||||
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
#include "PID.h"
|
#include "PID.h"
|
||||||
|
|
||||||
long
|
long
|
||||||
|
@ -37,7 +38,11 @@ PID::get_pid(long error, long dt, float scaler)
|
||||||
// Compute integral component if time has elapsed
|
// Compute integral component if time has elapsed
|
||||||
if (_ki && (dt > 0)) {
|
if (_ki && (dt > 0)) {
|
||||||
_integrator += (error * _ki) * scaler * delta_time;
|
_integrator += (error * _ki) * scaler * delta_time;
|
||||||
_integrator = constrain(_integrator, -_imax, _imax);
|
if (_integrator < -_imax) {
|
||||||
|
_integrator = -_imax;
|
||||||
|
} else if (_integrator > _imax) {
|
||||||
|
_integrator = _imax;
|
||||||
|
}
|
||||||
output += _integrator;
|
output += _integrator;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -45,11 +50,9 @@ PID::get_pid(long error, long dt, float scaler)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
PID::reset_I(void)
|
PID::imax(float v)
|
||||||
{
|
{
|
||||||
_integrator = 0;
|
_imax = fabs(v);
|
||||||
_last_error = 0;
|
|
||||||
_last_error_derivative = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
|
@ -24,7 +24,7 @@ public:
|
||||||
_address(address),
|
_address(address),
|
||||||
_integrator(0),
|
_integrator(0),
|
||||||
_last_error(0),
|
_last_error(0),
|
||||||
_last_error_derivative(0)
|
_last_derivative(0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
|
@ -38,7 +38,7 @@ public:
|
||||||
_address(0),
|
_address(0),
|
||||||
_integrator(0),
|
_integrator(0),
|
||||||
_last_error(0),
|
_last_error(0),
|
||||||
_last_error_derivative(0)
|
_last_derivative(0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
/// Iterate the PID, return the new control value
|
/// Iterate the PID, return the new control value
|
||||||
|
@ -55,7 +55,11 @@ public:
|
||||||
|
|
||||||
/// Reset the PID integrator
|
/// 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
|
/// Load gain properties
|
||||||
///
|
///
|
||||||
|
@ -75,11 +79,11 @@ public:
|
||||||
void kP(float v) { _kp = v; }
|
void kP(float v) { _kp = v; }
|
||||||
void kI(float v) { _ki = v; }
|
void kI(float v) { _ki = v; }
|
||||||
void kD(float v) { _kd = v; }
|
void kD(float v) { _kd = v; }
|
||||||
void imax(float v) { _imax = v; }
|
void imax(float v);
|
||||||
//@}
|
//@}
|
||||||
|
|
||||||
private:
|
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 *_gain_array; ///< pointer to the gains for this pid
|
||||||
|
|
||||||
float _kp; ///< proportional gain
|
float _kp; ///< proportional gain
|
||||||
|
@ -89,13 +93,14 @@ private:
|
||||||
|
|
||||||
float _integrator; ///< integrator value
|
float _integrator; ///< integrator value
|
||||||
int32_t _last_error; ///< last error for derivative
|
int32_t _last_error; ///< last error for derivative
|
||||||
|
float _last_derivative; ///< last derivative for low-pass filter
|
||||||
|
|
||||||
/// low pass filter cut frequency
|
/// low pass filter cut frequency
|
||||||
/// for derivative calculation,
|
/// for derivative calculation,
|
||||||
/// set to 20 Hz becasue anything over that
|
/// set to 20 Hz becasue anything over that
|
||||||
/// is probably noise, see
|
/// is probably noise, see
|
||||||
/// http://en.wikipedia.org/wiki/Low-pass_filter
|
/// http://en.wikipedia.org/wiki/Low-pass_filter
|
||||||
static uint8_t _RC = 20;
|
static const uint8_t _RC = 20;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -20,10 +20,10 @@ void setup()
|
||||||
//rc.trim();
|
//rc.trim();
|
||||||
radio_trim = APM_RC.InputCh(0);
|
radio_trim = APM_RC.InputCh(0);
|
||||||
|
|
||||||
pid.kP() = 1;
|
pid.kP(1);
|
||||||
pid.kI() = 0;
|
pid.kI(0);
|
||||||
pid.kD() = 0.5;
|
pid.kD(0.5);
|
||||||
pid.imax() = 50;
|
pid.imax(50);
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
|
|
Loading…
Reference in New Issue