From d1955a2ce0b6b0a8ed0ef29d05dfd6f1b7a3d22d Mon Sep 17 00:00:00 2001 From: boblark Date: Mon, 21 Mar 2022 16:26:40 -0700 Subject: [PATCH] added L and R BPF at fs/4 to AlignLR --- AudioAlignLR_F32.cpp | 258 ++++++++++++++++++++++- AudioAlignLR_F32.h | 134 +++++++++++- examples/TestTwinPeaks/TestTwinPeaks.ino | 2 +- 3 files changed, 386 insertions(+), 8 deletions(-) diff --git a/AudioAlignLR_F32.cpp b/AudioAlignLR_F32.cpp index a8e085d..3c26f86 100644 --- a/AudioAlignLR_F32.cpp +++ b/AudioAlignLR_F32.cpp @@ -1,3 +1,249 @@ + +/*------------------------------------------------------------------------------------ + AudioAlignLR_F32.cpp + + Author: Bob Larkin W7PUA + Date: 28 Feb 2022 + + See AudioAlignLR_F32.h for notes. + + Copyright (c) 2022 Robert 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. + ------------------------------------------------------------------------------- */ + +#include "AudioAlignLR_F32.h" + +void AudioAlignLR_F32::update(void) { static uint32_t nnn = 0; + audio_block_f32_t *block_L,*block_R; + audio_block_f32_t *block2_L,*block2_R; + audio_block_f32_t *blockOutTestSignal; + uint16_t i, j, k; + // uint32_t t0 = micros(); // Measure time + + if(currentTPinfo.TPstate == TP_IDLE) return; + + block_L = AudioStream_F32::receiveWritable_f32(0); + if (!block_L) return; + + block_R = AudioStream_F32::receiveWritable_f32(1); + if (!block_R) { + AudioStream_F32::release(block_L); + return; + } + + block2_L = AudioStream_F32::allocate_f32(); + if (!block2_L) + { + AudioStream_F32::release(block_L); + AudioStream_F32::release(block_R); + return; + } + + block2_R = AudioStream_F32::allocate_f32(); + if (!block2_R) + { + AudioStream_F32::release(block_L); + AudioStream_F32::release(block_R); + AudioStream_F32::release(block2_L); + return; + } + + if(currentTPinfo.TPsignalHardware == TP_SIGNAL_CODEC) + { + blockOutTestSignal = AudioStream_F32::allocate_f32(); + if (!blockOutTestSignal) + { + AudioStream_F32::release(block_L); + AudioStream_F32::release(block_R); + AudioStream_F32::release(block2_L); + AudioStream_F32::release(block2_R); + return; + } + } + + // Input data is now in block_L and block_R. Filter from there to + // block2_L and block2_R + if(useLRfilter) + { + arm_fir_f32(&fir_instL, block_L->data, block2_L->data, block_L->length); + arm_fir_f32(&fir_instR, block_R->data, block2_R->data, block_R->length); + } + else + for(i=0; ilength; i++) + { + block2_L->data[i] = block_L->data[i]; + block2_R->data[i] = block_R->data[i]; + } + // Input data is now in block_L and block_R. Filtered data is in + // block2_L and block2_R + + // One of these next 2 may be needed. They are saved for next update + TPextraL = block_L->data[127]; + TPextraR = block_R->data[127]; + + // Find four cross-correlations for time shifted L-R combinations. +// Use filtered data + if(currentTPinfo.TPstate==TP_MEASURE) + { + currentTPinfo.xcVal[0]=0.0f; // In phase + currentTPinfo.xcVal[1]=0.0f; // Shift I + currentTPinfo.xcVal[2]=0.0f; // DNApply, shift 2 time slots + currentTPinfo.xcVal[3]=0.0f; // Shift Q + for(j=0; j<4; j++) + { + for(k=0; k<124; k++) // Use sum of 124 x-c values on filtered data + { + currentTPinfo.xcVal[j] += block2_L->data[k] * block2_R->data[k+j]; + } + } + currentTPinfo.xNorm = 0.0f; + for(k=0; k<4; k++) + currentTPinfo.xNorm += fabsf( currentTPinfo.xcVal[k] ); + + // Decision time. Still in Measure. Can we leave? Need one more update()? + // Sort out the offset that is cross-correlated + if(currentTPinfo.nMeas>5 && currentTPinfo.xNorm > 0.0001f) // Get past junk at startup + { + currentTPinfo.TPerror = ERROR_TP_NONE; // Change later if not true + needOneMore = true; // Change later if not true + + // Redo (12 March 2022) with normalized values + float32_t xcN[4]; + for(j=0; j<4; j++) + xcN[j] = currentTPinfo.xcVal[j]/currentTPinfo.xNorm; + // Look for a good cross-correlation with the normalized values + if(xcN[0] - xcN[2] > 0.75f) + currentTPinfo.neededShift = 0; + else if(xcN[1] - xcN[3] > 0.75f) + currentTPinfo.neededShift = 1; + else if(xcN[3] - xcN[1] > 0.75f) + currentTPinfo.neededShift = -1; + else // Don't have a combination awith much cross-correlation + { + currentTPinfo.neededShift = 0; // Just a guess + currentTPinfo.TPerror = ERROR_TP_BAD_DATA; // Probably no, or low signal + needOneMore = false; // Not yet + } + } + + if(currentTPinfo.nMeas>5 && needOneMore==false && currentTPinfo.TPerror==ERROR_TP_NONE) + { + needOneMore = true; // Last may have been partial data set + } + else if(needOneMore==true && currentTPinfo.TPerror==ERROR_TP_NONE) // We're done measuring + { + currentTPinfo.TPstate = TP_RUN; // Not TP_MEASURE. State doesn't change from here on. + needOneMore = false; + if(currentTPinfo.TPsignalHardware == TP_SIGNAL_CODEC) + digitalWrite(controlPinNumber, 0 ^ (uint16_t)controlPinInvert); // Stop test audio + // Serial.println("Stop Square Wave audio path"); + } + else // Try again from the start + { + // Serial.println("Re-start TP Measure"); + currentTPinfo.TPstate = TP_MEASURE; + if(currentTPinfo.TPsignalHardware==TP_SIGNAL_CODEC) + digitalWrite(controlPinNumber, 1 & (uint16_t)controlPinInvert); + currentTPinfo.neededShift = 0; + currentTPinfo.TPerror = ERROR_TP_EARLY; + needOneMore = false; + } + } // End state==TP_MEASURE + + else if(currentTPinfo.TPstate == TP_RUN) + { + if(currentTPinfo.neededShift == 0) + { + // Serial.println("No shift"); + } + else if(currentTPinfo.neededShift == 1) + { + // Serial.println("Shift 1"); + for(i=127; i>0; i--) + block_L->data[i] = block_L->data[i-1]; // Move all down one + block_L->data[0] = TPextraL; // From last update + // Note: block_L->data[127] is saved for next update, and not + // transmitted now. + } + else if(currentTPinfo.neededShift == -1) + { + // Serial.println("Shift -1"); + for(i=127; i>0; i--) + block_R->data[i] = block_R->data[i-1]; + block_R->data[0] = TPextraR; + } + // Only transmit data in TP_RUN + AudioStream_F32::transmit(block_L, 0); // Shifted + AudioStream_F32::transmit(block_R, 1); // as needed + } // End state==TP_RUN + + // But, always release the blocks + AudioStream_F32::release(block_L); + AudioStream_F32::release(block_R); + AudioStream_F32::release(block2_L); + AudioStream_F32::release(block2_R); + + // TP_MEASURE needs a fs/4 test signal + if(currentTPinfo.TPsignalHardware == TP_SIGNAL_CODEC) + { + if(currentTPinfo.TPstate == TP_MEASURE) + { + for(int kk=0; kk<128; kk++) // Generate fs/4 square wave + { + // A +/- 0.8 square wave at fs/4 Hz + blockOutTestSignal->data[kk] = -0.8+1.6*(float32_t)((kk/2)&1); + } + } + AudioStream_F32::transmit(blockOutTestSignal, 2); // NOTE: Goes to third output + AudioStream_F32::release(blockOutTestSignal); + } + + currentTPinfo.nMeas++; + // Serial.println(micros() - t0); // for timing + } + + + + +#if 0 + + +/* +Twin Peaks L-R Synchronizer Test +Using I/O pin for cross-correlation test signal. + +Update ------------ Outputs ------------ +Number xNorm -1 0 1 Shift Error State +7, 130.039429, -64.215202, -0.259859, 64.248093, 1, 0, 2 + + +Update ------------ Outputs ------------ +Number xNorm -1 0 1 Shift Error State +7, 127.209366, -62.500359, -1.016678, 62.662319, 1, 0, 2 FILTER + + +7, 129.447464, 3.015868, 62.782230, -1.023435, 0, 0, 2 NO FILTER +*/ + +Update ------------ Outputs ------------ +Number xNorm -1 0 1 Shift Error State +7, 132.382294, 2.304047, 64.691902, -0.716843, 0, 0, 2 /*------------------------------------------------------------------------------------ AudioAlignLR_F32.cpp @@ -73,8 +319,8 @@ void AudioAlignLR_F32::update(void) { } // One of these may be needed. They are saved for next update - TPextraI = block_i->data[127]; - TPextraQ = block_q->data[127]; + TPextraL = block_i->data[127]; + TPextraR = block_q->data[127]; // Find four cross-correlations for time shifted L-R combinations if(currentTPinfo.TPstate==TP_MEASURE) @@ -158,7 +404,7 @@ void AudioAlignLR_F32::update(void) { else if(currentTPinfo.neededShift == 1) { // Serial.println("Shift 1"); - blockOut_i->data[0] = TPextraI; // From last update + blockOut_i->data[0] = TPextraL; // From last update // block_i->data[127] is saved for next update, and not // transmitted now. for(i=1; i<128; i++) @@ -169,7 +415,7 @@ void AudioAlignLR_F32::update(void) { else if(currentTPinfo.neededShift == -1) { // Serial.println("Shift -1"); - blockOut_q->data[0] = TPextraQ; + blockOut_q->data[0] = TPextraR; for(i=1; i<128; i++) blockOut_q->data[i] = block_q->data[i-1]; AudioStream_F32::transmit(block_i, 0); // Not shifted @@ -199,3 +445,7 @@ void AudioAlignLR_F32::update(void) { // Serial.println(micros() - t0); // for timing } + + + +#endif diff --git a/AudioAlignLR_F32.h b/AudioAlignLR_F32.h index 73d66a6..0b0c9f5 100644 --- a/AudioAlignLR_F32.h +++ b/AudioAlignLR_F32.h @@ -58,7 +58,7 @@ /* This is best placed in the setup() part of the INO. A signal needs to be generated * and this would look like: - uint32_t timeSquareWave = 0; + uint32_t timeSquareWave = 0; #define PIN_FOR_TP 2 pinMode (PIN_FOR_TP, OUTPUT); // Digital output pin @@ -140,6 +140,9 @@ public: currentTPinfo.neededShift = 0; currentTPinfo.TPerror = ERROR_TP_EARLY; needOneMore = false; + // Initialize FIR instance (ARM DSP Math Library) + arm_fir_init_f32(&fir_instL, 101, firBP, &StateF32L[0], block_size); + arm_fir_init_f32(&fir_instR, 101, firBP, &StateF32R[0], block_size); } // Returns all the status info, available anytime @@ -163,10 +166,17 @@ public: } } +/* void setThreshold(float32_t _TPthreshold) { //TPthreshold = _TPthreshold; Serial.println("ERROR: Threshold is no longer used. 12 Mar 2022"); } + */ + + // Do we need this after experiments are done? <<<<<<<<<<<<<<<<<<<<<< + void setLRfilter(bool _useLRfilter) { + useLRfilter = _useLRfilter; + } virtual void update(void); @@ -178,10 +188,128 @@ private: uint16_t block_size = 128; bool needOneMore = false; // float32_t TPthreshold = 1.0f; - float32_t TPextraI = 0.0f; - float32_t TPextraQ = 0.0f; + float32_t TPextraL = 0.0f; + float32_t TPextraR = 0.0f; TPinfo currentTPinfo; bool outputFlag = false; uint16_t count = 0; + bool useLRfilter = true; + + // ARM DSP Math library filter instances + arm_fir_instance_f32 fir_instL; + float32_t StateF32L[229]; + + arm_fir_instance_f32 fir_instR; + float32_t StateF32R[229]; + + /* Sampling frequency: 48000 Hz + * 0 Hz - 10100 Hz attenuation = 39.7 dB + * 10800 Hz - 13200 Hz ripple = 1.2 dB + * 13900 Hz - 24000 Hz attenuation = 39.7 dB + * NOTE: This will scale with sample rate and stay withpassband at fs/4. + */ + float32_t firBP[101] = { + 0.0010784874240328994, + -1.8511940285394994e-15, + 0.007173020540888694, + -2.446906631093228e-15, + -0.007077486809913102, + -2.3412063121164944e-15, + 0.009011618946946028, + -2.5923206852409818e-15, + -0.010712293622384057, + -2.3964501699408722e-15, + 0.011482971456733491, + -2.666527614775755e-15, + -0.010885141080842342, + -2.4082964205096483e-15, + 0.008647733991415402, + -2.4633438337819814e-15, + -0.004721228640000285, + -2.479178552837757e-15, + -0.0006641071730367476, + -2.5286128045656088e-15, + 0.006973730987499941, + -2.477532927164074e-15, + -0.01339871502510467, + -2.431267551488873e-15, + 0.01892093480750003, + -2.4098508014460433e-15, + -0.022430541924076213, + -2.405917616535165e-15, + 0.02285350193041076, + -2.509429940385098e-15, + -0.01935677493053742, + -2.483893222086823e-15, + 0.01141749520487472, + -2.601900577555639e-15, + 0.000985366589667628, + -2.5322636675246416e-15, + -0.01735448964139692, + -2.592937123951427e-15, + 0.03668097751462642, + -2.617655860016599e-15, + -0.057518688568419914, + -2.3833597705424817e-15, + 0.07812082879163866, + -2.532991478487954e-15, + -0.09663455293743198, + -2.412807130934187e-15, + 0.11132156511296272, + -2.5145289109982724e-15, + -0.12075435023719062, + -2.4314403797559724e-15, + 0.12400702102359241, + -2.4314403797559724e-15, + -0.12075435023719062, + -2.5145289109982724e-15, + 0.11132156511296272, + -2.412807130934187e-15, + -0.09663455293743198, + -2.532991478487954e-15, + 0.07812082879163866, + -2.3833597705424817e-15, + -0.057518688568419914, + -2.617655860016599e-15, + 0.03668097751462642, + -2.592937123951427e-15, + -0.01735448964139692, + -2.5322636675246416e-15, + 0.000985366589667628, + -2.601900577555639e-15, + 0.01141749520487472, + -2.483893222086823e-15, + -0.01935677493053742, + -2.509429940385098e-15, + 0.02285350193041076, + -2.405917616535165e-15, + -0.022430541924076213, + -2.4098508014460433e-15, + 0.01892093480750003, + -2.431267551488873e-15, + -0.01339871502510467, + -2.477532927164074e-15, + 0.006973730987499941, + -2.5286128045656088e-15, + -0.0006641071730367476, + -2.479178552837757e-15, + -0.004721228640000285, + -2.4633438337819814e-15, + 0.008647733991415402, + -2.4082964205096483e-15, + -0.010885141080842342, + -2.666527614775755e-15, + 0.011482971456733491, + -2.3964501699408722e-15, + -0.010712293622384057, + -2.5923206852409818e-15, + 0.009011618946946028, + -2.3412063121164944e-15, + -0.007077486809913102, + -2.446906631093228e-15, + 0.007173020540888694, + -1.8511940285394994e-15, + 0.0010784874240328994}; + }; #endif diff --git a/examples/TestTwinPeaks/TestTwinPeaks.ino b/examples/TestTwinPeaks/TestTwinPeaks.ino index b1d4810..c6d7864 100644 --- a/examples/TestTwinPeaks/TestTwinPeaks.ino +++ b/examples/TestTwinPeaks/TestTwinPeaks.ino @@ -79,7 +79,7 @@ AudioConnection_F32 connection7(TwinPeak, 1, q2, 0); #endif void setup(void) { - uint32_t tMillis = millis(); + static uint32_t tMillis = millis(); AudioMemory_F32(50, audio_settings); Serial.begin(100); // Any rate