From c1708db208b0b4a534ca1e9fc031262f8f4b01af Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 20 Nov 2017 14:35:34 +0900 Subject: [PATCH] AC_PID: add const and use is_positive in AC_PI_2D --- libraries/AC_PID/AC_PI_2D.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index 900c207f7b..331d6f1734 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/libraries/AC_PID/AC_PI_2D.cpp @@ -93,10 +93,10 @@ Vector2f AC_PI_2D::get_p() const Vector2f AC_PI_2D::get_i() { - if(!is_zero(_ki) && !is_zero(_dt)) { + if (!is_zero(_ki) && !is_zero(_dt)) { _integrator += (_input * _ki) * _dt; - float integrator_length = _integrator.length(); - if ((integrator_length > _imax) && (integrator_length > 0)) { + const float integrator_length = _integrator.length(); + if ((integrator_length > _imax) && (is_positive(integrator_length))) { _integrator *= (_imax / integrator_length); } return _integrator; @@ -108,10 +108,10 @@ Vector2f AC_PI_2D::get_i() Vector2f AC_PI_2D::get_i_shrink() { if (!is_zero(_ki) && !is_zero(_dt)) { - float integrator_length_orig = MIN(_integrator.length(),_imax); + const float integrator_length_orig = MIN(_integrator.length(),_imax); _integrator += (_input * _ki) * _dt; - float integrator_length_new = _integrator.length(); - if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) { + const float integrator_length_new = _integrator.length(); + if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) { _integrator *= (integrator_length_orig / integrator_length_new); } return _integrator; @@ -171,6 +171,6 @@ void AC_PI_2D::calc_filt_alpha() } // calculate alpha - float rc = 1/(M_2PI*_filt_hz); + const float rc = 1/(M_2PI*_filt_hz); _filt_alpha = _dt / (_dt + rc); }