You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
OpenAudio_ArduinoLibrary/analyze_CTCSS_F32.h

551 lines
26 KiB

/*
* analyze_CTCSS_F32.h For the OpenAudio_TeensyArduino library
* (floating point audio).
* MIT License on changed portions
* Bob Larkin March 2021 original write
* Rev 21 Jan 2022 Major redo and ready to publish. Bob L
* Rev 15 March 2023 - Added dynamic sample rate control; added 12 ksps; corrected
* receiveWritable_f32() for block with output. Bob L
*
* 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. (Mar 2023: Decimate by 8 for 12 ksps).
*
* 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 12 and 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();
}
// This is called at instantation and also if either freq or sample_rate_Hz is changed
void initCTCSS(void)
{
gEnabled = true;
float32_t nDecimatef = (float32_t)nDecimate; // Later use
// INFO: Initialize BiQuad instance (ARM DSP Math Library)
//https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html
// First, select the BPF for measuring the noise level
if(iirUserBpfCoeffs != NULL)
iirBpfCoeffs = iirUserBpfCoeffs; // From INO, always use if supplied
//else if(sample_rate_Hz>11000.0f && sample_rate_Hz<11050.0f)
//iirBpfCoeffs = coeff32bp11; Not currently implemented
else if(sample_rate_Hz>11975.0f && sample_rate_Hz<12025.0f)
iirBpfCoeffs = coeff32bp12;
//else if(sample_rate_Hz>23950.0f && sample_rate_Hz<24050.0f)
//iirBpfCoeffs = coeff32bp24; Not currently implemented
else 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 gEnabled = false;
arm_biquad_cascade_df1_init_f32(&iir_bpf_inst, 4, iirBpfCoeffs, &StateBpf32[0]);
if(sample_rate_Hz>11900 && sample_rate_Hz<24100) // Now choose a pre-decimation LPF
{
nDecimate = 8;
arm_biquad_cascade_df1_init_f32(&iir_lpf_inst, 4, &iirLpfCoeffs12[0], &StateLpf32[0]);
}
else if (sample_rate_Hz>44000 && sample_rate_Hz<100100)
{
nDecimate = 16;
arm_biquad_cascade_df1_init_f32(&iir_lpf_inst, 4, &iirLpfCoeffs44[0], &StateLpf32[0]);
}
else gEnabled = false;
nDecimatef = (float32_t)nDecimate;
sampleRate2 = sample_rate_Hz / nDecimatef;
for (int i=0; i<16; i++) // Need 8 when decimate is 16 or 16 if decimate is 8
{
d16a[i] = 0.0f;
d16b[i] = 0.0f;
}
arm_biquad_cascade_df1_init_f32(&iir_nulls_inst, 4, &iirNullsCoeffs[0], &StateNullsF32[0]);
// e.g. freq=103.5, tMeas=250, sample_rate_Hz = 44100, gLength=689
gLength = (uint16_t)(0.5 + sample_rate_Hz*tMeas/(1000.0f*nDecimatef)); // reinstated
// Now set the notch filters for the four-biquad notch filters
setNotchX4(freq);
// e.g., freq=103.5, sample_rate_Hz=44100, gCoefficient=2*cos(0.23594)=1.944634
gCoefficient = 2.0f*cosf(6.28318530718f*nDecimatef*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*nDecimatef*freq/sample_rate_Hz);
ccIm = -sin(6.28318530718f*nDecimatef*freq/sample_rate_Hz);
thresholds(0.1f, 1.0f);
}
// Set frequency and length of Goertzel detect tone in Hz and milliseconds.
void frequency(float32_t _freq) { // Defaults to 300 mSec
frequency(_freq, 300.0f);
}
void frequency(float32_t _freq, float32_t _tMeas) {
freq = _freq;
tMeas = _tMeas;
initCTCSS();
}
// Sample rates 12, 44.1, 48, 96 and 100 ksps are supported.
void setSampleRate_Hz(const float32_t &fs_Hz) {
sample_rate_Hz = fs_Hz;
initCTCSS();
}
bool available(void) {
__disable_irq();
bool flag = new_output;
if(flag)
new_output = false; // Prevent multiple reads of same data
__enable_irq();
return flag;
}
float32_t readTonePower(void) { return powerTone; }
float32_t 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(float32_t levelAbs, float32_t 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. Five
* precomputed filters are provided for 12, 44.1, 48, 96 and 100 kHz.
* Supply a NULL for the filterCoeffs to use these five.
* _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(float32_t *_filterCoeffs) {
iirUserBpfCoeffs = _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(float32_t *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;
float32_t ccRe=0.0f;
float32_t ccIm=0.0f;
float32_t freq = 103.5f;
float32_t tMeas = 300.0f;
uint16_t nDecimate = 16;
float32_t sampleRate2 = 2756.25f; // 44100/16 etc.
float32_t d16a[16]; // Small data blocks, after decimation
float32_t d16b[16]; // Reference power sum
float32_t gCoefficient = 1.9f; // Goertzel algorithm coefficient
float32_t gs1, gs2; // Goertzel algorithm state
float32_t out1, out2; // Goertzel algorithm state output
float32_t powerTone;
float32_t powerRef;
uint16_t gLength; // number of samples to analyze
uint16_t gCount; // how many left to analyze
float32_t powerSum = 0.0f;
uint16_t powerSumCount = 0;
float32_t threshAbs = 0.1f; // absolute threshold, 0.01 to 0.99
float32_t threshRel = 1.0f; // noise relative threshold
bool gEnabled = false;
volatile bool new_output = false;
audio_block_f32_t *inputQueueArray_f32[1];
float32_t sample_rate_Hz = AUDIO_SAMPLE_RATE;
uint16_t block_size = 128;
float32_t iirNullsCoeffs[20];
float32_t *iirBpfCoeffs;
float32_t *iirUserBpfCoeffs;
// 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.
// float32_t *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.
*/
float32_t StateLpf32[4*4];
float32_t StateBpf32[4*4];
float32_t StateNullsF32[4*4];
// The coefficients are stored in the array pCoeffs in the following order:
// {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
// where b1x and a1x are the coefficients for the first stage, b2x and a2x are
// the coefficients for the second stage, and so on. The pCoeffs array
// contains a total of 5*numStages values.
// a10, a20, etc are all 1.000 and not listed.
/*
* 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. These
* were calculated by Iowa Hills routines.
*/
//12000 260Hz -60 dB double elliptic 8 pole -60 dB at 300 Hz
float32_t iirLpfCoeffs12[20] = {
0.182145523f,-0.359763551f, 0.182145523f, 1.888598461f,-0.893125955f,
0.350161286f,-0.689819799f, 0.350161286f, 1.922991947f,-0.933494720f,
0.293698317f,-0.571683303f, 0.293698317f, 1.954562781f,-0.970276113f,
0.053599924f,-0.089098167f, 0.053599924f, 1.973912800f,-0.992014481f };
float32_t iirLpfCoeffs44[20] = {
0.188636212f,-0.376924253f, 0.188636212f, 1.969403901f,-0.969752071f,
0.357249908f,-0.713703189f, 0.357249908f, 1.980638291f,-0.981434917f,
0.294316431f,-0.587455758f, 0.294316431f, 1.990628020f,-0.991805124f,
0.049761875f,-0.098177786f, 0.049761875f, 1.996468755f,-0.997814718f};
/* 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 five different "built-in" rates.
*
* Sample LPF Decimation CTCSS BPF
* Rate Fco Ratio Samp Rate Name
* ------- ------ -------- -------- -----
* 12.0kHz 260Hz 8 1500.0Hz BP12
* 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).
*
% ========== Sidebar: 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 five predefined bandpass filters:
// BP12 Decimated fSample = 1500 sps
// 65 to 260, 0.1 dB rip, -60 dB elliptic, -7.5 dB at 300 Hz, -60 at 475
float32_t coeff32bp12[20] = {
0.386816925f,-0.769638284f, 0.386816925f, 1.586751090f,-0.706113049f,
0.265346713f, 0.247789162f, 0.265346713f, 0.996735842f,-0.484375851f,
0.461314461f,-0.921736474f, 0.461314461f, 1.864126624f,-0.928392720f,
0.380685155f, 0.664524969f, 0.380685155f, 0.720627184f,-0.766126702f};
// BP44 Decimated fSample = 2756.2 Hz
float32_t 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
// 67 to 254 Band Pass 60 dB reject elliptic -7.5 at 300 Hz -35 at 450 Hz
float32_t coeff32bp48[20] = {
0.272147027f,-0.543594385f, 0.272147027f, 1.819349787f,-0.850367415f,
0.226417971f,-0.138869864f, 0.226417971f, 1.574952243f,-0.707479581f,
0.261002559f,-0.521878771f, 0.261002559f, 1.949332941f,-0.965207141f,
0.232822864f, 0.221820730f, 0.232822864f, 1.572088740f, -0.860996505};
// BP96 Decimated fSample = 6000 Hz
float32_t 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
float32_t 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(float32_t frequency)
{
float32_t kf[] = {0.9955f, 0.9982f, 1.0018f, 1.0045f}; // Offset ratios
float32_t Q = 60.0f;
float32_t 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*nDecimate / 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