From 937f22d70871c8e7b85f82101a81df45507743ea Mon Sep 17 00:00:00 2001
From: Bill Geyer <bnsgeyer@aol.com>
Date: Mon, 14 Mar 2022 22:53:57 -0400
Subject: [PATCH] Copter: make systemid use new chirp math function

---
 ArduCopter/mode.h            |  4 +++-
 ArduCopter/mode_systemid.cpp | 44 ++++--------------------------------
 2 files changed, 7 insertions(+), 41 deletions(-)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 721db3a025..782c7934fa 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Copter.h"
+#include <AP_Math/chirp.h>
 class Parameters;
 class ParametersG2;
 
@@ -1481,6 +1482,8 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    Chirp chirp_input;
+
 protected:
 
     const char *name() const override { return "SYSTEMID"; }
@@ -1489,7 +1492,6 @@ protected:
 private:
 
     void log_data() const;
-    float waveform(float time);
 
     enum class AxisType {
         NONE = 0,           // none
diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp
index 63ac09a1e1..0ce41da15a 100644
--- a/ArduCopter/mode_systemid.cpp
+++ b/ArduCopter/mode_systemid.cpp
@@ -95,6 +95,8 @@ bool ModeSystemId::init(bool ignore_checks)
     systemid_state = SystemIDModeState::SYSTEMID_STATE_TESTING;
     log_subsample = 0;
 
+    chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
+
     gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis);
 
     copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out);
@@ -172,7 +174,8 @@ void ModeSystemId::run()
     }
 
     waveform_time += G_Dt;
-    waveform_sample = waveform(waveform_time - SYSTEM_ID_DELAY);
+    waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
+    waveform_freq_rads = chirp_input.get_frequency_rads();
 
     switch (systemid_state) {
         case SystemIDModeState::SYSTEMID_STATE_STOPPED:
@@ -291,43 +294,4 @@ void ModeSystemId::log_data() const
     copter.Log_Write_Attitude();
 }
 
-// init_test - initialises the test
-float ModeSystemId::waveform(float time)
-{
-    float wMin = 2 * M_PI * frequency_start;
-    float wMax = 2 * M_PI * frequency_stop;
-
-    float window;
-    float output;
-
-    float B = logf(wMax / wMin);
-
-    if (time <= 0.0f) {
-        window = 0.0f;
-    } else if (time <= time_fade_in) {
-        window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in);
-    } else if (time <= time_record - time_fade_out) {
-        window = 1.0;
-    } else if (time <= time_record) {
-        window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI);
-    } else {
-        window = 0.0;
-    }
-
-    if (time <= 0.0f) {
-        waveform_freq_rads = wMin;
-        output = 0.0f;
-    } else if (time <= time_const_freq) {
-        waveform_freq_rads = wMin;
-        output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq);
-    } else if (time <= time_record) {
-        waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq));
-        output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1));
-    } else {
-        waveform_freq_rads = wMax;
-        output = 0.0f;
-    }
-    return output;
-}
-
 #endif