AC_PID: add const and use is_positive in AC_PI_2D

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();
+        const float integrator_length = _integrator.length();
-        if ((integrator_length > _imax) && (integrator_length > 0)) {
+        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();
+        const float integrator_length_new = _integrator.length();
-        if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) {
+        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);
 }