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 . + */ + +/* +* 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 +#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; + +};