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.
271 lines
13 KiB
271 lines
13 KiB
/*


* AudioAnalyzePhase_F32.h


*


* 31 March 2020, Rev 8 April 2020


* Status Tested OK T3.6 and T4.0.


* Bob Larkin, in support of the library:


* Chip Audette, OpenAudio, Apr 2017


* 


* There are two inputs, 0 and 1 (Left and Right)


* There is one output, the phase angle between 0 & 1 expressed in


* radians (180 degrees is Pi radians) or degrees. This is a 180degree


* type of phase detector. See RadioIQMixer_F32 for a 360 degree type.


*


* This block can be used to measure phase between two sinusoids, and the default IIR filter is suitable for this with a cutoff


* frequency of 100 Hz. The only IIR configuration is 4cascaded satages of BiQuad. For this, 20 coefficients must be provided


* in 4 times (b0, b1, b2, a1, a2) order (example below). This IIR filter inherently does not have very good


* linearity in phase vs. frequency. This can be a problem for communications systems.


* As an alternative, a linear phase (as long as coefficients are symmetrical)


* FIR filter can be set up with the begin method. The built in FIR LP filter has a cutoff frequency of 4 kHz when used


* at a 44.1 kHz sample rate. This filter uses 53 coefficients (called taps). Any FIR filter with 4 to 200 coefficients can be used


* as set up by the begin method.


*


* DEFAULTS: 100 Hz IIR LP, output is in radians, and this does *NOT* need a call to begin(). This can be changed, including


* using a FIR LP where linear phase is needed, or NO_LP_FILTER that leaves harmonics of the input frequency. Method begin()


* changes the options. For instance, to use a 60 coefficient FIR the setup() in the .INO might do


* myAnalyzePhase.begin(FIR_LP_FILTER, &myFIRCoefficients[0], 60, DEGREES_PHASE);


* If _pcoefficients is NULL, the coefficients will be left default. For instance, to use the default 100 Hz IIR filter, with degree output


* myAnalyzePhase.begin(IIR_LP_FILTER, NULL, 20, DEGREES_PHASE);


* To provide a new set of IIR coefficients (note strange coefficient order and negation for a() that CMSIS needs)


* myAnalyzePhase.begin(IIR_LP_FILTER, &myIIRCoefficients[0], 20, RADIANS_PHASE);


* In begin() the pdConfig can be set (see #defines below). The default is to use no limiter, but to measure the input levels over the


* block and use that to scale the multiplier output. This will cause successive blocks to change slightly in output level due to


* errors in level measurement, but is other wise fine. If the limiter is used, the narrow band IIR filter should also be used to


* prevent artifacts from "beats" between the sample rate and the input frequency.


*


* Three different scaling routines are available following the LP filter. These deal with the issue that the multiplier type


* of phase detector produces an output proportional to the cosine of the phase angle between the two input sine waves.


* If the inputs both have a magnitude ranging from 1.0 to 1.0, the output will be cos(phase difference). Other values of


* sine wave will multiply this by the product of the two maximum levels. The selection of "fast" or "accurate" acos() will


* make the output approximately the angle, as scaled by UNITS_MASK. The ACOS_MASK bits in pdConfig, set by begin(), selects the


* acos used. Note that if acos function is used, the output range is 0 to pi radians, i.e., 0 to 180 degrees. "Units" have no


* effect when acos90 is not being used, as that would make little sense for the (1,1) output.


*


* Functions:


* setAnalyzePhaseConfig(const uint16_t LPType, float32_t *pCoeffs, uint16_t nCoeffs)


* setAnalyzePhaseConfig(const uint16_t LPType, float32_t *pCoeffs, uint16_t nCoeffs, uint16_t pdConfig)


* are used to chhange the output filter from the IIR default, where:


* LPType is NO_LP_FILTER, IIR_LP_FILTER, FIR_LP_FILTER to select the output filter


* pCoeffs is a pointer to filter coefficients, either IIR or FIR


* nCoeffs is the number of filter coefficients


* pdConfig is bitwise selection (default 0b1100) of


* Bit 0: 0=No Limiter (default) 1=Use limiter


* Bit 2 and 1: 00=Use no acos linearizer 01=undefined


* 10=Fast, mathcontinuous acos() (default) 11=Accurate acosf()


* Bit 3: 0=No scale of multiplier 1=scale to minmax (default)


* Bit 4: 0=Output in degrees 1=Output in radians (default)


* showError(uint16_t e) sets whether error printing comes from update (e=1) or not (e=0).


*


* Examples: AudioTestAnalyzePhase.ino and AudioTestSinCos.ino


*


* Some measured time data for a 128 size block, Teensy 3.6, parts of update():


* Default settings, total time 123 microseconds


* Overhead of update(), loading arrays, handling blocks, less than 2 microseconds


* Minmax calculation, 23 microseconds


* Multiplier DBMixer 8 microseconds


* IIR LPF (default filter) 57 microseconds


* 53term FIR filter 149 microseconds


* Fast acos_32() linearizer 32 microseconds


* Accurate acosf(x) seems to vary (with x?), 150 to 350 microsecond range


*


* Measured total update() time for the minmax scaling, fast acos(), and 53term FIR filtering


* case is 214 microseconds for Teensy 3.6 and 45 microseconds for Teensy 4.0.


*


* Copyright (c) 2020 Bob Larkin


*


* 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 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.


*/




