From 152c98831b421ccaaa9502f3278e9ba6bfe9c923 Mon Sep 17 00:00:00 2001
From: Bill Geyer <bnsgeyer@aol.com>
Date: Wed, 9 Mar 2022 00:00:49 -0500
Subject: [PATCH] AP_Math: add chirp function to support frequency sweeps in
 systemid and autotune

---
 libraries/AP_Math/chirp.cpp | 87 +++++++++++++++++++++++++++++++++++++
 libraries/AP_Math/chirp.h   | 53 ++++++++++++++++++++++
 2 files changed, 140 insertions(+)
 create mode 100644 libraries/AP_Math/chirp.cpp
 create mode 100644 libraries/AP_Math/chirp.h

diff --git a/libraries/AP_Math/chirp.cpp b/libraries/AP_Math/chirp.cpp
new file mode 100644
index 0000000000..b01250bca6
--- /dev/null
+++ b/libraries/AP_Math/chirp.cpp
@@ -0,0 +1,87 @@
+/*
+ * chirp.cpp
+ *
+ * Copyright (C) Leonard Hall 2020
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+* This object generates a chirp signal based on the input variables.  A chirp is
+* a sine wave starting from the minimum frequency and ending at the maximum frequency.
+* The progression in frequency is not linear but designed to increase exponentially.
+* The chirp can be designed to dwell at the minimum frequency for a specified time
+* before sweeping through the frequencies and also can fade in the magnitude at the
+* beginning and fade out the magnitude at the end.  This object can also generate
+* constant frequency sine waves by setting the minimum and maximum frequency to the
+* same value.
+*/
+#include <AP_Math/AP_Math.h>
+#include "chirp.h"
+
+// constructor
+Chirp::Chirp() {}
+
+// initializes the chirp object
+void Chirp::init(float time_record, float frequency_start_hz, float frequency_stop_hz, float time_fade_in, float time_fade_out, float time_const_freq)
+{
+    // pass in variables to class
+    record = time_record;
+    wMin = M_2PI * frequency_start_hz;
+    wMax = M_2PI * frequency_stop_hz;
+    fade_in = time_fade_in;
+    fade_out = time_fade_out;
+    const_freq = time_const_freq;
+
+    B = logf(wMax / wMin);
+
+}
+
+// determine chirp signal output at the specified time and amplitude
+float Chirp::update(float time, float waveform_magnitude)
+{
+    magnitude = waveform_magnitude;
+    if (time <= 0.0f) {
+        window = 0.0f;
+    } else if (time <= fade_in) {
+        window = 0.5 - 0.5 * cosf(M_PI * time / fade_in);
+    } else if (time <= record - fade_out) {
+        window = 1.0;
+    } else if (time <= record) {
+        window = 0.5 - 0.5 * cosf(M_PI * (time - (record - fade_out)) / fade_out + M_PI);
+    } else {
+        window = 0.0;
+    }
+
+    if (time <= 0.0f) {
+        waveform_freq_rads = wMin;
+        output = 0.0f;
+    } else if (time <= const_freq) {
+        waveform_freq_rads = wMin;
+        output = window * magnitude * sinf(wMin * time - wMin * const_freq);
+    } else if (time <= record) {
+        // handles constant frequency dwells and chirps
+        if (is_equal(wMin, wMax)) {
+            waveform_freq_rads = wMin;
+            output = window * magnitude * sinf(wMin * time);
+        } else {
+            waveform_freq_rads = wMin * expf(B * (time - const_freq) / (record - const_freq));
+            output = window * magnitude * sinf((wMin * (record - const_freq) / B) * (expf(B * (time - const_freq) / (record - const_freq)) - 1));
+        }
+    } else {
+        waveform_freq_rads = wMax;
+        output = 0.0f;
+    }
+    return output;
+}
diff --git a/libraries/AP_Math/chirp.h b/libraries/AP_Math/chirp.h
new file mode 100644
index 0000000000..23487e21e5
--- /dev/null
+++ b/libraries/AP_Math/chirp.h
@@ -0,0 +1,53 @@
+#pragma once
+
+class Chirp {
+
+public:
+
+    // constructor
+    Chirp();
+
+    // initializes the chirp object
+    void init(float time_record, float frequency_start_hz, float frequency_stop_hz, float time_fade_in, float time_fade_out, float time_const_freq);
+
+    // determine chirp signal output at the specified time and amplitude
+    float update(float time, float waveform_magnitude);
+
+    // accessor for the current waveform frequency
+    float get_frequency_rads() {return waveform_freq_rads; }
+
+private:
+    // Total chirp length in seconds
+    float record;
+
+    // Chirp oscillation amplitude
+    float magnitude;
+
+    // Chirp start frequency in rad/s
+    float wMin;
+
+    // Chirp end frequency in rad/s
+    float wMax;
+
+    // Amplitude fade in time in seconds
+    float fade_in;
+
+    // Amplitude fade out time in seconds
+    float fade_out;
+
+    // Time that chirp will remain at the min frequency before increasing to max frequency
+    float const_freq;
+
+    // frequency ratio
+    float B;
+
+    // current waveform frequency in rad/s
+    float waveform_freq_rads;
+
+    // current amplitude of chirp
+    float window;
+
+    // output of chirp signal at the requested time
+    float output;
+
+};