From 67024a95161dec35241869e81da22a57f53f02f2 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sun, 4 Dec 2022 21:05:37 +1030
Subject: [PATCH] AP_Math: Support changing update period

---
 libraries/AP_Math/AP_Math.cpp         | 13 ++++++++++---
 libraries/AP_Math/control.cpp         | 18 +++++++++++-------
 libraries/AP_Math/tests/test_math.cpp |  4 +---
 3 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index d684eee755..f76b64755a 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -400,11 +400,18 @@ Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf,
  */
 float calc_lowpass_alpha_dt(float dt, float cutoff_freq)
 {
-    if (dt <= 0.0f || cutoff_freq <= 0.0f) {
+    if (is_negative(dt) || is_negative(cutoff_freq)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
         return 1.0;
     }
-    float rc = 1.0f/(M_2PI*cutoff_freq);
-    return dt/(dt+rc);
+    if (is_zero(cutoff_freq)) {
+        return 1.0;
+    }
+    if (is_zero(dt)) {
+        return 0.0;
+    }
+    float rc = 1.0f / (M_2PI * cutoff_freq);
+    return dt / (dt + rc);
 }
 
 #ifndef AP_MATH_FILL_NANF_USE_MEMCPY
diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp
index bfabe1182d..a0995167d7 100644
--- a/libraries/AP_Math/control.cpp
+++ b/libraries/AP_Math/control.cpp
@@ -112,9 +112,11 @@ void shape_accel(float accel_input, float& accel,
     }
 
     // jerk limit acceleration change
-    float accel_delta = accel_input - accel;
-    accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
-    accel += accel_delta;
+    if (is_positive(dt)) {
+        float accel_delta = accel_input - accel;
+        accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
+        accel += accel_delta;
+    }
 }
 
 // 2D version
@@ -128,9 +130,11 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
     }
 
     // jerk limit acceleration change
-    Vector2f accel_delta = accel_input - accel;
-    accel_delta.limit_length(jerk_max * dt);
-    accel = accel + accel_delta;
+    if (is_positive(dt)) {
+        Vector2f accel_delta = accel_input - accel;
+        accel_delta.limit_length(jerk_max * dt);
+        accel = accel + accel_delta;
+    }
 }
 
 void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
@@ -410,7 +414,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt)
             correction_rate = error * p;
         }
     }
-    if (!is_zero(dt)) {
+    if (is_positive(dt)) {
         // this ensures we do not get small oscillations by over shooting the error correction in the last time step.
         return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
     } else {
diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp
index fb52cdbe04..ccc959d2fe 100644
--- a/libraries/AP_Math/tests/test_math.cpp
+++ b/libraries/AP_Math/tests/test_math.cpp
@@ -665,10 +665,8 @@ TEST(MathTest, VELCORRECTION)
 TEST(MathTest, LOWPASSALPHA)
 {
     const float accuracy = 1.0e-5f;
-    EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(0.0f, 2.0f));
-    EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(-1.0f, 2.0f));
+    EXPECT_EQ(0.0f, calc_lowpass_alpha_dt(0.0f, 2.0f));
     EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, 0.0f));
-    EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, -2.0f));
 
     EXPECT_NEAR(0.926288f, calc_lowpass_alpha_dt(1.0f, 2.0f), accuracy);
 }