mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_PID: replace fabs() with fabsf()
This commit is contained in:
parent
70bef20a29
commit
9d74f57ed3
@ -52,7 +52,7 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
|
||||
_kp = initial_p;
|
||||
_ki = initial_i;
|
||||
_kd = initial_d;
|
||||
_imax = fabs(initial_imax);
|
||||
_imax = fabsf(initial_imax);
|
||||
filt_hz(initial_filt_hz);
|
||||
|
||||
// reset input filter to first value received
|
||||
@ -69,7 +69,7 @@ void AC_PID::set_dt(float dt)
|
||||
// filt_hz - set input filter hz
|
||||
void AC_PID::filt_hz(float hz)
|
||||
{
|
||||
_filt_hz.set(fabs(hz));
|
||||
_filt_hz.set(fabsf(hz));
|
||||
|
||||
// sanity check _filt_hz
|
||||
_filt_hz = max(_filt_hz, AC_PID_FILT_HZ_MIN);
|
||||
@ -171,7 +171,7 @@ void AC_PID::load_gains()
|
||||
_ki.load();
|
||||
_kd.load();
|
||||
_imax.load();
|
||||
_imax = fabs(_imax);
|
||||
_imax = fabsf(_imax);
|
||||
_filt_hz.load();
|
||||
}
|
||||
|
||||
@ -191,7 +191,7 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f
|
||||
_kp = p;
|
||||
_ki = i;
|
||||
_kd = d;
|
||||
_imax = fabs(imaxval);
|
||||
_imax = fabsf(imaxval);
|
||||
_filt_hz = input_filt_hz;
|
||||
_dt = dt;
|
||||
}
|
||||
|
@ -69,7 +69,7 @@ public:
|
||||
void kP(const float v) { _kp.set(v); }
|
||||
void kI(const float v) { _ki.set(v); }
|
||||
void kD(const float v) { _kd.set(v); }
|
||||
void imax(const float v) { _imax.set(fabs(v)); }
|
||||
void imax(const float v) { _imax.set(fabsf(v)); }
|
||||
void filt_hz(const float v);
|
||||
|
||||
float get_integrator() const { return _integrator; }
|
||||
|
@ -40,7 +40,7 @@ AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float i
|
||||
|
||||
_kp = initial_p;
|
||||
_ki = initial_i;
|
||||
_imax = fabs(initial_imax);
|
||||
_imax = fabsf(initial_imax);
|
||||
filt_hz(initial_filt_hz);
|
||||
|
||||
// reset input filter to first value received
|
||||
@ -58,7 +58,7 @@ void AC_PI_2D::set_dt(float dt)
|
||||
// filt_hz - set input filter hz
|
||||
void AC_PI_2D::filt_hz(float hz)
|
||||
{
|
||||
_filt_hz.set(fabs(hz));
|
||||
_filt_hz.set(fabsf(hz));
|
||||
|
||||
// sanity check _filt_hz
|
||||
_filt_hz = max(_filt_hz, AC_PI_2D_FILT_HZ_MIN);
|
||||
@ -136,7 +136,7 @@ void AC_PI_2D::load_gains()
|
||||
_kp.load();
|
||||
_ki.load();
|
||||
_imax.load();
|
||||
_imax = fabs(_imax);
|
||||
_imax = fabsf(_imax);
|
||||
_filt_hz.load();
|
||||
|
||||
// calculate the input filter alpha
|
||||
@ -157,7 +157,7 @@ void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz,
|
||||
{
|
||||
_kp = p;
|
||||
_ki = i;
|
||||
_imax = fabs(imaxval);
|
||||
_imax = fabsf(imaxval);
|
||||
_filt_hz = input_filt_hz;
|
||||
_dt = dt;
|
||||
// calculate the input filter alpha
|
||||
|
@ -62,7 +62,7 @@ public:
|
||||
// set accessors
|
||||
void kP(const float v) { _kp.set(v); }
|
||||
void kI(const float v) { _ki.set(v); }
|
||||
void imax(const float v) { _imax.set(fabs(v)); }
|
||||
void imax(const float v) { _imax.set(fabsf(v)); }
|
||||
void filt_hz(const float v);
|
||||
|
||||
Vector2f get_integrator() const { return _integrator; }
|
||||
|
Loading…
Reference in New Issue
Block a user