From 426413a1695a7c51384790d08b3606ac82eedc23 Mon Sep 17 00:00:00 2001 From: boblark Date: Tue, 8 Mar 2022 20:52:46 -0800 Subject: [PATCH] Add AudioAlignLR_F32 --- AudioAlignLR_F32.cpp | 192 +++++++++++++++++++++++ AudioAlignLR_F32.h | 161 +++++++++++++++++++ OpenAudio_ArduinoLibrary.h | 1 + examples/TestTwinPeaks/TestTwinPeaks.ino | 167 ++++++++++++++++++++ 4 files changed, 521 insertions(+) create mode 100644 AudioAlignLR_F32.cpp create mode 100644 AudioAlignLR_F32.h create mode 100644 examples/TestTwinPeaks/TestTwinPeaks.ino diff --git a/AudioAlignLR_F32.cpp b/AudioAlignLR_F32.cpp new file mode 100644 index 0000000..afddc15 --- /dev/null +++ b/AudioAlignLR_F32.cpp @@ -0,0 +1,192 @@ +/*------------------------------------------------------------------------------------ + 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) { + audio_block_f32_t *block_i,*block_q; + audio_block_f32_t *blockOut_i,*blockOut_q; + audio_block_f32_t *blockOut_2; + uint16_t i, j, k; + // uint32_t t0 = micros(); // Measure time + + if(currentTPinfo.TPstate == TP_IDLE) return; + + block_i = AudioStream_F32::receiveWritable_f32(0); + if (!block_i) return; + + block_q = AudioStream_F32::receiveWritable_f32(1); + if (!block_q) { + AudioStream_F32::release(block_i); + return; + } + + blockOut_i = AudioStream_F32::allocate_f32(); + if (!blockOut_i) + { + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + return; + } + + blockOut_q = AudioStream_F32::allocate_f32(); + if (!blockOut_q) + { + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + AudioStream_F32::release(blockOut_i); + return; + } + blockOut_2 = AudioStream_F32::allocate_f32(); + if (!blockOut_2) + { + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + AudioStream_F32::release(blockOut_i); + AudioStream_F32::release(blockOut_q); + return; + } + + // One of these may be needed. They are saved for next update + TPextraI = block_i->data[127]; + TPextraQ = block_q->data[127]; + + // Find four cross-correlations for time shifted L-R combinations + 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 + { + currentTPinfo.xcVal[j] += block_i->data[k] * block_q->data[k+j]; + } + } + + // Decision time. Still in Measure. Can we leave? Need one more update()? + // Sort out the offset that is cross-correlated + if(currentTPinfo.nMeas>5) // Get past junk at startup + { + currentTPinfo.TPerror = ERROR_TP_NONE; // Change later if not true + needOneMore = true; // Change later if not true + if(currentTPinfo.xcVal[0]>TPthreshold && currentTPinfo.xcVal[2]<-TPthreshold) + currentTPinfo.neededShift = 0; + else if(currentTPinfo.xcVal[1]>TPthreshold && currentTPinfo.xcVal[3]<-TPthreshold) + currentTPinfo.neededShift = 1; + else if(currentTPinfo.xcVal[3]>TPthreshold && currentTPinfo.xcVal[1]<-TPthreshold) + currentTPinfo.neededShift = -1; + else // Don't have a combination above the threshold. + { + 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; + // Unless a reason for sending bad data come up, we will not send it: + //AudioStream_F32::transmit(block_i, 0); + //AudioStream_F32::transmit(block_q, 1); + } + } // End state==TP_MEASURE + + else if(currentTPinfo.TPstate == TP_RUN) + { + if(currentTPinfo.neededShift == 0) + { + // Serial.println("No shift"); + AudioStream_F32::transmit(block_i, 0); // Not shifted + AudioStream_F32::transmit(block_q, 1); // Not shifted + } + else if(currentTPinfo.neededShift == 1) + { + // Serial.println("Shift 1"); + blockOut_i->data[0] = TPextraI; // From last update + // block_i->data[127] is saved for next update, and not + // transmitted now. + for(i=1; i<128; i++) + blockOut_i->data[i] = block_i->data[i-1]; + AudioStream_F32::transmit(blockOut_i, 0); // Shifted + AudioStream_F32::transmit(block_q, 1); // Not shifted + } + else if(currentTPinfo.neededShift == -1) + { + // Serial.println("Shift -1"); + blockOut_q->data[0] = TPextraQ; + for(i=1; i<128; i++) + blockOut_q->data[i] = block_q->data[i-1]; + AudioStream_F32::transmit(block_i, 0); // Not shifted + AudioStream_F32::transmit(blockOut_q, 1); // Shifted + } + } // End state==TP_RUN + + + // TP_MEASURE needs a fs/4 test signal + if(currentTPinfo.TPstate == TP_MEASURE && + currentTPinfo.TPsignalHardware == TP_SIGNAL_CODEC) + { + for(int kk=0; kk<128; kk++) // Generate fs/4 square wave + { + // A +/- 0.8 square wave at fs/4 Hz + blockOut_2->data[kk] = -0.8+1.6*(float32_t)((kk/2)&1); + } + AudioStream_F32::transmit(blockOut_2, 2); // NOTE: Goes to third output + } + + currentTPinfo.nMeas++; + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + AudioStream_F32::release(blockOut_i); + AudioStream_F32::release(blockOut_q); + AudioStream_F32::release(blockOut_2); + // Serial.println(micros() - t0); // for timing + } + diff --git a/AudioAlignLR_F32.h b/AudioAlignLR_F32.h new file mode 100644 index 0000000..c57ee15 --- /dev/null +++ b/AudioAlignLR_F32.h @@ -0,0 +1,161 @@ +/* ------------------------------------------------------------------------------------ + AudioAlignLR_F32.h + + Function: Waits for CODEC startup and measures L-R time alignment errors. + Automatically corrects delay errors. Requires extra control to disable + analog audio path from DAC Q to both ADC I and ADC Q. + See the following for more background on the problem being solved: + https://forum.pjrc.com/threads/42336-Reset-audio-board-codec-SGTL5000-in-realtime-processing/page2 + https://forum.pjrc.com/threads/57362-AudioSDR-A-single-Audio-block-SDR-(software-defined-radio)-processor-demodulator/page3 + "Twin Peaks", or TP, comes from Frank, DD4WH who found, in hsi Convolution SDR, double spectral responses when + the L and R channels were out of time alignment. + + Author: Bob Larkin W7PUA + Date: 28 Feb 2022 + + Input: data_I, data_Q + Outputs: dataOut_I, dataOut_Q, dataTransmit_Q + + Update() time: About 12 microseconds for a T4.x. + + 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. +------------------------------------------------------------------------------- */ + +//REF: +// Manual I2S codec Phase Correction Utility by Ron Carr, Updated for F32, K7MDL +// Reference: [URL]https://forum.pjrc.com/threads/57362-AudioSDR-A-single-Audio-block-SDR-(software-defined-radio)-processor-demodulator?p=263048&viewfull=1#post263048[/URL] +// +// The variable, currentTPinfo.TPsignalHardware, changes source of +// the signal for going to L&R for correlation. +// If TP_SIGNAL_CODEC there needs to be a switchable hardware link between +// the DAC right channel and both of ADC right and ADC left. +// If TP_SIGNAL_IO_PIN the INO must update a roughly fs/4 +// square wave using an i/o pin. That signal is generated by +// the .INO as it is not possible in an audio object. +// Default is TP_SIGNAL_CODEC. +// +// BETA NOTE That calls and operations may change. BETA + +#ifndef audio_align_lr_f32_h_ +#define audio_align_lr_f32_h_ + +#include "AudioStream_F32.h" +#include "Arduino.h" + +#define TP_IDLE 0 +#define TP_MEASURE 1 +#define TP_RUN 2 + +#define TP_SIGNAL_CODEC 0 +#define TP_SIGNAL_IO_PIN 1 + +#define ERROR_TP_EARLY -2 +#define ERROR_TP_BAD_DATA -1 +#define ERROR_TP_NONE 0 + +// Needs to be available to INO and AudioAlignLR_F32 also. +struct TPinfo{ + uint16_t TPstate; + uint32_t nMeas; + float32_t xcVal[4]; // I-Q cross-correlation sums + int16_t neededShift; + int16_t TPerror; + uint16_t TPsignalHardware; + }; + +class AudioAlignLR_F32 : public AudioStream_F32 +{ +//GUI: inputs:2, outputs:3 //this line used for automatic generation of GUI node +//GUI: shortName: AlignLR +public: + AudioAlignLR_F32(uint16_t _hardware, uint16_t _controlPinNumber, bool _controlPinInvert): + AudioStream_F32(2,inputQueueArray) + { + currentTPinfo.TPsignalHardware = _hardware; + controlPinNumber = _controlPinNumber; + controlPinInvert = _controlPinInvert; + sample_rate_Hz = AUDIO_SAMPLE_RATE; + // Changes would be needed for block data sizes other than 128 + block_size = AUDIO_BLOCK_SAMPLES; + initTP(); + } + + AudioAlignLR_F32 + (uint16_t _hardware, uint16_t _controlPinNumber, bool _controlPinInvert, const AudioSettings_F32 &settings): + AudioStream_F32(2,inputQueueArray) + { + currentTPinfo.TPsignalHardware = _hardware; + controlPinNumber = _controlPinNumber; + controlPinInvert = _controlPinInvert; + sample_rate_Hz = settings.sample_rate_Hz; + block_size = settings.audio_block_samples; + initTP(); + } + + void initTP(void) { // Common calls for constructors, not INO's + currentTPinfo.TPstate = TP_IDLE; + currentTPinfo.neededShift = 0; + currentTPinfo.TPerror = ERROR_TP_EARLY; + needOneMore = false; + } + + // Returns all the status info, available anytime + TPinfo *read(void) { + return ¤tTPinfo; + } + + void stateAlignLR(int _TPstate) { + currentTPinfo.TPstate = _TPstate; + if(currentTPinfo.TPstate==TP_MEASURE) + { + currentTPinfo.nMeas = 0; // Timing for the DAC & ADC delays + currentTPinfo.neededShift = 0; + currentTPinfo.TPerror = ERROR_TP_EARLY; + needOneMore = true; + if(currentTPinfo.TPsignalHardware==TP_SIGNAL_CODEC) + { + pinMode(controlPinNumber, OUTPUT); // Turn on special audio path + digitalWrite(controlPinNumber, 1 & (uint16_t)controlPinInvert); + } + } + } + + void setThreshold(float32_t _TPthreshold) { + TPthreshold = _TPthreshold; + } + + virtual void update(void); + +private: + audio_block_f32_t *inputQueueArray[2]; + float32_t sample_rate_Hz = AUDIO_SAMPLE_RATE; + uint16_t controlPinNumber = 0; + bool controlPinInvert = true; + uint16_t block_size = 128; + bool needOneMore = false; + float32_t TPthreshold = 1.0f; + float32_t TPextraI = 0.0f; + float32_t TPextraQ = 0.0f; + TPinfo currentTPinfo; + bool outputFlag = false; + uint16_t count = 0; +}; +#endif diff --git a/OpenAudio_ArduinoLibrary.h b/OpenAudio_ArduinoLibrary.h index f6a957d..7928294 100644 --- a/OpenAudio_ArduinoLibrary.h +++ b/OpenAudio_ArduinoLibrary.h @@ -1,4 +1,5 @@ +#include #include #include #include diff --git a/examples/TestTwinPeaks/TestTwinPeaks.ino b/examples/TestTwinPeaks/TestTwinPeaks.ino new file mode 100644 index 0000000..58f84f4 --- /dev/null +++ b/examples/TestTwinPeaks/TestTwinPeaks.ino @@ -0,0 +1,167 @@ +/* TestTwinPeaks.ino Bob Larkin 26 Feb 2022 + * Tests the AlignLR class for finding the relative + * time order of the Codec ADC L and R channels, and then + * corrects these offsets. + * + * This applies a common analog square wave signal to the L and R inputs. + * The cross-correlation between the channels is found for + * the different offsets, showing the identical output. + * + * The outputs of the AlignLR object is then corrected, if necessary, + * by delaying either the L or R channel. + * + * This test allows the analog signal to come from either the Codec + * DAC or from a digital I/O pin on the Teensy. + * + */ + +#include "Audio.h" +#include "OpenAudio_ArduinoLibrary.h" + +// ******* MINI CONTROL PANEL ******* +// +// Pick one, based on the analog signal source harware being used: +//#define SIGNAL_HARDWARE TP_SIGNAL_CODEC +#define SIGNAL_HARDWARE TP_SIGNAL_IO_PIN +// +// Show the Teensy pin used for both Codec and I/O pin signal source methods +#define PIN_FOR_TP 2 +// +// Set threshold as needed. Examine 3 output data around update #15 +// and use about half of maximum positive value. +#define TP_THRESHOLD 11.0f +// +// Un-comment the next to print samples of the phase-adjusted L&R data +// #define PRINT_OUTPUT_DATA +// +// End Control Panel + +const float sample_rate_Hz = 44100.0f; +const int audio_block_samples = 128; +AudioSettings_F32 audio_settings(sample_rate_Hz, audio_block_samples); + +uint16_t nMeasLast = 0; +uint32_t timeSquareWave = 0; // Switch every 45 microseconds + +AudioInputI2S_F32 i2sIn; +// Pin or Codec Pin, Invert +AudioAlignLR_F32 TwinPeak(SIGNAL_HARDWARE, PIN_FOR_TP, false, audio_settings); +AudioOutputI2S_F32 i2sOut; +#ifdef PRINT_OUTPUT_DATA +AudioRecordQueue_F32 q1; +AudioRecordQueue_F32 q2; +#endif +AudioControlSGTL5000 codec; + +AudioConnection_F32 connection1(i2sIn, 0, TwinPeak, 0); +AudioConnection_F32 connection2(i2sIn, 1, TwinPeak, 1); + +#if SIGNAL_HARDWARE==TP_SIGNAL_CODEC +AudioConnection_F32 connection4(TwinPeak, 2, i2sOut, 0); // DAC L +AudioConnection_F32 connection6(TwinPeak, 2, i2sOut, 1); // DAC R +#endif + +// For test, send to queue. For real, send to receiver. +#ifdef PRINT_OUTPUT_DATA +AudioConnection_F32 connectionA(TwinPeak, 0, q1, 0); +AudioConnection_F32 connectionB(TwinPeak, 1, q2, 0); +#endif + +void setup(void) { + AudioMemory_F32(30, audio_settings); + Serial.begin(100); // Any rate + delay(500); + Serial.println("Twin Peaks L-R Synchronizer Test"); + + codec.inputSelect(AUDIO_INPUT_LINEIN); + codec.enable(); // This needs to preceed TwinPeak setup + +#if SIGNAL_HARDWARE==TP_SIGNAL_CODEC + Serial.println("Using SGTL5000 Codec output for cross-correlation test signal."); +#endif +#if SIGNAL_HARDWARE==TP_SIGNAL_IO_PIN + pinMode (PIN_FOR_TP, OUTPUT); // Digital output pin + Serial.println("Using I/O pin for cross-correlation test signal."); +#endif + + TwinPeak.setThreshold(TP_THRESHOLD); + TwinPeak.stateAlignLR(TP_MEASURE); // Comes up TP_IDLE + + Serial.println(""); + Serial.println("Update ------- Outputs -------"); + Serial.println("Number -1 0 1 Shift Error");// Column headings +} + +void loop(void) { + // The following, under PRINT_OUTPUT_DATA, is an output of the L & R channels + // suitable for examination in a spreadsheet. +#ifdef PRINT_OUTPUT_DATA +static int32_t ii=0; + if(ii==5) + { + q1.begin(); + q2.begin(); + } + if(ii>5 && ii<15) + { + if(q1.available()) + { + Serial.println(" ===================="); + float* pf1 = q1.readBuffer(); + for(int mm=0; mm<128; mm++) + Serial.println(*(pf1 + mm),5); + q1.freeBuffer(); + } + Serial.println("^--L"); + if(q2.available()) + { + float* pf2 = q2.readBuffer(); + for(int mm=0; mm<128; mm++) + Serial.println(*(pf2 + mm),5); + q2.freeBuffer(); + } + Serial.println("^--R"); + } + if(ii==16) + { + q1.end(); + q2.end(); + ii++; + } +#endif + + // uint32_t tt=micros(); + TPinfo* pData = TwinPeak.read(); + if(pData->nMeas > nMeasLast && pData->nMeas<20) + { + nMeasLast = pData->nMeas; // This print takes about 6 microseconds + Serial.print(pData->nMeas); Serial.print(", "); + Serial.print(pData->xcVal[3], 6); Serial.print(", "); + Serial.print(pData->xcVal[0], 6); Serial.print(", "); + Serial.print(pData->xcVal[1], 6); Serial.print(", "); + Serial.print(pData->neededShift); Serial.print(", "); + Serial.println(pData->TPerror); + //Serial.println(pData->TPstate); +#if SIGNAL_HARDWARE==TP_SIGNAL_IO_PIN + if(pData->TPerror == 0) + { + TwinPeak.stateAlignLR(TP_RUN); // TP is done + digitalWrite(PIN_FOR_TP, 0); + } +#endif + // Serial.println(micros()-tt); + } + +#if SIGNAL_HARDWARE==TP_SIGNAL_IO_PIN + // Generate 11.11 kHz square wave + // For other sample rates, set to roughly 2 sample periods, in microseconds + if(micros()-timeSquareWave >= 45 && pData->TPstate==TP_MEASURE) + { + static uint16_t squareWave = 0; + timeSquareWave = micros(); + squareWave = squareWave^1; + digitalWrite(PIN_FOR_TP, squareWave); + } +#endif + + }