parent
04621cb54e
commit
42419879ee
@ -0,0 +1,121 @@ |
||||
/*
|
||||
* analyze_CTCSS_F32.cpp Converted to float from PJRC Teensy Audio Library |
||||
* MIT License on changed portions |
||||
* Bob Larkin March 2021 |
||||
* |
||||
* Some parts of this cpp came from: Audio Library for Teensy 3.X |
||||
* Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com |
||||
* |
||||
* Development of this audio library was funded by PJRC.COM, LLC by sales of |
||||
* Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop |
||||
* open source software by purchasing Teensy or other PJRC products. |
||||
* |
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy |
||||
* of this software and associated documentation files (the "Software"), to deal |
||||
* in the Software without restriction, including without limitation the rights |
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
||||
* copies of the Software, and to permit persons to whom the Software is |
||||
* furnished to do so, subject to the following conditions: |
||||
* |
||||
* The above copyright notice, development funding notice, and this permission |
||||
* notice shall be included in all copies or substantial portions of the Software. |
||||
* |
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
||||
* THE SOFTWARE. |
||||
*/ |
||||
/* From Pavel Rajmic (2021). Generalized Goertzel algorithm
|
||||
* https://www.mathworks.com/matlabcentral/fileexchange/35103-generalized-goertzel-algorithm
|
||||
* MATLAB Central File Exchange. Retrieved December 18, 2021. |
||||
*/ |
||||
|
||||
#include <Arduino.h> |
||||
#include "analyze_CTCSS_F32.h" |
||||
|
||||
void analyze_CTCSS_F32::update(void) { |
||||
audio_block_f32_t *block; |
||||
float gs0=0.0; |
||||
|
||||
block = receiveReadOnly_f32(); |
||||
if (!block) return; |
||||
if (!gEnabled) { |
||||
release(block); |
||||
return; |
||||
} |
||||
|
||||
// This process is working with frequencies of 254 Hz or less. Thus
|
||||
// it is beneficial to decimate before processing. The chosen
|
||||
// decimation rate is 16. For example, if the basic sample rate is 44.1 kHz,
|
||||
// the decimated rate is 44100/16=2756.25 Hz Before decimation, we
|
||||
// low pass filter to prevent alias problems. Returns 128 pts in block.
|
||||
arm_biquad_cascade_df1_f32(&iir_lpf_inst, block->data, |
||||
block->data, block->length); |
||||
|
||||
// And decimate, using the first sample, giving 128/16=8 samples to
|
||||
// be processed per block.
|
||||
// Create a small data block, normally 8, d16a[].
|
||||
for(int i=0; i<8; i++) |
||||
d16a[i] = *(block->data + 16*i); // Decimated sample, only every 16
|
||||
|
||||
// Filter down to 67-254Hz band, leaving result in d16a[];
|
||||
arm_biquad_cascade_df1_f32(&iir_bpf_inst, d16a, d16a, 8); |
||||
|
||||
// For reference measurement, only, null out the tone, creating d16b[].
|
||||
// This d16b signal path is not used for the Goertzel.
|
||||
arm_biquad_cascade_df1_f32(&iir_nulls_inst, d16a, d16b, 8); |
||||
|
||||
for(int i=0; i<8; i++) |
||||
{ |
||||
if(gCount++ < gLength) // Main Goertzel calculation
|
||||
{ |
||||
// Use decimated sample, only every 16
|
||||
gs0 = d16a[i] + gCoefficient*gs1 - gs2; |
||||
gs2 = gs1; |
||||
gs1 = gs0; |
||||
// Do reference channel power measurement of the 67 to 254 Hz
|
||||
// band, including nulls at tone frequency.
|
||||
powerSum += d16b[i]*d16b[i]; |
||||
powerSumCount++; |
||||
} |
||||
else // The last, special, calculation
|
||||
{ |
||||
gs0 = gCoefficient*gs1 - gs2; // Generalized non-integer freq Goertzel
|
||||
out1 = gs0 - gs1*ccRe; |
||||
out2 = - gs1*ccIm; |
||||
// At this point the phase is still in need of correction. But we
|
||||
// only use amplitude so, leave the phase as is.
|
||||
powerTone = 4.0f*(out1*out1 + out2*out2)/((float)gLength*(float)gLength); |
||||
// 2.0f*sqrtf(powerTone)/(float)gLength; <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
gs1 = 0.0; // Initialize to start new measurement
|
||||
gs2 = 0.0; |
||||
gCount = 0; |
||||
// The 2.04 factor makes the reference for sine waves the same as the Goertzel
|
||||
powerRef = 2.04f*powerSum/((float)powerSumCount); |
||||
powerSum = 0.0f; |
||||
powerSumCount = 0; |
||||
new_output = true; |
||||
} |
||||
} |
||||
// If the CTCSS tone is detected, the output is the original data block.
|
||||
// If silenced, zeros are returned.
|
||||
if(powerTone>threshAbs && powerTone>threshRel*powerRef) |
||||
transmit(block); |
||||
else |
||||
{ |
||||
for(int i = 0; i<128; i++) |
||||
*(block->data + i) = 0.0; |
||||
transmit(block); |
||||
} |
||||
release(block); |
||||
} |
||||
|
||||
analyze_CTCSS_F32::operator bool() { |
||||
float pThresh; |
||||
pThresh = ((float)gLength)*threshAbs; |
||||
pThresh *= pThresh; |
||||
return (powerTone >= pThresh); |
||||
} |
@ -0,0 +1,505 @@ |
||||
/*
|
||||
* analyze_CTCSS_F32.h For the OpenAudio_TeensyArduino library |
||||
* (floating point audio). |
||||
* MIT License on changed portions |
||||
* Bob Larkin March 2021 |
||||
* |
||||
* This is specific for the CTCSS tone system |
||||
* with tones in the 67.0 to 254.1 Hz range. |
||||
* |
||||
* Parts of this are based on analyze_toneDetect: Audio Library for Teensy 3.X |
||||
* Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com |
||||
* |
||||
* Development of this audio library was funded by PJRC.COM, LLC by sales of |
||||
* Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop |
||||
* open source software by purchasing Teensy or other PJRC products. |
||||
* |
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy |
||||
* of this software and associated documentation files (the "Software"), to deal |
||||
* in the Software without restriction, including without limitation the rights |
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
||||
* copies of the Software, and to permit persons to whom the Software is |
||||
* furnished to do so, subject to the following conditions: |
||||
* |
||||
* The above copyright notice, development funding notice, and this permission |
||||
* notice shall be included in all copies or substantial portions of the Software. |
||||
* |
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
||||
* THE SOFTWARE. |
||||
*/ |
||||
|
||||
/* The tone frequency is set by the function frequency(float freq, float tMeas).
|
||||
* The parameter freq is in Hz. The parameter tMeas is optional and is the |
||||
* measuring time for the Goertzel algorithm, measured in milliseconds. Sort |
||||
* tMeas, say 100, give fast response, but poor selectivity to adjacent tone |
||||
* frequencies. Long values, say 500 to 1000, are sluggish and for NBFM systems |
||||
* tend to omit initial words. The default for tMeas is 300 milliseconds. |
||||
* |
||||
* CTCSS ranges from 67.0 to 254.1 Hz |
||||
(Actual CTCSS use seems to be 77.0 to 203.5 Hz) |
||||
This object can be used on any frequency in the range. |
||||
But, the standard CTCSS channel numbers, designations and |
||||
frequencies, in Hz, are: |
||||
1 XZ 67 25 5A 156.7 |
||||
39 WZ 69.3 40 159.8 |
||||
2 XA 71.9 26 5B 162.2 |
||||
3 WA 74.4 41 165.5 |
||||
4 XB 77 27 6Z 167.9 |
||||
5 WB 79.7 42 171.3 |
||||
6 YZ 82.5 28 6A 173.8 |
||||
7 YA 85.4 43 177.3 |
||||
8 YB 88.5 29 6B 179.9 |
||||
9 ZZ 91.5 44 183.5 |
||||
10 ZA 94.8 30 7Z 186.2 |
||||
11 ZB 97.4 45 189.9 |
||||
12 1Z 100 31 7A 192.8 |
||||
13 1A 103.5 46 196.6 |
||||
14 1B 107.2 47 199.5 |
||||
15 2Z 110.9 32 M1 203.5 |
||||
16 2A 114.8 48 8Z 206.5 |
||||
17 2B 118.8 33 M2 210.7 |
||||
18 3Z 123 34 M3 218.1 |
||||
19 3A 127.3 35 M4 225.7 |
||||
20 3B 131.8 49 9Z 229.1 |
||||
21 4Z 136.5 36 M5 233.6 |
||||
22 4A 141.3 37 M6 241.8 |
||||
23 4B 146.2 38 M7 250.3 |
||||
24 5Z 151.4 50 0Z 254.1 |
||||
*/ |
||||
|
||||
/* FUTURE (Let's see this all working without windowing, first):
|
||||
* This CTCSS detector has an optional windowing function. The default is |
||||
* no windowing, but this leaves the first Goertzel filter |
||||
* sideband at about -13 dB relative to the tone frequency response. See |
||||
* setGWindow(float *pWinCoeff) below. The caller supplied window function is |
||||
* an array of NWINDOW floats corresponding to half of the window |
||||
* function, starting with the outside (non-center) coefficient. The number |
||||
* of coefficients depends on the sample rate of the ADC and the amount |
||||
* time to make a detector measurement. The following might be used in an |
||||
* .ino to set the size of the half array. This is in pre-compiler lingo |
||||
* as it needs to be installed at compile time, not run time: |
||||
* #define SAMPLE_RATE 44117.0f |
||||
* #define DETECTOR_TIME 300.0f |
||||
* #define NWINDOW (uint16_t)( 0.5 + SAMPLE_RATE * DETECTOR_TIME / 32000.0f ) |
||||
* See example/ToneDetect2. Note that DETECTOR_TIME is in milliseconds. |
||||
*/ |
||||
|
||||
/* This class can be used to see if a sub-audible tone is present with resulting
|
||||
* action coming through the .ino. This uses available() and readTonePresent(). |
||||
* Alternatively, the threshold(s) can be set |
||||
* and the comparison results used to gate the input signal through to other |
||||
* audio blocks. Done this way, the audio gating is done without |
||||
* any .ino processing. |
||||
* |
||||
* Note - Both the Goertzel tone output and the reference band outputs are in |
||||
* "power." This is convenient and meaningful for the CTCSS detector, but |
||||
* different than the Teensy library tone detectors, which take the square |
||||
* root of the power for output. |
||||
* |
||||
* Two outputs are available, TonePower and RefPower. TonePower is the output |
||||
* of the Goertzel sharply tuned filter and is the conventional CTCSS output |
||||
* that is compared to a threshold. RefPower measures the power in the entire |
||||
* 67 to 254 Hz sub-audible band, except at the tone frequency. This is a measure |
||||
* of both FM quieting and also the presence of a tone away from the TonePower |
||||
* frequency. By comparing TonePower to RefPower several dB of increased |
||||
* sensitivity is achieved for threshold setting. |
||||
* |
||||
* Thresholds are used in two ways. FirstThey can be read by the .ino |
||||
* with readTonePresent(what) where "what" allows an absolute threshod comparison |
||||
* on Tone Power, a relative comparison between Tone Power and RefPower, |
||||
* or both. The relative comparison is the preferred form for many applicatins. |
||||
* Parameter "what" can be isAbsThreshold, isRelThreshold or isBothThreshold. |
||||
* |
||||
* The second method of threshold useage is to gate the input of the block to |
||||
* output 0 of the block. This happens as part of the class update and requires |
||||
* no .ino action. The control comes from the function |
||||
* thresholds(levelAbs, levelRel). Setting either levelbs or levelRel to zero |
||||
* removes that measure from the gating process. So, a typical function call |
||||
* would be detCTCSS.thresholds(0.0f, 0.4f); |
||||
* |
||||
* This class uses decimation-by-16 to improve the performance of the low-frequency |
||||
* filtering and to reduce processing load. The filter parameters of the decimation |
||||
* LPF can be used at any sampling rate from 44 to 100 kHz. Other rates will need |
||||
* a new design for the LPF. See the discussion below for "iirLpfCoeffs" showing |
||||
* how to design such a filter. |
||||
* |
||||
* The bandpass filter to cover 67 to 254 Hz is predesigned for sample rates near |
||||
* 44, 48, 96 or 100 kHz. The discussion below "BPF iir Coefficients" has |
||||
* information for designing BPF for other sample rates. The new design is entered |
||||
* via the function setCTCSS_BP(float *filterCoeffs). |
||||
* |
||||
* Each update of an 128-input block takes about 42 uSec on a Teensy 3.6. |
||||
* |
||||
* Note - Pieces of this support block sizes other than 128, but not all. |
||||
* Work needs to be done to implement and test variable block size. The same is |
||||
* true for sample rates outside the 44.1 to 100 kHz range. |
||||
*/ |
||||
|
||||
#ifndef analyze_CTCSS_F32_h_ |
||||
#define analyze_CTCSS_F32_h_ |
||||
|
||||
#include "Arduino.h" |
||||
#include "AudioStream_F32.h" |
||||
|
||||
class analyze_CTCSS_F32 : public AudioStream_F32 |
||||
{ |
||||
//GUI: inputs:1, outputs:1 //this line used for automatic generation of GUI node
|
||||
//GUI: shortName:DetCTCSS
|
||||
public: |
||||
analyze_CTCSS_F32(void) |
||||
: AudioStream_F32(1, inputQueueArray_f32) { |
||||
sample_rate_Hz = AUDIO_SAMPLE_RATE; |
||||
block_size = AUDIO_BLOCK_SAMPLES; |
||||
initCTCSS(); |
||||
} |
||||
// Option of AudioSettings_F32 change to block size and/or sample rate:
|
||||
analyze_CTCSS_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1,inputQueueArray_f32){ |
||||
sample_rate_Hz = settings.sample_rate_Hz; |
||||
block_size = settings.audio_block_samples; |
||||
initCTCSS(); |
||||
} |
||||
|
||||
void initCTCSS(void) |
||||
{ |
||||
frequency(103.5f, 300.0f); // Defaults
|
||||
setNotchX4(103.5f); |
||||
// Initialize BiQuad instance (ARM DSP Math Library)
|
||||
//https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html
|
||||
arm_biquad_cascade_df1_init_f32(&iir_lpf_inst, 2, &iirLpfCoeffs[0], &StateLpf32[0]); |
||||
arm_biquad_cascade_df1_init_f32(&iir_bpf_inst, 4, iirBpfCoeffs, &StateBpf32[0]); |
||||
for (int i=0; i<8; i++) |
||||
{ |
||||
d16a[i] = 0.0f; |
||||
d16b[i] = 0.0f; |
||||
} |
||||
arm_biquad_cascade_df1_init_f32(&iir_nulls_inst, 4, &iirNullsCoeffs[0], &StateNullsF32[0]); |
||||
thresholds(0.1f, 1.0f); |
||||
gEnabled = true; |
||||
setCTCSS_BP(NULL); // May be invalid if Sample rate is not provided for here.
|
||||
} |
||||
|
||||
// Set frequency and length of Goertzel in Hz and milliseconds.
|
||||
void frequency(float freq) { // Defaults to 300 mSec
|
||||
frequency(freq, 300.0f); |
||||
} |
||||
|
||||
void frequency(float freq, float tMeas) { |
||||
// e.g. freq=103.5, tMeas=250, sample_rate_Hz = 44100, gLength=689
|
||||
gLength = (uint16_t)(0.5 + sample_rate_Hz*tMeas/(1000.0f*16)); // reinstated
|
||||
// Now set the notch filters for the four-biquad notch filters
|
||||
setNotchX4(freq); |
||||
// and the sub-audible general BP filter
|
||||
setCTCSS_BP(NULL); |
||||
// e.g., freq=103.5, sample_rate_Hz=44100, gCoefficient=2*cos(0.23594)=1.944634
|
||||
gCoefficient = 2.0f*cosf(6.28318530718f*16.0f*freq/sample_rate_Hz); |
||||
// Another constant for ending calculation is exp(-j 2*pi*16*freq*(glength-1)/sample_rate_Hz)
|
||||
ccRe = cos(6.28318530718f*16.0f*freq/sample_rate_Hz); |
||||
ccIm = -sin(6.28318530718f*16.0f*freq/sample_rate_Hz); |
||||
} |
||||
|
||||
bool available(void) { |
||||
__disable_irq(); |
||||
bool flag = new_output; |
||||
if(flag) |
||||
new_output = false; // Prevent multiple reads of same data
|
||||
__enable_irq(); |
||||
return flag; |
||||
} |
||||
|
||||
float readTonePower(void) { return powerTone; } |
||||
|
||||
float readRefPower(void) { return powerRef; } |
||||
|
||||
const uint16_t isAbsThreshold=1; |
||||
const uint16_t isRelThreshold=2; |
||||
const uint16_t isBothThreshold=3; |
||||
bool readTonePresent(uint16_t what) { |
||||
if(what == isAbsThreshold) |
||||
return (powerTone>threshAbs); |
||||
else if(what == isRelThreshold) |
||||
return (powerTone>threshRel*powerRef); |
||||
else if(what == isBothThreshold) |
||||
return (powerTone>threshAbs && powerTone>threshRel*powerRef); |
||||
else return false; // Something was wrong about call
|
||||
} |
||||
|
||||
void thresholds(float levelAbs, float levelRel) { |
||||
// The absolute threshold is compared with tonePower
|
||||
if (levelAbs < 0.000001f) threshAbs = 0.000001f; |
||||
else threshAbs = levelAbs; |
||||
|
||||
// The relative threshold is multiples of refPower
|
||||
if (levelRel < 0.000001f) threshRel = 0.000001f; |
||||
else threshRel = levelRel; |
||||
} |
||||
|
||||
operator bool(); // true if at or above threshold, false if below
|
||||
|
||||
virtual void update(void); |
||||
|
||||
/* The bandpass filter covers 67 to 254 Hz to allow a comparison
|
||||
* measurement for the tone. This changes with sample rate. Four |
||||
* precomputed filters are provided for 44.1, 48, 96 and 100 kHz. |
||||
* Supply a NULL for the filterCoeffs to use these 4. |
||||
* filterCoeffs points to an array of 20 floats that are IIR |
||||
* coefficients for four BiQuad Stages. To use caller supplied |
||||
* filter coefficients, call with a pointer to that 4x BiQuad filter at |
||||
* filterCoeffs. |
||||
*/ |
||||
float* setCTCSS_BP(float *filterCoeffs) |
||||
{ |
||||
if(filterCoeffs == NULL) |
||||
{ |
||||
if(sample_rate_Hz>43900.0f && sample_rate_Hz<44200.0f) |
||||
iirBpfCoeffs = coeff32bp44; |
||||
else if(sample_rate_Hz>47900.0f && sample_rate_Hz<48100.0f) |
||||
iirBpfCoeffs = coeff32bp48; |
||||
else if(sample_rate_Hz>95800.0f && sample_rate_Hz<96200.0f) |
||||
iirBpfCoeffs = coeff32bp96; |
||||
else if(sample_rate_Hz>99800.0f && sample_rate_Hz<100200.0f) |
||||
iirBpfCoeffs = coeff32bp100; |
||||
else return NULL; |
||||
} |
||||
else iirBpfCoeffs = filterCoeffs; // Caller supplied
|
||||
return iirBpfCoeffs; |
||||
} |
||||
|
||||
/* To use a windowing fuction to change the Goertzel sidelobe levels,
|
||||
* in the .ino compute half the window function and put that in an array. |
||||
* Call this function with a pointer to that array. To stop windowing, |
||||
* call this function with NULL as an argument. |
||||
* / Windowing not yet implemented <<<------------------- |
||||
void setGWindow(float *pWinCoeff) |
||||
{ |
||||
windowCoeff = pWinCoeff; // Point to the caller supplied window function
|
||||
if(pWinCoeff != NULL) |
||||
nWindow = (uint16_t)(0.5f + sample_rate_Hz*tMeas/(2.0f*1000.0f*16.0f)); |
||||
else |
||||
nWindow = 0; |
||||
} |
||||
*/ |
||||
|
||||
private: |
||||
int16_t count16 = 0; unsigned long cnt = 0UL; |
||||
float ccRe=0.0f; |
||||
float ccIm=0.0f; |
||||
float freq = 103.5f; |
||||
float tMeas = 300.0f; |
||||
float d16a[8]; // Small data blocks, after decimation
|
||||
float d16b[8]; |
||||
float gCoefficient = 1.9f; // Goertzel algorithm coefficient
|
||||
float gs1, gs2; // Goertzel algorithm state
|
||||
float out1, out2; // Goertzel algorithm state output
|
||||
float powerTone; |
||||
float powerRef; |
||||
uint16_t gLength; // number of samples to analyze
|
||||
uint16_t gCount; // how many left to analyze
|
||||
float powerSum = 0.0f; |
||||
uint16_t powerSumCount = 0; |
||||
float threshAbs = 0.1f; // absolute threshold, 0.01 to 0.99
|
||||
float threshRel = 1.0f; // noise relative threshold
|
||||
bool gEnabled = false; |
||||
volatile bool new_output = false; |
||||
audio_block_f32_t *inputQueueArray_f32[1]; |
||||
float sample_rate_Hz = AUDIO_SAMPLE_RATE; |
||||
uint16_t block_size = 128; |
||||
float iirNullsCoeffs[20]; |
||||
float *iirBpfCoeffs; |
||||
// windowCoeff needs to store half of values (symmetry) for up to
|
||||
// 0.25 second at 100KHz sample rate, or 0.25*0.5*100000/16=782 pts.
|
||||
// This is extravagant and needs to be in caller area to allow
|
||||
// frugality when there is no window at all.
|
||||
// float *windowCoeff; <<- Windowing is not currently implemented
|
||||
// uint16_t nWindow = 0; <<- ditto
|
||||
/* Info - The structure from arm_biquad_casd_df1_inst_f32 instance consists of
|
||||
* uint32_t numStages; |
||||
* const float32_t *pCoeffs; //Points to the array of coefficients, length 5*numStages.
|
||||
* float32_t *pState; //Points to the array of state variables, length 4*numStages.
|
||||
* We have 3 IIR's: |
||||
*/ |
||||
arm_biquad_casd_df1_inst_f32 iir_lpf_inst; |
||||
arm_biquad_casd_df1_inst_f32 iir_bpf_inst; |
||||
arm_biquad_casd_df1_inst_f32 iir_nulls_inst; |
||||
|
||||
/* Each Biquad stage has 4 state variables x[n-1], x[n-2], y[n-1], and y[n-2].
|
||||
* See https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html
|
||||
* The state variables are arranged in the pState array as: |
||||
* {x[n-1], x[n-2], y[n-1], y[n-2]} |
||||
* The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, |
||||
* and so on. The state array has a total length of 4*numStages values. |
||||
* The state variables are updated after each block of data is processed; the |
||||
* coefficients are untouched. |
||||
*/ |
||||
float StateLpf32[2*4]; |
||||
float StateBpf32[4*4]; |
||||
float StateNullsF32[4*4]; |
||||
|
||||
/*
|
||||
* LPF for decimation before CTCSS tone detector. |
||||
* Pick cutoff frequency to pass 254 Hz with fs=44.1kHz |
||||
* or fc=2*254/44100=0.01152. This passes highest tone |
||||
* frequency at the lowest sample rate. Octave code: |
||||
* FSample = 44100; |
||||
* halfFSample = 0.5 * FSample |
||||
* [z,p,g] = ellip(4,0.1,60,254/halfFSample); |
||||
* sos = zp2sos(z,p,g); |
||||
* sos1 = sos(1:1,:) % 1st row |
||||
* sos2 = sos(2:2,:) % 2nd row |
||||
* % This puts the stop band at 0.02*fs or for fs=100 KHz |
||||
* % at 2 kHz. This, in turn, supports an 4 kHz |
||||
* % sampling rate after decimation. Thus a single LPF design |
||||
* % can be used for sample rates from 44.1 to 100 kHz. |
||||
* % See notes below on how to convert Octave output to coefficients: |
||||
*/ |
||||
float iirLpfCoeffs[10] = // See lp1ForCtcss.gif
|
||||
{9.937058251573265e-04f, -1.898847656031816e-03f, 9.937058251573265e-04f, |
||||
1.953233139252097e+00f, -9.540781328043288e-01f, |
||||
1.000000000000000e+00f, -1.983872743032985e+00f, 9.999999999999998e-01f, |
||||
1.980584507653506e+00f, -9.822943824311938e-01f}; |
||||
|
||||
/* There is a band pass filter that is used after decimation to limit the
|
||||
* spectrum that is seen by the CTCSS detector to (67, 254) Hz. This |
||||
* provides the entire CTCSS band to compare against to see if any other |
||||
* tones are present. It also restricts the amount of voice spectrum |
||||
* presented to the CTCSS detector. This filter is sample rate dependent |
||||
* this routine allows for four different "built-in" rates. |
||||
* |
||||
* Sample LPF Decimation CTCSS BPF |
||||
* Rate Fco Ratio Samp Rate Name |
||||
* ------- ------ -------- -------- ----- |
||||
* 44.1kHz 254Hz 16 2756.2Hz BP44 |
||||
* 48.0kHz 276Hz 16 3000.0Hz BP48 |
||||
* 96.0kHz 553Hz 16 6000.0Hz BP96 |
||||
* 100kHz 576Hz 16 6250.0Hz BP100 |
||||
* |
||||
* The following allow computing a bandpass filter for any sample rate in |
||||
* the 44 to 100 kHz range (gnu octave script). |
||||
* |
||||
% ===================== BPF iir Coefficients ============================= |
||||
% This GNU Octave m script will generate the BPF iir coefficients |
||||
% for other sample rates than the four listed here. It will probably |
||||
% run under Matlab, but that is untested. |
||||
% |
||||
format long e |
||||
FSample = 6250.0; |
||||
halfFSample = 0.5 * FSample |
||||
[z,p,g] = ellip(4,0.1,60,[67 254]/halfFSample); |
||||
sos = zp2sos(z,p,g); |
||||
sos1 = sos(1:1,:) % 1st row |
||||
sos2 = sos(2:2,:) % 2nd row |
||||
sos3 = sos(3:3,:) % 3rd row |
||||
sos4 = sos(4:4,:) % 4th row |
||||
% |
||||
% sos are the "Second Order State" (BiQuad) consisting of 4 sets of 6 numbers. |
||||
% These are numerator and denominator in form of the Open Audio Matlab |
||||
% IIR function setFilterCoeff_Matlab(float32_t b[], float32_t a[]) |
||||
% in AudioFilterIIR_F32.h of |
||||
% https://github.com/chipaudette/OpenAudio_ArduinoLibrary/blob/master/AudioFilterIIR_F32.h
|
||||
% |
||||
% sosi, i=1 to 4, are each 6 double precision numbers, a0, a1, a2, |
||||
% b0, b1, b2. To use these with the ARM library, |
||||
% arm_biquad_cascade_df1_f32, two changes are needed. The fourth |
||||
% number is a0 that is always 1.000 and for the arm routine, it is omitted. |
||||
% The fifth and sixth numbers are -a1 and -a2, so they need to have their |
||||
% signs reversed. So, for example, for the 44.1 kHz sample rate: |
||||
% sos1 = |
||||
% 4.386558177463118e-03 4.835083945591431e-03 4.386558177463117e-03 |
||||
% 1.000000000000000e+00 -1.503280453219001e+00 8.498498588788136e-01% |
||||
% sos2 = |
||||
% 1.000000000000000e+00 -4.234328155653108e-01 1.000000000000000e+00 |
||||
% 1.000000000000000e+00 -1.524529304401012e+00 6.838502689975459e-01 |
||||
% sos3 = |
||||
% 1.000000000000000e+00 -1.996808089693871e+00 9.999999999999998e-01 |
||||
% 1.000000000000000e+00 -1.797650895821358e+00 8.356641238925192e-01 |
||||
% sos4 = |
||||
% 1.000000000000000e+00 -1.999398698654737e+00 1.000000000000000e+00 |
||||
% 1.000000000000000e+00 -1.941833848024410e+00 9.614543316508065e-01 |
||||
% and the resulting arm coefficient array, including all 4 biquads, is |
||||
% coeff32BP44[20] = { |
||||
% 4.386558177463118e-03, 4.835083945591431e-03, 4.386558177463117e-03, |
||||
% 1.503280453219001e+00, -8.498498588788136e-01, |
||||
% 1.000000000000000e+00, -4.234328155653108e-01, 1.000000000000000e+00, |
||||
% 1.524529304401012e+00, -6.838502689975459e-01, |
||||
% 1.000000000000000e+00, -1.996808089693871e+00, 9.999999999999998e-01, |
||||
% 1.797650895821358e+00, -8.356641238925192e-01, |
||||
% 1.000000000000000e+00, -1.999398698654737e+00, 1.000000000000000e+00, |
||||
% 1.941833848024410e+00, -9.614543316508065e-01}; |
||||
% |
||||
*/ |
||||
// The four predefined bandpass filters:
|
||||
// BP44 Decimated fSample = 2756.2 Hz
|
||||
float coeff32bp44[20] = { |
||||
4.386558177463118e-03f, 4.835083945591431e-03f, 4.386558177463117e-03f, |
||||
1.503280453219001e+00f, -8.498498588788136e-01f, |
||||
1.000000000000000e+00f, -4.234328155653108e-01f, 1.000000000000000e+00f, |
||||
1.524529304401012e+00f, -6.838502689975459e-01f, |
||||
1.000000000000000e+00f, -1.996808089693871e+00f, 9.999999999999998e-01f, |
||||
1.797650895821358e+00f, -8.356641238925192e-01f, |
||||
1.000000000000000e+00f, -1.999398698654737e+00f, 1.000000000000000e+00f, |
||||
1.941833848024410e+00f, -9.614543316508065e-01f}; |
||||
|
||||
// BP48 Decimated fSample = 3000 Hz
|
||||
float coeff32bp48[20] = { |
||||
3.764439090680418e-03f, 3.650362666517592e-03f, 3.764439090680417e-03f, |
||||
1.000000000000000e+00f, -5.926113222413726e-01f, 1.000000000000000e+00f, |
||||
1.569400123464342e+00f, -7.055760636246757e-01f, |
||||
1.000000000000000e+00f, -1.997300534603388e+00f, 9.999999999999998e-01f, |
||||
1.816002936969042e+00f, -8.483092827984080e-01f, |
||||
1.000000000000000e+00f, -1.999491318923814e+00f, 1.000000000000000e+00f, |
||||
1.947984087804752e+00f, -9.645809301964445e-01f}; |
||||
|
||||
// BP96 Decimated fSample = 6000 Hz
|
||||
float coeff32bp96[20] = { |
||||
1.554430361573611e-03f, 5.765630035510897e-04f, 1.554430361573611e-03f, |
||||
1.803990601116592e+00f, -8.406882700444890e-01f, |
||||
1.000000000000000e+00f, -1.541034615722940e+00f, 9.999999999999998e-01f, |
||||
1.846550880066576e+00f, -9.250235034256622e-01f, |
||||
1.000000000000000e+00f, -1.999319784384852e+00f, 9.999999999999998e-01f, |
||||
1.913429616511321e+00f, -9.218266406823739e-01f, |
||||
1.000000000000000e+00f, -1.999871669777634e+00f, 1.000000000000000e+00f, |
||||
1.978038062492293e+00f, -9.822341038950552e-01f}; |
||||
|
||||
// BP100 Decimated fSample = 6250 Hz
|
||||
float coeff32bp100[20] = { |
||||
1.504180063396739e-03f, -6.773089598829345e-04f, 1.504180063396738e-03f, |
||||
1.812635069874353e+00f, -8.465624856328144e-01f, |
||||
1.000000000000000e+00f, -1.573630872142354e+00f, 1.000000000000000e+00f, |
||||
1.855377871833802e+00f, -9.278512110118003e-01f, |
||||
1.000000000000000e+00f, -1.999372984381045e+00f, 9.999999999999998e-01f, |
||||
1.917101697268557e+00f, -9.248525551899550e-01f, |
||||
1.000000000000000e+00f, -1.999881702874664e+00f, 1.000000000000000e+00f, |
||||
1.979072537983060e+00f, -9.829412184208248e-01f}; |
||||
|
||||
/* Designs a four-notch filter to null out a tone. At 100 Hz the
|
||||
* null width of the four staggered tones is about 2Hz wide. |
||||
* iirNullsCoeffs is a pointer to an array of 20 floats. The nulls |
||||
* filter is always designed here and there is no need for |
||||
* caller-designed filters. |
||||
*/ |
||||
void setNotchX4(float frequency) |
||||
{ |
||||
float kf[] = {0.9955f, 0.9982f, 1.0018f, 1.0045f}; // Offset ratios
|
||||
float Q = 60.0f; |
||||
float w0, sinW0, alpha, cosW0, scale; |
||||
for (int ii = 0; ii<4; ii++) // Over 4 cascaded BiQuad filters
|
||||
{ |
||||
w0 = kf[ii]*frequency*(2.0f*3.141592654f*16.0f / sample_rate_Hz); |
||||
sinW0 = sin(w0); |
||||
alpha = sinW0 / (Q * 2.0f); |
||||
cosW0 = cos(w0); |
||||
scale = 1.0f / (1.0f+alpha); |
||||
/* b0 */ iirNullsCoeffs[5*ii + 0] = scale; |
||||
/* b1 */ iirNullsCoeffs[5*ii + 1] = -2.0f * cosW0 * scale; |
||||
/* b2 */ iirNullsCoeffs[5*ii + 2] = scale; |
||||
/* a0 always 1.0f */ |
||||
/* a1 */ iirNullsCoeffs[5*ii + 3] = 2.0f * cosW0 * scale; |
||||
/* a2 */ iirNullsCoeffs[5*ii + 4] = -(1.0f - alpha) * scale; |
||||
} |
||||
} |
||||
}; |
||||
#endif |
Loading…
Reference in new issue