PID: added get_pid_4500()
this is a version of get_pid() that returns an int16_t constrained to -4500 to 4500. This will prevent overflow errors for large PID gains in ArduPlane and Rover
This commit is contained in:
parent
624ebced38
commit
6123ea2dac
@ -88,6 +88,12 @@ float PID::get_pid(float error, float scaler)
|
|||||||
return output;
|
return output;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int16_t PID::get_pid_4500(float error, float scaler)
|
||||||
|
{
|
||||||
|
float v = get_pid(error, scaler);
|
||||||
|
return constrain(v, -4500, 4500);
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
PID::reset_I()
|
PID::reset_I()
|
||||||
{
|
{
|
||||||
|
@ -42,6 +42,9 @@ public:
|
|||||||
///
|
///
|
||||||
float get_pid(float error, float scaler = 1.0);
|
float get_pid(float error, float scaler = 1.0);
|
||||||
|
|
||||||
|
// get_pid() constrained to +/- 4500
|
||||||
|
int16_t get_pid_4500(float error, float scaler = 1.0);
|
||||||
|
|
||||||
/// Reset the PID integrator
|
/// Reset the PID integrator
|
||||||
///
|
///
|
||||||
void reset_I();
|
void reset_I();
|
||||||
|
Loading…
Reference in New Issue
Block a user