Line data    Source code

       1             : /*
       2             :  * chirp.cpp
       3             :  *
       4             :  * Copyright (C) Leonard Hall 2020
       5             :  *
       6             :  * This file is free software: you can redistribute it and/or modify it
       7             :  * under the terms of the GNU General Public License as published by the
       8             :  * Free Software Foundation, either version 3 of the License, or
       9             :  * (at your option) any later version.
      10             :  *
      11             :  * This file is distributed in the hope that it will be useful, but
      12             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
      13             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      14             :  * See the GNU General Public License for more details.
      15             :  *
      16             :  * You should have received a copy of the GNU General Public License along
      17             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      18             :  */
      19             : 
      20             : /*
      21             : * This object generates a chirp signal based on the input variables.  A chirp is
      22             : * a sine wave starting from the minimum frequency and ending at the maximum frequency.
      23             : * The progression in frequency is not linear but designed to increase exponentially.
      24             : * The chirp can be designed to dwell at the minimum frequency for a specified time
      25             : * before sweeping through the frequencies and also can fade in the magnitude at the
      26             : * beginning and fade out the magnitude at the end.  This object can also generate
      27             : * constant frequency sine waves by setting the minimum and maximum frequency to the
      28             : * same value.
      29             : */
      30             : #include <AP_Math/AP_Math.h>
      31             : #include "chirp.h"
      32             : 
      33             : // constructor
      34        1776 : Chirp::Chirp() {}
      35             : 
      36             : // initializes the chirp object
      37         147 : 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)
      38             : {
      39             :     // pass in variables to class
      40         147 :     record = time_record;
      41         147 :     wMin = M_2PI * frequency_start_hz;
      42         147 :     wMax = M_2PI * frequency_stop_hz;
      43         147 :     fade_in = time_fade_in;
      44         147 :     fade_out = time_fade_out;
      45         147 :     const_freq = time_const_freq;
      46             : 
      47         147 :     B = logf(wMax / wMin);
      48             : 
      49             :     // Mark as incomplete
      50         147 :     complete = false;
      51         147 : }
      52             : 
      53             : // determine chirp signal output at the specified time and amplitude
      54      521304 : float Chirp::update(float time, float waveform_magnitude)
      55             : {
      56      521304 :     magnitude = waveform_magnitude;
      57      521304 :     if (time <= 0.0f) {
      58           0 :         window = 0.0f;
      59      521304 :     } else if (time <= fade_in) {
      60           0 :         window = 0.5 - 0.5 * cosf(M_PI * time / fade_in);
      61      521304 :     } else if (time <= record - fade_out) {
      62      514103 :         window = 1.0;
      63        7201 :     } else if (time <= record) {
      64        7201 :         window = 0.5 - 0.5 * cosf(M_PI * (time - (record - fade_out)) / fade_out + M_PI);
      65             :     } else {
      66           0 :         window = 0.0;
      67             :     }
      68             : 
      69      521304 :     if (time <= 0.0f) {
      70           0 :         waveform_freq_rads = wMin;
      71           0 :         output = 0.0f;
      72      521304 :     } else if (time <= const_freq) {
      73           0 :         waveform_freq_rads = wMin;
      74           0 :         output = window * magnitude * sinf(wMin * time - wMin * const_freq);
      75      521304 :     } else if (time <= record) {
      76             :         // handles constant frequency dwells and chirps
      77      521304 :         if (is_equal(wMin, wMax)) {
      78      439583 :             waveform_freq_rads = wMin;
      79      439583 :             output = window * magnitude * sinf(wMin * time);
      80             :         } else {
      81       81721 :             waveform_freq_rads = wMin * expf(B * (time - const_freq) / (record - const_freq));
      82       81721 :             output = window * magnitude * sinf((wMin * (record - const_freq) / B) * (expf(B * (time - const_freq) / (record - const_freq)) - 1));
      83             :         }
      84             :     } else {
      85           0 :         waveform_freq_rads = wMax;
      86           0 :         output = 0.0f;
      87             :     }
      88             : 
      89      521304 :     complete = time > record;
      90             : 
      91      521304 :     return output;
      92             : }