#ifndef _analyze_phase_f32_h


#define _analyze_phase_f32_h




#define N_STAGES 4


#define NFIR_MAX 200


#define NO_LP_FILTER 0


#define IIR_LP_FILTER 1


#define FIR_LP_FILTER 2


#define RADIANS_PHASE 1.0


#define DEGREES_PHASE 57.295779




// Test the number of microseconds to execute update()


#define TEST_TIME 1




#define LIMITER_MASK 0b00001


#define ACOS_MASK 0b00110


#define SCALE_MASK 0b01000


#define UNITS_MASK 0b10000




#include "AudioStream_F32.h"


#include <math.h>




class AudioAnalyzePhase_F32 : public AudioStream_F32 {


//GUI: inputs:2, outputs:1 //this line used for automatic generation of GUI node


//GUI: shortName: AnalyzePhase


public:


// Option of AudioSettings_F32 change to block size or sample rate:


AudioAnalyzePhase_F32(void) : AudioStream_F32(2, inputQueueArray_f32) { // default block_size and sampleRate_Hz


// Initialize BiQuad IIR instance (ARM DSP Math Library)


arm_biquad_cascade_df1_init_f32(&iir_inst, N_STAGES, &iir_coeffs[0], &IIRStateF32[0]);


}


// Constructor including new block_size and/or sampleRate_Hz


AudioAnalyzePhase_F32(const AudioSettings_F32 &settings) : AudioStream_F32(2, inputQueueArray_f32) {


block_size = settings.audio_block_samples;


sampleRate_Hz = settings.sample_rate_Hz;


// Initialize BiQuad IIR instance (ARM DSP Math Library)


arm_biquad_cascade_df1_init_f32(&iir_inst, N_STAGES, &iir_coeffs[0], &IIRStateF32[0]);


}




// Set AnalyzePhaseConfig while leaving pdConfig as is


void setAnalyzePhaseConfig(const uint16_t _LPType, float32_t *_pCoeffs, uint16_t _nCoeffs) {


setAnalyzePhaseConfig( _LPType, _pCoeffs, _nCoeffs, pdConfig);


}


// Set AnalyzePhaseConfig in full generality


void setAnalyzePhaseConfig(const uint16_t _LPType, float32_t *_pCoeffs, uint16_t _nCoeffs, uint16_t _pdConfig) {


AudioNoInterrupts(); // No interrupts while changing parameters


LPType = _LPType;


if (LPType == NO_LP_FILTER) {


//Serial.println("Advice: in AnalyzePhase, for NO_LP_FILTER the output contains 2nd harmonics");


//Serial.println(" that need external filtering.");


}


else if (LPType == IIR_LP_FILTER) {


if(_pCoeffs != NULL){


pIirCoeffs = _pCoeffs;


nIirCoeffs = _nCoeffs;


}


if (nIirCoeffs != 20){


//Serial.println("Error, in AnalyzePhase, for IIR_LP_FILTER there must be 20 coefficients.");


nIirCoeffs = 20;


}


arm_biquad_cascade_df1_init_f32(&iir_inst, N_STAGES, pIirCoeffs, &IIRStateF32[0]);


}


else if (LPType==FIR_LP_FILTER) {


if(_pCoeffs != NULL){


pFirCoeffs = _pCoeffs;


nFirCoeffs = _nCoeffs;


}


if (nFirCoeffs<4  nFirCoeffs>NFIR_MAX) { // Too many or too few


//Serial.print("Error, in AnalyzePhase, for FIR_LP_FILTER there must be >4 and <=");


//Serial.print(NFIR_MAX);


//Serial.println(" coefficients.");


//Serial.println(" Restoring default IIR Filter.");


LPType = IIR_LP_FILTER;


pIirCoeffs = &iir_coeffs[0];


nIirCoeffs = 20; // Number of coefficients 20


pdConfig = 0b11100;


LPType = IIR_LP_FILTER; // Variables were set in setup() above


}


else { //Acceptable number, so initialize it


arm_fir_init_f32(&fir_inst, nFirCoeffs, pFirCoeffs, &FIRStateF32[0], block_size);


}


}


pdConfig = _pdConfig;


AudioInterrupts();


}




void showError(uint16_t e) {


errorPrint = e;


}




void update(void);




private:


float32_t sampleRate_Hz = AUDIO_SAMPLE_RATE_EXACT;


uint16_t block_size = AUDIO_BLOCK_SAMPLES;


// Two input data pointers


audio_block_f32_t *inputQueueArray_f32[2];


// Variables controlling the configuration


uint16_t LPType = IIR_LP_FILTER; // NO_LP_FILTER, IIR_LP_FILTER or FIR_LP_FILTER


float32_t *pIirCoeffs = &iir_coeffs[0]; // Coefficients for IIR


float32_t *pFirCoeffs = &fir_coeffs[0]; // Coefficients for FIR


uint16_t nIirCoeffs = 20; // Number of coefficients 20


uint16_t nFirCoeffs = 53; // Number of coefficients <=200


uint16_t pdConfig = 0b11100; // No limiter, fast acos, scale multiplier, radians out;


// Control error printing in update(). Should never be enabled


// until all audio objects have been initialized.


// Only used as 0 or 1 now, but 16 bits are available.


uint16_t errorPrint = 0;




// *Temporary*  TEST_TIME allows measuring time in microseconds for each part of the update()


#if TEST_TIME


elapsedMicros tElapse;


int32_t iitt = 998000; // count up to a million during startup


#endif




/* FIR filter designed with http://tfilter.appspot.com


* Sampling frequency: 44100 Hz


* 0 Hz  4000 Hz gain = 1.0, ripple = 0.101 dB


* 7000  22000 Hz attenuation >= 81.8 dB


* Suitable for measuring phase in communications systems with linear phase.


*/


float32_t fir_coeffs[53] = {


0.000206064,0.000525129,0.00083518, 0.000774011, 2.5925E05,


0.001614912, 0.003431897, 0.004335125, 0.003127158, 0.000566047,


0.005566484,0.009192163,0.008417443,0.001801824, 0.008839149,


0.018273049, 0.019879265, 0.009349346,0.011696836, 0.034389317,


0.045008839,0.030706279, 0.013824834, 0.082060266, 0.156328996,


0.213799940, 0.235420817, 0.213799940, 0.156328996, 0.082060266,


0.013824834,0.030706279,0.045008839,0.034389317, 0.011696836,


0.009349346, 0.019879265, 0.018273049, 0.008839149, 0.001801824,


0.008417443,0.009192163,0.005566484,0.000566047, 0.003127158,


0.004335125, 0.003431897, 0.001614912, 2.5925E05, 0.000774011,


0.000835180,0.000525129,0.000206064 };




// 8pole Biquad fc=0.0025fs, 80 dB Iowa Hills


// This is roughly the narrowest that doesn't have


// artifacts from numerical errors more than about


// 0.001 radians (0.06 deg), per experiments using F32.


// b0,b1,b2,a1,a2 for each BiQuad. Start with stage 0


float32_t iir_coeffs[5 * N_STAGES]={


0.08686551007982608,


0.1737214710369926,


0.08686551007982608,


1.9951804375779567,


0.9951899867006161,


// and stage 1


0.20909791845765324,


0.4181667739705088,


0.20909791845765324,


1.9965910753714984,


0.9966201383162961,


// stage 2


0.18360046797931723,


0.3671514768697197,


0.18360046797931723,


1.9981966389027592,


0.998246097991674,


// stage 3


0.03079484444321144,


0.061529427044071175,


0.03079484444321144,


1.999421284937329,


0.9994815467796806};




// ARM DSP Math library IIR filter instance


arm_biquad_casd_df1_inst_f32 iir_inst;




// And a FIR type, as either can be used via begin()


arm_fir_instance_f32 fir_inst;




// Delay line space for the FIR


float32_t FIRStateF32[AUDIO_BLOCK_SAMPLES + NFIR_MAX];




// Delay line space for the Biquad, each arranged as {x[n1], x[n2], y[n1], y[n2]}


float32_t IIRStateF32[4 * N_STAGES];


};


#endif


