From ae99656d297540dbd8abde5cd2ac3eba5449b09b Mon Sep 17 00:00:00 2001 From: boblark Date: Sun, 28 Jun 2020 11:49:04 -0700 Subject: [PATCH] Add AudioFilter 90Deg and RadioIQMixer plus support for these. --- AudioEffectDelay_OA_F32.cpp | 235 ++++++++++++++ AudioEffectDelay_OA_F32.h | 152 +++++++++ AudioFilter90Deg_F32.cpp | 95 ++++++ AudioFilter90Deg_F32.h | 153 +++++++++ OpenAudio_ArduinoLibrary.h | 3 + RadioIQMixer_F32.cpp | 106 +++++++ RadioIQMixer_F32.h | 138 +++++++++ .../FineFreqShift_OA/FineFreqShift_OA.ino | 290 ++++++++++++++++++ examples/FineFreqShift_OA/hilbert121A.h | 123 ++++++++ examples/FineFreqShift_OA/hilbert251A.h | 253 +++++++++++++++ mathDSP_F32.cpp | 114 +++++++ mathDSP_F32.h | 50 +++ readme.md | 6 + sinTable512_f32.h | 98 ++++++ 14 files changed, 1816 insertions(+) create mode 100644 AudioEffectDelay_OA_F32.cpp create mode 100644 AudioEffectDelay_OA_F32.h create mode 100644 AudioFilter90Deg_F32.cpp create mode 100644 AudioFilter90Deg_F32.h create mode 100644 RadioIQMixer_F32.cpp create mode 100644 RadioIQMixer_F32.h create mode 100644 examples/FineFreqShift_OA/FineFreqShift_OA.ino create mode 100644 examples/FineFreqShift_OA/hilbert121A.h create mode 100644 examples/FineFreqShift_OA/hilbert251A.h create mode 100644 mathDSP_F32.cpp create mode 100644 mathDSP_F32.h create mode 100644 sinTable512_f32.h diff --git a/AudioEffectDelay_OA_F32.cpp b/AudioEffectDelay_OA_F32.cpp new file mode 100644 index 0000000..3c0e048 --- /dev/null +++ b/AudioEffectDelay_OA_F32.cpp @@ -0,0 +1,235 @@ +/* + * + * + * Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * Extended by Chip Audette, Open Audio, April 2018 + * + * 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. + */ + +#include +#include "AudioEffectDelay_OA_F32.h" + +void AudioEffectDelay_OA_F32::update(void) +{ + //Serial.println("AudioEffectDelay_OA_F32: update()..."); + receiveIncomingData(); //put the in-coming audio data into the queue + discardUnneededBlocksFromQueue(); //clear out queued data this is no longer needed + transmitOutgoingData(); //put the queued data into the output + return; +} + +void AudioEffectDelay_OA_F32::receiveIncomingData(void) { + //Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: starting..."); + + //prepare the receiving queue + uint16_t head = headindex; //what block to write to + uint16_t tail = tailindex; //what block to read from + if (queue[head] == NULL) { + //if (!Serial) Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: Allocating queue[head]."); + queue[head] = allocate_f32(); + if (queue[head] == NULL) { + //if (!Serial) Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: Null memory 1. Returning."); + return; + } + } + + //prepare target memory nto which we'll copy the incoming data into the queue + int dest_ind = writeposition; //inclusive + if (dest_ind >= (queue[head]->full_length)) { + head++; dest_ind = 0; + if (head >= DELAY_QUEUE_SIZE) head = 0; + if (queue[head] != NULL) { + if (head==tail) {tail++; if (tail >= DELAY_QUEUE_SIZE) tail = 0; } + AudioStream_F32::release(queue[head]); + queue[head]=NULL; + } + } + if (queue[head]==NULL) { + queue[head] = allocate_f32(); + if (queue[head] == NULL) { + if (!Serial) Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: Null memory 2. Returning."); + return; + } + } + + //receive the in-coming audio data block + audio_block_f32_t *input = receiveReadOnly_f32(); + if (input == NULL) { + //if (!Serial) Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: Input data is NULL. Returning."); + return; + } + int n_copy = input->length; + last_received_block_id = input->id; + + // Now we'll loop over the individual samples of the in-coming data + float32_t *dest = queue[head]->data; + float32_t *source = input->data; + int end_write = dest_ind + n_copy; //this may go past the end of the destination array + int end_loop = min(end_write,(int)(queue[head]->full_length)); //limit to the end of the array + int src_count=0, dest_count=dest_ind; + for (int i=dest_ind; i= DELAY_QUEUE_SIZE) head = 0; + if (queue[head] != NULL) { + if (head==tail) {tail++; if (tail >= DELAY_QUEUE_SIZE) tail = 0; } + AudioStream_F32::release(queue[head]); + queue[head]=NULL; + } + + if (queue[head]==NULL) { + queue[head] = allocate_f32(); + if (queue[head] == NULL) { + Serial.println("AudioEffectDelay_OA_F32::receiveIncomingData: Null memory 3. Returning."); + AudioStream_F32::release(input); + return; + } + } + float32_t *dest = queue[head]->data; + end_loop = end_write - (queue[head]->full_length); + dest_count = dest_ind; + for (int i=dest_ind; i < end_loop; i++) dest[dest_count++]=source[src_count++]; + } + + AudioStream_F32::release(input); + writeposition = dest_count; + headindex = head; + tailindex = tail; + return; +} + + +void AudioEffectDelay_OA_F32::discardUnneededBlocksFromQueue(void) { + uint16_t head = headindex; //what block to write to + uint16_t tail = tailindex; //last useful block of data + uint32_t count; + + // discard unneeded blocks from the queue + if (head >= tail) { + count = head - tail; + } else { + count = DELAY_QUEUE_SIZE + head - tail; + } +/* if (head>0) { + Serial.print("AudioEffectDelay_OA_F32::discardUnneededBlocksFromQueue: head, tail, count, maxblocks, DELAY_QUEUE_SIZE: "); + Serial.print(head); Serial.print(", "); + Serial.print(tail); Serial.print(", "); + Serial.print(count); Serial.print(", "); + Serial.print(maxblocks); Serial.print(", "); + Serial.print(DELAY_QUEUE_SIZE); Serial.print(", "); + Serial.println(); + } */ + if (count > maxblocks) { + count -= maxblocks; + do { + if (queue[tail] != NULL) { + AudioStream_F32::release(queue[tail]); + queue[tail] = NULL; + } + if (++tail >= DELAY_QUEUE_SIZE) tail = 0; + } while (--count > 0); + } + tailindex = tail; +} + +void AudioEffectDelay_OA_F32::transmitOutgoingData(void) { + uint16_t head = headindex; //what block to write to + //uint16_t tail = tailindex; //last useful block of data + audio_block_f32_t *output; + int channel; //, index, prev, offset; + //const float32_t *src, *end; + //float32_t *dst; + + // transmit the delayed outputs using queue data + for (channel = 0; channel < 8; channel++) { + if (!(activemask & (1<length; + if (ref_samp_long < offset_samp) { //when (ref_samp_long - offset_samp) goes negative, the uint32_t will fail, so we do this logic check + ref_samp_long = ref_samp_long + (((uint32_t)(DELAY_QUEUE_SIZE))*((uint32_t)AUDIO_BLOCK_SIZE_F32)); + } + ref_samp_long = ref_samp_long - offset_samp; + uint16_t source_queue_ind = (uint16_t)(ref_samp_long / ((uint32_t)AUDIO_BLOCK_SIZE_F32)); + int source_samp = (int)(ref_samp_long - (((uint32_t)source_queue_ind)*((uint32_t)AUDIO_BLOCK_SIZE_F32))); + + //pull the data from the first source data block + int dest_counter=0; + int n_output = output->length; + float32_t *dest = output->data; + audio_block_f32_t *source_block = queue[source_queue_ind]; + if (source_block == NULL) { + //fill destination with zeros for this source block + int Iend = min(source_samp+n_output,AUDIO_BLOCK_SIZE_F32); + for (int Isource = source_samp; Isource < Iend; Isource++) { + dest[dest_counter++]=0.0; + } + } else { + //fill destination with this source block's values + float32_t *source = source_block->data; + int Iend = min(source_samp+n_output,AUDIO_BLOCK_SIZE_F32); + for (int Isource = source_samp; Isource < Iend; Isource++) { + dest[dest_counter++]=source[Isource]; + } + } + + //pull the data from the second source data block, if needed + if (dest_counter < n_output) { + //yes, we need to keep filling the output + int Iend = n_output - dest_counter; //how many more data points do we need + source_queue_ind++; source_samp = 0; //which source block will we draw from (and reset the source sample counter) + if (source_queue_ind >= DELAY_QUEUE_SIZE) source_queue_ind = 0; //wrap around on our source black. + source_block = queue[source_queue_ind]; //get the source block + if (source_block == NULL) { //does it have data? + //no, it doesn't have data. fill destination with zeros + for (int Isource = source_samp; Isource < Iend; Isource++) { + dest[dest_counter++] = 0.0; + } + } else { + //source block does have data. use this block's values + float32_t *source = source_block->data; + for (int Isource = source_samp; Isource < Iend; Isource++) { + dest[dest_counter++]=source[Isource]; + } + } + } + + //add the id of the last received audio block + output->id = last_received_block_id; + + //transmit and release + AudioStream_F32::transmit(output, channel); + AudioStream_F32::release(output); + } +} + + diff --git a/AudioEffectDelay_OA_F32.h b/AudioEffectDelay_OA_F32.h new file mode 100644 index 0000000..8dc8f7d --- /dev/null +++ b/AudioEffectDelay_OA_F32.h @@ -0,0 +1,152 @@ +/* + * AudioEffectDelay_OA_F32.h + * + * Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * Extended by Chip Audette, Open Audio, Apr 2018 + * Isolated names for Open Audio (F32) RSL June 2020 + * + * 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. + */ + +#ifndef AudioEffecDelay_OA_F32_h_ +#define AudioEffecDelay_OA_F32_h_ + +#include "Arduino.h" +#include "AudioStream_F32.h" +#include "utility/dspinst.h" + +#define AUDIO_BLOCK_SIZE_F32 AUDIO_BLOCK_SAMPLES //what is the maximum length of the F32 audio blocks + +// Are these too big??? I think the same as I16---half as much? <<<<<<<<<<<<<<<< +#if defined(__IMXRT1062__) + // 4.00 second maximum on Teensy 4.0 + #define DELAY_QUEUE_SIZE (176512 / AUDIO_BLOCK_SAMPLES) +#elif defined(__MK66FX1M0__) + // 2.41 second maximum on Teensy 3.6 + #define DELAY_QUEUE_SIZE (106496 / AUDIO_BLOCK_SIZE_F32) +#elif defined(__MK64FX512__) + // 1.67 second maximum on Teensy 3.5 + #define DELAY_QUEUE_SIZE (73728 / AUDIO_BLOCK_SIZE_F32) +#elif defined(__MK20DX256__) + // 0.45 second maximum on Teensy 3.1 & 3.2 + #define DELAY_QUEUE_SIZE (19826 / AUDIO_BLOCK_SIZE_F32) +#else + // 0.14 second maximum on Teensy 3.0 + #define DELAY_QUEUE_SIZE (6144 / AUDIO_BLOCK_SIZE_F32) +#endif + +class AudioEffectDelay_OA_F32 : public AudioStream_F32 +{ +//GUI: inputs:1, outputs:1 //this line used for automatic generation of GUI node +//GUI: shortName:delay +public: + AudioEffectDelay_OA_F32() : AudioStream_F32(1, inputQueueArray) { + activemask = 0; + headindex = 0; + tailindex = 0; + maxblocks = 0; + memset(queue, 0, sizeof(queue)); + } + AudioEffectDelay_OA_F32(const AudioSettings_F32 &settings) : + AudioStream_F32(1,inputQueueArray) { + activemask = 0; + headindex = 0; + tailindex = 0; + maxblocks = 0; + memset(queue, 0, sizeof(queue)); + setSampleRate_Hz(settings.sample_rate_Hz); + } + void setSampleRate_Hz(float _fs_Hz) { + //Serial.print("AudioEffectDelay_OA_F32: setSampleRate_Hz to "); + //Serial.println(_fs_Hz); + sampleRate_Hz = _fs_Hz; + //audio_block_len_samples = block_size; //this is the actual size that is being used in each audio_block_f32 + } + void delay(uint8_t channel, float milliseconds) { + if (channel >= 8) return; + if (milliseconds < 0.0) milliseconds = 0.0; + uint32_t n = (milliseconds*(sampleRate_Hz/1000.0))+0.5; + uint32_t nmax = AUDIO_BLOCK_SIZE_F32 * (DELAY_QUEUE_SIZE-1); + if (n > nmax) n = nmax; + uint32_t blks = (n + (AUDIO_BLOCK_SIZE_F32-1)) / AUDIO_BLOCK_SIZE_F32 + 1; + if (!(activemask & (1< maxblocks) maxblocks = blks; + activemask |= (1< delay_samps[channel]) { + // new delay is greater than previous setting + if (blks > maxblocks) maxblocks = blks; + delay_samps[channel] = n; + } else { + // new delay is less than previous setting + delay_samps[channel] = n; + recompute_maxblocks(); + } + } + } + void disable(uint8_t channel) { + if (channel >= 8) return; + // diable this channel + activemask &= ~(1< max) max = n; + } + } while(++channel < 8); + maxblocks = max; + } + uint8_t activemask; // which output channels are active + uint16_t headindex; // head index (incoming) data in queue + uint16_t tailindex; // tail index (outgoing) data from queue + uint16_t maxblocks; // number of blocks needed in queue +//#if DELAY_QUEUE_SIZE * AUDIO_BLOCK_SAMPLES < 65535 +// uint16_t writeposition; +// uint16_t delay_samps[8]; // # of samples to delay for each channel +//#else + int writeposition; //position within current head buffer in the queue + uint32_t delay_samps[8]; // # of samples to delay for each channel +//#endif + audio_block_f32_t *queue[DELAY_QUEUE_SIZE]; + audio_block_f32_t *inputQueueArray[1]; + float sampleRate_Hz = AUDIO_SAMPLE_RATE_EXACT; //default. from AudioStream.h?? + //int audio_block_len_samples = AUDIO_BLOCK_SAMPLES; + void receiveIncomingData(void); + void discardUnneededBlocksFromQueue(void); + void transmitOutgoingData(void); + unsigned long last_received_block_id = 0; +}; + +#endif diff --git a/AudioFilter90Deg_F32.cpp b/AudioFilter90Deg_F32.cpp new file mode 100644 index 0000000..1016ece --- /dev/null +++ b/AudioFilter90Deg_F32.cpp @@ -0,0 +1,95 @@ +/* + * AudioFilter90Deg_F32.cpp + * + * 22 March 2020 + * Bob Larkin, in support of the library: + * Chip Audette, OpenAudio, Apr 2017 + * ------------------- + * There are two channels that update synchronously, but operate + * independently. The I-channel, coresponding to a "Left" + * audio channel, is filtered with an N coefficient + * Hilbert Transform in FIR form. + * The Q-channel, or "Right" channel, is simply + * delayed by the (N-1)/2 sample periods. The phase between + * the two outputs is 90-Degrees. The amplitude response cuts off low + * frequencies and depends on N. + * + * The I-channel FIR is a Hilbert Transform and this has every other term 0. + * These need not be multiplied-and-accumulated, but for now, we do. By the + * time the indexes are calculated, it may be quicker to process everything. + * Additionally, to not require a half-sample delay, the number of terms in + * the Hilbert FIR needs to be odd. + * + * MIT License, Use at your own risk. +*/ + +#include "AudioFilter90Deg_F32.h" + +void AudioFilter90Deg_F32::update(void) +{ + audio_block_f32_t *block_i, *block_q, *blockOut_i=NULL; + uint16_t i; + + // Get first input, i, that will be filtered + block_i = AudioStream_F32::receiveWritable_f32(0); + if (!block_i) { + if(errorPrint) Serial.println("FIL90-ERR: No i input memory"); + return; + } + + // Get second input, q, that will be delayed + block_q = AudioStream_F32::receiveWritable_f32(1); + if (!block_q){ + if(errorPrint) Serial.println("FIL90-ERR: No q input memory"); + AudioStream_F32::release(block_i); + return; + } + + // If there's no coefficient table, give up. + if (coeff_p == NULL) { + if(errorPrint) Serial.println("FIL90-ERR: No Hilbert FIR coefficients"); + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + return; + } + + // Try to get a block for the FIR output + blockOut_i = AudioStream_F32::allocate_f32(); + if (!blockOut_i){ // Didn't have any + if(errorPrint) Serial.println("FIL90-ERR: No out 0 block"); + AudioStream_F32::release(block_i); + AudioStream_F32::release(block_q); + return; + } + + // Apply the Hilbert transform FIR. This includes multiplies by zero. + // Is there any way to play with the indexes and not multiply by zero + // without spending more time than is saved? How about something like + // pick right nn, then for(j=0; jdata, blockOut_i->data, block_i->length); + AudioStream_F32::release(block_i); // Not needed further + + // Now enter block_size points to the delay loop and move earlier points to re-used block + float32_t *pin, *pout; + pin = &(block_q->data[0]); // point to beginning of block_q + for(i=0; idata[0]); // Re-use the input block + for(i=0; i 251"); + } + + // For the equalizing delay in q, if n_coeffs==19, n_delay=9 + // Max of 251 coeffs needs a delay of 125 sample periods. + n_delay = (uint8_t)((n_coeffs-1)/2); + in_index = n_delay; + out_index = 0; + for (uint16_t i=0; i<256; i++){ + delayData[i] = 0.0F; + } + } // End of begin() + + void showError(uint16_t e) { + errorPrint = e; + } + + void update(void); + +private: + uint16_t block_size = AUDIO_BLOCK_SAMPLES; + // Two input data pointers + audio_block_f32_t *inputQueueArray_f32[2]; + // One output pointer + audio_block_f32_t *blockOut_i; + +#if TEST_TIME_90D +// *Temporary* - allows measuring time in microseconds for each part of the update() +elapsedMicros tElapse; +int32_t iitt = 999000; // count up to a million during startup +#endif + + // Control error printing in update() 0=No print + uint16_t errorPrint = 0; + + //float32_t tmpHil[5]={0.0, 1.0, 0.0, -1.0, 0.0}; coeff_p = &tmpHil[0]; + // pointer to current coefficients or NULL + const float32_t *coeff_p = NULL; + uint16_t n_coeffs = 0; + + // Variables for the delayed q-channel: + // For the q-channel, we need a delay of ((Ncoeff - 1) / 2) samples. This + // is 9 delay for 19 coefficient FIR. This can be implemented as a simple circular + // buffer if we make the buffer a power of 2 in length and binary-truncate the index. + // Choose 2^8 = 256. For a 251 long Hilbert this wastes 256-128-125 = 3, but + // more for shorter Hilberts. + float32_t delayData[256]; // The circular delay line + uint16_t in_index; + uint16_t out_index; + // And a mask to make the circular buffer limit to a power of 2 + uint16_t delayBufferMask = 0X00FF; + uint16_t n_delay; + + // ARM DSP Math library filter instance + arm_fir_instance_f32 Ph90Deg_inst; + float32_t StateF32[AUDIO_BLOCK_SAMPLES + HILBERT_MAX_COEFFS]; +}; +#endif diff --git a/OpenAudio_ArduinoLibrary.h b/OpenAudio_ArduinoLibrary.h index d589444..133f339 100644 --- a/OpenAudio_ArduinoLibrary.h +++ b/OpenAudio_ArduinoLibrary.h @@ -32,3 +32,6 @@ #include "AudioSwitch_OA_F32.h" #include "FFT_Overlapped_OA_F32.h" #include "AudioEffectFreqShiftFD_OA_F32.h" +#include "AudioEffectDelay_OA_F32.h" +#include "RadioIQMixer_F32.h" +#include "AudioFilter90Deg_F32.h" diff --git a/RadioIQMixer_F32.cpp b/RadioIQMixer_F32.cpp new file mode 100644 index 0000000..e6e677d --- /dev/null +++ b/RadioIQMixer_F32.cpp @@ -0,0 +1,106 @@ +/* + * RadioIQMixer_F32.cpp + * + * 22 March 2020 + * Bob Larkin, in support of the library: + * Chip Audette, OpenAudio, Apr 2017 + * ------------------- + * A single signal channel comes in and is multiplied (mixed) with a sin + * and cos of the same frequency. The pair of mixer outputs are + * referred to as i and q. The conversion in frequency is either + * up or down, and a pair of filters on i and q determine which is allow + * to pass to the output. + * + * The sin/cos LO is from synth_sin_cos_f32.cpp See that for details. + * + * There are two then two outputs. + * + * MIT License, Use at your own risk. +*/ + +#include "RadioIQMixer_F32.h" +// 513 values of the sine wave in a float array: +#include "sinTable512_f32.h" + +void RadioIQMixer_F32::update(void) { + audio_block_f32_t *blockIn, *blockOut_i=NULL, *blockOut_q=NULL; + uint16_t index, i; + float32_t a, b, deltaPhase, phaseC; + + // Get first input, i, that will be filtered + blockIn = AudioStream_F32::receiveWritable_f32(0); + if (!blockIn) { + if(errorPrintIQM) Serial.println("IQMIXER-ERR: No input memory"); + return; + } + + // Try to get a pair of blocks for the IQ output + blockOut_i = AudioStream_F32::allocate_f32(); + if (!blockOut_i){ // Didn't have any + if(errorPrintIQM) Serial.println("IQMIXER-ERR: No I output memory"); + AudioStream_F32::release(blockIn); + return; + } + blockOut_q = AudioStream_F32::allocate_f32(); + if (!blockOut_q){ + if(errorPrintIQM) Serial.println("IQMIXER-ERR: No Q output memory"); + AudioStream_F32::release(blockIn); + AudioStream_F32::release(blockOut_i); + return; + } + + // doSimple has amplitude (-1, 1) and sin/cos differ by 90.00 degrees. + if (doSimple) { + for (i=0; i < block_size; i++) { + phaseS += phaseIncrement; + if (phaseS > 512.0f) + phaseS -= 512.0f; + index = (uint16_t) phaseS; + deltaPhase = phaseS -(float32_t) index; + /* Read two nearest values of input value from the sin table */ + a = sinTable512_f32[index]; + b = sinTable512_f32[index+1]; + // Linear interpolation and multiplying (DBMixer) with input + blockOut_i->data[i] = blockIn->data[i] * (a + 0.001953125*(b-a)*deltaPhase); + + /* Repeat for cosine by adding 90 degrees phase */ + index = (index + 128) & 0x01ff; + /* Read two nearest values of input value from the sin table */ + a = sinTable512_f32[index]; + b = sinTable512_f32[index+1]; + /* deltaPhase will be the same as used for sin */ + blockOut_q->data[i] = blockIn->data[i]*(a + 0.001953125*(b-a)*deltaPhase); + } + } + else { // Do a more flexible update, i.e., not doSimple + for (i=0; i < block_size; i++) { + phaseS += phaseIncrement; + if (phaseS > 512.0f) phaseS -= 512.0f; + index = (uint16_t) phaseS; + deltaPhase = phaseS -(float32_t) index; + /* Read two nearest values of input value from the sin table */ + a = sinTable512_f32[index]; + b = sinTable512_f32[index+1]; + // We now have a sine value, so multiply with the input data and save + // Linear interpolate sine and multiply with the input and amplitude (about 1.0) + blockOut_i->data[i] = amplitude_pk * blockIn->data[i] * (a + 0.001953125*(b-a)*deltaPhase); + + /* Shift forward phaseS_C and get cos. First, the calculation of index of the table */ + phaseC = phaseS + phaseS_C; + if (phaseC > 512.0f) phaseC -= 512.0f; + index = (uint16_t) phaseC; + deltaPhase = phaseC -(float32_t) index; + /* Read two nearest values of input value from the sin table */ + a = sinTable512_f32[index]; + b = sinTable512_f32[index+1]; + // Same as sin, but leave amplitude of LO at +/- 1.0 + blockOut_q->data[i] = blockIn->data[i] * (a + 0.001953125*(b-a)*deltaPhase); + } + } + AudioStream_F32::release(blockIn); // Done with this + //transmit the data + AudioStream_F32::transmit(blockOut_i, 0); // send the I outputs + AudioStream_F32::release(blockOut_i); + AudioStream_F32::transmit(blockOut_q, 1); // and the Q outputs + AudioStream_F32::release(blockOut_q); + } diff --git a/RadioIQMixer_F32.h b/RadioIQMixer_F32.h new file mode 100644 index 0000000..d92a35d --- /dev/null +++ b/RadioIQMixer_F32.h @@ -0,0 +1,138 @@ +/* + * RadioIQMixer_F32 + * 8 April 2020 Bob Larkin + * With much credit to: + * Chip Audette (OpenAudio) Feb 2017 + * and of course, to PJRC for the Teensy and Teensy Audio Library + * + * A basic building block is a pair of mixers fed in parallel with the + * LO going to the mixers at the same frequency, but differing in phase + * by 90 degrees. This provides two outputs I and Q that are offset in + * frequency but also 90 degrees apart in phase. The LO are included + * in the block, but there are no post-mixing filters. + * + * The frequency is set by .frequency(float freq_Hz) + * Particularly for use in transmitting, there is provision for varying + * the phase between the sine and cosine oscillators. Technically this is no + * longer sin and cos, but that is what real hardware needs. + * + * The output levels are 0.5 times the input level. + * + * Status: Tested in doSimple==1 + * Tested in FineFreqShift_OA.ino, T3.6 and T4.0 + * + * Inputs: 0 is signal + * Outputs: 0 is I 1 is Q + * + * Functions, available during operation: + * void frequency(float32_t fr) Sets BFO frequency Hz + * void iqmPhaseS(float32_t ps) Sets Phase of Sine in radians + * void phaseS_C_r(float32_t pc) Sets relative phase of Cosine in radians, approximately pi/2 + * void amplitudeC(float32_t a) Sets relative amplitude of Sine, approximately 1.0 + * void useSimple(bool s) Faster if 1, but no phase/amplitude adjustment + * void setSampleRate_Hz(float32_t fs_Hz) Allows dynamic sample rate change for this function + * + * Time: T3.6 For an update of a 128 sample block, doSimple=1, 46 microseconds + * T4.0 For an update of a 128 sample block, doSimple=1, 20 microseconds + */ + +#ifndef _radioIQMixer_f32_h +#define _radioIQMixer_f32_h + +#include "AudioStream_F32.h" +#include "arm_math.h" +#include "mathDSP_F32.h" + +class RadioIQMixer_F32 : public AudioStream_F32 { +//GUI: inputs:2, outputs:2 //this line used for automatic generation of GUI node +//GUI: shortName: IQMixer +public: + // Option of AudioSettings_F32 change to block size or sample rate: + RadioIQMixer_F32(void) : AudioStream_F32(1, inputQueueArray_f32) { + // Defaults + } + RadioIQMixer_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1, inputQueueArray_f32) { + setSampleRate_Hz(settings.sample_rate_Hz); + block_size = settings.audio_block_samples; + } + + void frequency(float32_t fr) { // Frequency in Hz + freq = fr; + if (freq < 0.0f) freq = 0.0f; + else if (freq > sample_rate_Hz/2.0f) freq = sample_rate_Hz/2.0f; + phaseIncrement = 512.0f * freq / sample_rate_Hz; + } + + /* Externally, phase comes in the range (0,2*M_PI) keeping with C math functions + * Internally, the full circle is represented as (0.0, 512.0). This is + * convenient for finding the entry to the sine table. + */ + void iqmPhaseS(float32_t a) { + while (a < 0.0f) a += MF_TWOPI; + while (a > MF_TWOPI) a -= MF_TWOPI; + phaseS = 512.0f * a / MF_TWOPI; + doSimple = false; + return; + } + + // phaseS_C_r is the number of radians that the cosine output leads the + // sine output. The default is M_PI_2 = pi/2 = 1.57079633 radians, + // corresponding to 90.00 degrees cosine leading sine. + void iqmPhaseS_C(float32_t a) { + while (a < 0.0f) a += MF_TWOPI; + while (a > MF_TWOPI) a -= MF_TWOPI; + // Internally a full circle is 512.00 of phase + phaseS_C = 512.0f * a / MF_TWOPI; + doSimple = false; + return; + } + + // The amplitude, a, is the peak, as in zero-to-peak. This produces outputs + // ranging from -a to +a. Both outputs are the same amplitude. + void iqmAmplitude(float32_t a) { + amplitude_pk = a; + doSimple = false; + return; + } + + // Speed up calculations by setting phaseS_C=90deg, amplitude=1 + void useSimple(bool s) { + doSimple = s; + if(doSimple) { + phaseS_C = 128.0f; + amplitude_pk = 1.0f; + } + return; + } + + void setSampleRate_Hz(float32_t fs_Hz) { + // Check freq range + if (freq > sample_rate_Hz/2.0f) freq = sample_rate_Hz/2.f; + // update phase increment for new frequency + phaseIncrement = 512.0f * freq / fs_Hz; + } + + void showError(uint16_t e) { // Serial.print errors in update() + errorPrintIQM = e; + } + + virtual void update(void); + +private: + audio_block_f32_t *inputQueueArray_f32[1]; + float32_t freq = 1000.0f; + float32_t phaseS = 0.0f; + float32_t phaseS_C = 128.00; // 512.00 is 360 degrees + float32_t amplitude_pk = 1.0f; + float32_t sample_rate_Hz = AUDIO_SAMPLE_RATE_EXACT; + float32_t phaseIncrement = 512.00f * freq /sample_rate_Hz; + uint16_t block_size = AUDIO_BLOCK_SAMPLES; + uint16_t errorPrintIQM = 0; // Normally off + // if only freq() is used, the complexities of phase, phaseS_C, + // and amplitude are not used, speeding up the sin and cos: + bool doSimple = true; + +}; + +#endif + diff --git a/examples/FineFreqShift_OA/FineFreqShift_OA.ino b/examples/FineFreqShift_OA/FineFreqShift_OA.ino new file mode 100644 index 0000000..6715829 --- /dev/null +++ b/examples/FineFreqShift_OA/FineFreqShift_OA.ino @@ -0,0 +1,290 @@ +/* FineFreqShift_OA.ino + * + * 1 Hz Resolution Frequency Shift and Synthetic Stereo + * + * Written for Open Audio Bob Larkin June 2020 + * + * This sketch demonstrates the use of dual multipliers along with broad-band 90-degree + * phase shifters (Hilbert transforms) and sine/cosine generator to shift all frequencies + * by a fixed amount. The shift amount is set by a digital oscillator frequency + * and can be made arbitrarily precise. Thus the name "Fine Frequency Shifter." + * + * Two blocks do most of the work of shifting the frequency band. The RadioIQMixer_F32 + * has a pair of multipliers that have one of their inputs from an oscillator that produces + * a sine and a cosine waveform of the same frequency. This oscillator phase difference + * is transferred to the output producing phase differences of 90-degrees. The multipliers, + * called double-balanced mixers in the analog world, produce sum and difference frequencies + * but the original input frequencies are suppressed. In the analog case, this suppression + * is limited by circuit balance, but for digital multipliers, the suppression is + * close-to-perfect. Thus the outputs of the I-Q Mixers is a pair of frequency sum and + * difference signals. Since the oscillator can be a low frequency, such as 100 Hz, + * the frequency band of sum and difference signals will include much overlap. + * + * The next block, the "AudioFilter90Deg_F32" applies a constant phase difference between + * the two inputs, which are then added or subtracted. That turns out to cause either the + * frequency sum or difference to be cancelled out. At that point only one audio signal + * remains and it is frequency shifted by the oscillator frequency. + * + * This INO also demonstrates the conversion of monaural to stereo sound by + * delaying one channel. This technique has been used for many years to de-correlate + * monaural noise before sending it to both ears. This can engage the the human brain + * to hear correlated signals better. It also adds a stereo presence to monaural voice + * that is pleasant to hear. It is tossed in here for experimentation. In general, + * delays up to 20 msec give the illusion of presence, whereas larger delays start to + * sound like loud echos! Play with it. Headphones vs. speakers change the perception. + * + * Control is done over the "USB Serial" using the Serial Monitor of the Arduino + * IDE. The commands control most functions and a list can be seen by typing + * "h", or looking in the listing below, at printHelp(). + * + * Refs - The phasing method of SSB generation goes way back. + * https://en.wikipedia.org/wiki/Single-sideband_modulation#Hartley_modulator + * The precision of DSP makes it practical for overlapping input and output bands. + * I first encountered this in conversation with friend Johan Forrer: + * https://www.arrl.org/files/file/Technology/tis/info/pdf/9609x008.pdf + * I need to find the German description of delay stereo from the 1980's. Both audio + * shifting and delay stereo were put into the DSP-10 audio processor: + * http://www.janbob.com/electron/dsp10/dsp10.htm + * + * Tested OK for Teensy 3.6 and 4.0. + * For settings: + * sample rate (Hz) = 44117.00 + * block size (samples) = 128 + * N_FFT = 512 + * Hilbert 251 taps + * CPU Cur/Peak, Teensy 3.6: 27.28%/27.49% + * CPU Cur/Peak, Teensy 4.0: 5.82%/5.84% + * Memory useage is 4 for I16 Memory + * Memory for F32 is 6 plus 1 more for every 2.9 mSec of Stereo Delay. + * + * This INO sketch is in the public domain. + */ + +#include "Audio.h" +#include "AudioStream_F32.h" +#include "OpenAudio_ArduinoLibrary.h" + +//set the sample rate and block size +const float sample_rate_Hz = 44117.f; +const int audio_block_samples = 128; +AudioSettings_F32 audio_settings(sample_rate_Hz, audio_block_samples); + +//create audio library objects for handling the audio +AudioInputI2S i2sIn; // This I16 input/output is T4.x compatible +AudioConvert_I16toF32 cnvrt1; // Convert to float +RadioIQMixer_F32 iqmixer1; +AudioFilter90Deg_F32 hilbert1; +AudioMixer4_F32 sum1; // Summing node for the SSB receiver +AudioFilterFIR_F32 fir1; // Low Pass Filter to frequency limit the SSB +AudioEffectDelay_OA_F32 delay1; // Pleasant and useful sound between L & R +AudioConvert_F32toI16 cnvrt2; +AudioConvert_F32toI16 cnvrt3; +AudioOutputI2S i2sOut; +AudioControlSGTL5000 codec; +//Make all of the audio connections +AudioConnection patchCord0(i2sIn, 0, cnvrt1, 0); // connect to Left codec, 16-bit +AudioConnection_F32 patchCord1(cnvrt1, 0, iqmixer1, 0); // Input to 2 mixers +AudioConnection_F32 patchCord2(cnvrt1, 0, iqmixer1, 1); +AudioConnection_F32 patchCord3(iqmixer1, 0, hilbert1, 0); // Broadband 90 deg phase +AudioConnection_F32 patchCord4(iqmixer1, 1, hilbert1, 1); +AudioConnection_F32 patchCord5(hilbert1, 0, sum1, 0); // Sideband select +AudioConnection_F32 patchCord6(hilbert1, 1, sum1, 1); +AudioConnection_F32 patchCord7(sum1, 0, delay1, 0); // delay channel 0 +AudioConnection_F32 patchCord9(sum1, 0, cnvrt2, 0); // connect to the left output +AudioConnection_F32 patchCordA(delay1, 0, cnvrt3, 0); // right output +AudioConnection patchCordB(cnvrt2, 0, i2sOut, 0); +AudioConnection patchCordC(cnvrt3, 0, i2sOut, 1); + +//control display and serial interaction +bool enable_printCPUandMemory = false; +void togglePrintMemoryAndCPU(void) { enable_printCPUandMemory = !enable_printCPUandMemory; }; + +// Filter for AudioFilter90Deg_F32 hilbert1 +#include "hilbert251A.h" + +//inputs and levels +float gain_dB = -15.0f; +float gain = 0.177828f; // Same as -15 dB +float sign = 1.0f; +float deltaGain_dB = 2.5f; +float frequencyLO = 100.0f; +float delayms = 1.0f; + +// *************** SETUP ********************************** +void setup() { + Serial.begin(1); delay(1000); + Serial.println("*** Fine Frequency Shifter - June 2020 ***"); + Serial.print("Sample Rate in Hz = "); + Serial.println(audio_settings.sample_rate_Hz, 0); + Serial.print("Block size, samples = "); + Serial.println(audio_settings.audio_block_samples); + + AudioMemory(10); // I16 type + AudioMemory_F32(200, audio_settings); + + //Enable the codec to start the audio flowing! + codec.enable(); + codec.adcHighPassFilterEnable(); + codec.inputSelect(AUDIO_INPUT_LINEIN); + + iqmixer1.frequency(frequencyLO); // Frequency shift, Hz + deltaFrequency(0.0f); // Print freq + hilbert1.begin(hilbert251A, 251); // Set the Hilbert transform FIR filter + sum1.gain(0, gain*sign); // Set gains + sum1.gain(1, gain); + delay1.delay(0, delayms); // Delay right channel + deltaDelay(0.0f); // Print delay + + //finish the setup by printing the help menu to the serial connections + printHelp(); +} + +// ************************* LOOP **************************** +void loop() { + //respond to Serial commands + while (Serial.available()) respondToByte((char)Serial.read()); + + //check to see whether to print the CPU and Memory Usage + if (enable_printCPUandMemory) printCPUandMemory(millis(), 3000); //print every 3000 msec + +} //end loop(); + +//This routine prints the current and maximum CPU usage and the current usage of the AudioMemory that has been allocated +void printCPUandMemory(unsigned long curTime_millis, unsigned long updatePeriod_millis) { + //static unsigned long updatePeriod_millis = 3000; //how many milliseconds between updating gain reading? + static unsigned long lastUpdate_millis = 0; + + //has enough time passed to update everything? + if (curTime_millis < lastUpdate_millis) lastUpdate_millis = 0; //handle wrap-around of the clock + if ((curTime_millis - lastUpdate_millis) > updatePeriod_millis) { //is it time to update the user interface? + Serial.print("CPU Cur/Peak: "); + Serial.print(audio_settings.processorUsage()); + Serial.print("%/"); + Serial.print(audio_settings.processorUsageMax()); + Serial.println("%"); + Serial.print(" Audio MEM Float32 Cur/Peak: "); + Serial.print(AudioMemoryUsage_F32()); + Serial.print("/"); + Serial.println(AudioMemoryUsageMax_F32()); + Serial.print(" Audio MEM Int16 Cur/Peak: "); + Serial.print(AudioMemoryUsage()); + Serial.print("/"); + Serial.println(AudioMemoryUsageMax()); + + lastUpdate_millis = curTime_millis; //we will use this value the next time around. + } +} + +void incrementGain(float increment_dB) { + gain_dB += increment_dB; + // gain is set in the "mixer" block, including sign for raise/lower freq + gain = powf(10.0f, 0.05f * gain_dB); + sum1.gain(0, gain*sign); + sum1.gain(1, gain); + printGainSettings(); +} + +void printGainSettings(void) { + Serial.print("Gain in dB = "); Serial.println(gain_dB, 1); +} + +void deltaFrequency(float dfr) { + frequencyLO += dfr; + Serial.print("Frequency shift in Hz = "); + Serial.println(frequencyLO, 1); + if(frequencyLO < 0.0f) + sign = 1.0f; + else + sign = -1.0f; + iqmixer1.frequency(fabsf(frequencyLO)); + incrementGain(0.0f); +} + +void deltaDelay(float dtau) { + delayms += dtau; + if (delayms < 0.0f) + delayms = 0.0f; + delay1.delay(0, delayms); // Delay right channel + Serial.print("Delay in milliseconds = "); + Serial.println(delayms, 1); +} + +//switch yard to determine the desired action +void respondToByte(char c) { + char s[2]; + s[0] = c; s[1] = 0; + if( !isalpha((int)c) && c!='?') return; + switch (c) { + case 'h': case '?': + printHelp(); + break; + case 'g': case 'G': + printGainSettings(); + break; + case 'k': + incrementGain(deltaGain_dB); + break; + case 'K': // which is "shift k" + incrementGain(-deltaGain_dB); + break; + case 'C': case 'c': + Serial.println("Toggle printing of memory and CPU usage."); + togglePrintMemoryAndCPU(); + break; + case 'd': + deltaFrequency(1.0f); + break; + case 'D': + deltaFrequency(-1.0f); + break; + case 'e': + deltaFrequency(10.0f); + break; + case 'E': + deltaFrequency(-10.0f); + break; + case 'f': + deltaFrequency(100.0f); + break; + case 'F': + deltaFrequency(-100.0f); + break; + case 't': + deltaDelay(1.0f); + break; + case 'T': + deltaDelay(-1.0f); + break; + case 'u': + deltaDelay(10.0f); + break; + case 'U': + deltaDelay(-10.0f); + break; + default: + Serial.print("You typed "); Serial.print(s); + Serial.println(". What command?"); + } +} + +void printHelp(void) { + Serial.println(); + Serial.println("Help: Available Commands:"); + Serial.println(" h: Print this help"); + Serial.println(" g: Print the gain settings of the device."); + Serial.println(" C: Toggle printing of CPU and Memory usage"); + Serial.print( " k: Increase the gain of both channels by "); + Serial.print(deltaGain_dB); Serial.println(" dB"); + Serial.print( " K: Decrease the gain of both channels by "); + Serial.print(-deltaGain_dB); Serial.println(" dB"); + Serial.println(" d: Raise frequency by 1 Hz"); + Serial.println(" D: Lower frequency by 1 Hz"); + Serial.println(" e: Raise frequency by 10 Hz"); + Serial.println(" E: Lower frequency by 10 Hz"); + Serial.println(" f: Raise frequency by 100 Hz"); + Serial.println(" F: Lower frequency by 100 Hz"); + Serial.println(" t: Raise stereo delay by 1 msec"); + Serial.println(" T: Lower stereo delay by 1 msec"); + Serial.println(" u: Raise stereo delay by 10 msec"); + Serial.println(" U: Lower stereo delay by 10 msec"); +} diff --git a/examples/FineFreqShift_OA/hilbert121A.h b/examples/FineFreqShift_OA/hilbert121A.h new file mode 100644 index 0000000..d624d04 --- /dev/null +++ b/examples/FineFreqShift_OA/hilbert121A.h @@ -0,0 +1,123 @@ +// Following is 121 term Hilbert FIR filter +float32_t hilbert121A[121] = { + 0.000000000000000000, + 0.000773378567767513, + 0.000000000000000000, + 0.001046207887980644, + 0.000000000000000000, + 0.001368896533613985, + 0.000000000000000000, + 0.001746769975247667, + 0.000000000000000000, + 0.002185555845922462, + 0.000000000000000000, + 0.002691457154069645, + 0.000000000000000000, + 0.003271251311125927, + 0.000000000000000000, + 0.003932423233774751, + 0.000000000000000000, + 0.004683343721596901, + 0.000000000000000000, + 0.005533508538632429, + 0.000000000000000000, + 0.006493859804516438, + 0.000000000000000000, + 0.007577220484233372, + 0.000000000000000000, + 0.008798886675905997, + 0.000000000000000000, + 0.010177443901536392, + 0.000000000000000000, + 0.011735907609641917, + 0.000000000000000000, + 0.013503343224246872, + 0.000000000000000000, + 0.015517212970554440, + 0.000000000000000000, + 0.017826854793349920, + 0.000000000000000000, + 0.020498780519188083, + 0.000000000000000000, + 0.023625003856774591, + 0.000000000000000000, + 0.027336628208641155, + 0.000000000000000000, + 0.031827023036304102, + 0.000000000000000000, + 0.037393534868609392, + 0.000000000000000000, + 0.044517689704988733, + 0.000000000000000000, + 0.054032871748808158, + 0.000000000000000000, + 0.067515548043274365, + 0.000000000000000000, + 0.088347125250410385, + 0.000000000000000000, + 0.125324201622410869, + 0.000000000000000000, + 0.210709715079613419, + 0.000000000000000000, + 0.634897508268964295, + 0.000000000000000000, +-0.634897508268964295, + 0.000000000000000000, +-0.210709715079613419, + 0.000000000000000000, +-0.125324201622410869, + 0.000000000000000000, +-0.088347125250410385, + 0.000000000000000000, +-0.067515548043274365, + 0.000000000000000000, +-0.054032871748808158, + 0.000000000000000000, +-0.044517689704988733, + 0.000000000000000000, +-0.037393534868609392, + 0.000000000000000000, +-0.031827023036304102, + 0.000000000000000000, +-0.027336628208641155, + 0.000000000000000000, +-0.023625003856774591, + 0.000000000000000000, +-0.020498780519188083, + 0.000000000000000000, +-0.017826854793349920, + 0.000000000000000000, +-0.015517212970554440, + 0.000000000000000000, +-0.013503343224246872, + 0.000000000000000000, +-0.011735907609641917, + 0.000000000000000000, +-0.010177443901536392, + 0.000000000000000000, +-0.008798886675905997, + 0.000000000000000000, +-0.007577220484233372, + 0.000000000000000000, +-0.006493859804516438, + 0.000000000000000000, +-0.005533508538632429, + 0.000000000000000000, +-0.004683343721596901, + 0.000000000000000000, +-0.003932423233774751, + 0.000000000000000000, +-0.003271251311125927, + 0.000000000000000000, +-0.002691457154069645, + 0.000000000000000000, +-0.002185555845922462, + 0.000000000000000000, +-0.001746769975247667, + 0.000000000000000000, +-0.001368896533613985, + 0.000000000000000000, +-0.001046207887980644, + 0.000000000000000000, +-0.000773378567767513, + 0.000000000000000000}; diff --git a/examples/FineFreqShift_OA/hilbert251A.h b/examples/FineFreqShift_OA/hilbert251A.h new file mode 100644 index 0000000..745db86 --- /dev/null +++ b/examples/FineFreqShift_OA/hilbert251A.h @@ -0,0 +1,253 @@ +// Following is 251 term Hilbert FIR filter +float32_t hilbert251A[]={ +0.0000003255, +0.0000000000, +0.0000030702, +0.0000000000, +0.0000089286, +0.0000000000, +0.0000183061, +0.0000000000, +0.0000316287, +0.0000000000, +0.0000493436, +0.0000000000, +0.0000719193, +0.0000000000, +0.0000998451, +0.0000000000, +0.0001336320, +0.0000000000, +0.0001738120, +0.0000000000, +0.0002209393, +0.0000000000, +0.0002755899, +0.0000000000, +0.0003383625, +0.0000000000, +0.0004098790, +0.0000000000, +0.0004907853, +0.0000000000, +0.0005817525, +0.0000000000, +0.0006834782, +0.0000000000, +0.0007966881, +0.0000000000, +0.0009221383, +0.0000000000, +0.0010606178, +0.0000000000, +0.0012129515, +0.0000000000, +0.0013800041, +0.0000000000, +0.0015626848, +0.0000000000, +0.0017619529, +0.0000000000, +0.0019788241, +0.0000000000, +0.0022143787, +0.0000000000, +0.0024697715, +0.0000000000, +0.0027462425, +0.0000000000, +0.0030451312, +0.0000000000, +0.0033678928, +0.0000000000, +0.0037161183, +0.0000000000, +0.0040915578, +0.0000000000, +0.0044961498, +0.0000000000, +0.0049320558, +0.0000000000, +0.0054017033, +0.0000000000, +0.0059078375, +0.0000000000, +0.0064535860, +0.0000000000, +0.0070425380, +0.0000000000, +0.0076788436, +0.0000000000, +0.0083673390, +0.0000000000, +0.0091137048, +0.0000000000, +0.0099246683, +0.0000000000, +0.0108082660, +0.0000000000, +0.0117741868, +0.0000000000, +0.0128342256, +0.0000000000, +0.0140028938, +0.0000000000, +0.0152982506, +0.0000000000, +0.0167430570, +0.0000000000, +0.0183664064, +0.0000000000, +0.0202060801, +0.0000000000, +0.0223120327, +0.0000000000, +0.0247516963, +0.0000000000, +0.0276183140, +0.0000000000, +0.0310445375, +0.0000000000, +0.0352256211, +0.0000000000, +0.0404611696, +0.0000000000, +0.0472354231, +0.0000000000, +0.0563851215, +0.0000000000, +0.0694911881, +0.0000000000, +0.0899418673, +0.0000000000, +0.1265473875, +0.0000000000, +0.2116132716, +0.0000000000, +0.6358933477, +0.0000000000, +-0.6358933478, +0.0000000000, +-0.2116132717, +0.0000000000, +-0.1265473876, +0.0000000000, +-0.0899418674, +0.0000000000, +-0.0694911882, +0.0000000000, +-0.0563851216, +0.0000000000, +-0.0472354232, +0.0000000000, +-0.0404611697, +0.0000000000, +-0.0352256212, +0.0000000000, +-0.0310445376, +0.0000000000, +-0.0276183141, +0.0000000000, +-0.0247516964, +0.0000000000, +-0.0223120328, +0.0000000000, +-0.0202060802, +0.0000000000, +-0.0183664065, +0.0000000000, +-0.0167430571, +0.0000000000, +-0.0152982507, +0.0000000000, +-0.0140028939, +0.0000000000, +-0.0128342257, +0.0000000000, +-0.0117741869, +0.0000000000, +-0.0108082661, +0.0000000000, +-0.0099246684, +0.0000000000, +-0.0091137049, +0.0000000000, +-0.0083673391, +0.0000000000, +-0.0076788437, +0.0000000000, +-0.0070425381, +0.0000000000, +-0.0064535861, +0.0000000000, +-0.0059078376, +0.0000000000, +-0.0054017034, +0.0000000000, +-0.0049320559, +0.0000000000, +-0.0044961499, +0.0000000000, +-0.0040915579, +0.0000000000, +-0.0037161184, +0.0000000000, +-0.0033678929, +0.0000000000, +-0.0030451313, +0.0000000000, +-0.0027462426, +0.0000000000, +-0.0024697716, +0.0000000000, +-0.0022143788, +0.0000000000, +-0.0019788242, +0.0000000000, +-0.0017619530, +0.0000000000, +-0.0015626849, +0.0000000000, +-0.0013800042, +0.0000000000, +-0.0012129516, +0.0000000000, +-0.0010606179, +0.0000000000, +-0.0009221384, +0.0000000000, +-0.0007966882, +0.0000000000, +-0.0006834783, +0.0000000000, +-0.0005817526, +0.0000000000, +-0.0004907854, +0.0000000000, +-0.0004098791, +0.0000000000, +-0.0003383626, +0.0000000000, +-0.0002755900, +0.0000000000, +-0.0002209394, +0.0000000000, +-0.0001738121, +0.0000000000, +-0.0001336321, +0.0000000000, +-0.0000998452, +0.0000000000, +-0.0000719194, +0.0000000000, +-0.0000493437, +0.0000000000, +-0.0000316288, +0.0000000000, +-0.0000183062, +0.0000000000, +-0.0000089287, +0.0000000000, +-0.0000030703, +0.0000000000, +-0.0000003256}; diff --git a/mathDSP_F32.cpp b/mathDSP_F32.cpp new file mode 100644 index 0000000..ea232a5 --- /dev/null +++ b/mathDSP_F32.cpp @@ -0,0 +1,114 @@ + +#include "mathDSP_F32.h" +#include + +/* acos_f32(x) Bob Larkin 2020 + * This acos(x) approximation is intended as being fast, resonably accurate, and + * with continuous function and derivative (slope) between -1 and 1 x value. + * It is the result of a "Chebychev-zero" fit to the true values, and is a 7th + * order polynomial for the full (-1.0, 1.0) range. + * Max error from -0.99 to 0.99 is < 0.018/Pi (1.0 deg) + * Error at -1 or +1 is 0.112/Pi (6.4 deg) + * For acos, speed and accuracy are in conflict near x = +/- 1, but that + * is not where communications phase detectors are normally used. + * Using T3.6 this function, by itself, measures as 0.18 uSec + * + * Thanks to Bob K3KHF for ideas on minimizing errors with acos(). + * RSL 5 April 2020. + */ +float mathDSP_F32::acos_f32(float x) { + float w; + // These next two error checks use 0.056 uSec per call + if(x > 1.00000) return 0.0f; + if(x < -1.00000) return MF_PI; + w = x * x; + return 1.5707963268f+(x*((-0.97090f)+w*((-0.529008f)+w*(1.00279f-w*0.961446)))); +} + +/* *** Not currently used, but possible substitute for acosf(x) *** + * Apparently based on Handbook of Mathematical Functions + * M. Abramowitz and I.A. Stegun, Ed. Check before using. + * https://developer.download.nvidia.com/cg/acos.html + * Absolute error <= 6.7e-5, good, but not as good as acosf() + * T3.6 this measures 0.51 uSec (0.23 uSec from sqrtf() ), + * better than acosf(x) by a factor of 2. + */ +float mathDSP_F32::approxAcos(float x) { + if(x > 0.999999) return 0.0f; + if(x < -0.999999) return M_PI; // 3.14159265358979f; + float negate = float(x < 0); + x = fabsf(x); + float ret = -0.0187293f; + ret = ret * x; + ret = ret + 0.0742610f; + ret = ret * x; + ret = ret - 0.2121144f; + ret = ret * x; + ret = ret + 1.5707288f; + ret = ret * sqrtf(1.0f-x); + ret = ret - 2 * negate * ret; + return negate * MF_PI + ret; +} + +/* Polynomial approximating arctangenet on iput range (-1, 1) + * giving result in a range of approximately (-pi/4, pi/4) + * Max error < 0.005 radians (or 0.29 degrees) + * + * Directly from www.dsprelated.com/showarticle/1052.php + * Thank you Nic Taylor---nice work. + */ +float mathDSP_F32::fastAtan2(float y, float x) { + if (x != 0.0f) { + if (fabsf(x) > fabsf(y)) { + const float z = y / x; + if (x > 0.0) + // atan2(y,x) = atan(y/x) if x > 0 + return _Atan(z); + else if (y >= 0.0) + // atan2(y,x) = atan(y/x) + PI if x < 0, y >= 0 + return _Atan(z) + M_PI; + else + // atan2(y,x) = atan(y/x) - PI if x < 0, y < 0 + return _Atan(z) - M_PI; + } + else { // Use property atan(y/x) = PI/2-atan(x/y) if |y/x| > 1. + const float z = x / y; + if (y > 0.0) + // atan2(y,x) = PI/2 - atan(x/y) if |y/x| > 1, y > 0 + return -_Atan(z) + M_PI_2; + else + // atan2(y,x) = -PI/2 - atan(x/y) if |y/x| > 1, y < 0 + return -_Atan(z) - M_PI_2; + } + } + else { + if (y > 0.0f) // x = 0, y > 0 + return M_PI_2; + else if (y < 0.0f) // x = 0, y < 0 + return -M_PI_2; + } + return 0.0f; // x,y = 0. Undefined, stay finite. +} + +/* float i0f(float x) Returns the modified Bessel function Io(x). + * Algorithm is based on Abromowitz and Stegun, Handbook of Mathematical + * Functions, and Press, et. al., Numerical Recepies in C. + * All in 32-bit floating point + */ +float mathDSP_F32::i0f(float x) { + float af, bf, cf; + if( (af=fabsf(x)) < 3.75f ) { + cf = x/3.75f; + cf = cf*cf; + bf=1.0f+cf*(3.515623f+cf*(3.089943f+cf*(1.20675f+cf*(0.265973f+ + cf*(0.0360768f+cf*0.0045813f))))); + } + else { + cf = 3.75f/af; + bf=(expf(af)/sqrtf(af))*(0.3989423f+cf*(0.0132859f+cf*(0.0022532f+ + cf*(-0.0015756f+cf*(0.0091628f+cf*(-0.0205771f+cf*(0.0263554f+ + cf*(-0.0164763f+cf*0.0039238f)))))))); + } + return bf; + } + diff --git a/mathDSP_F32.h b/mathDSP_F32.h new file mode 100644 index 0000000..6540421 --- /dev/null +++ b/mathDSP_F32.h @@ -0,0 +1,50 @@ +/* + mathDSP.h - Definitions and functions to support OpenAudio_ArduinoLibrary_F32 + Created by Bob Larkin 15 April 2020. + +*/ +#ifndef mathDSP_F32_h +#define mathDSP_F32_h + + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_TWOPI +#define M_TWOPI 6.28318530717958647692 +#endif + +#ifndef MF_PI_2 +#define MF_PI_2 1.5707963f +#endif + +#ifndef MF_PI +#define MF_PI 3.14159265f +#endif + +#ifndef MF_TWOPI +#define MF_TWOPI 6.2831853f +#endif + +class mathDSP_F32 +{ + public: + float acos_f32(float x); + float approxAcos(float x); + float fastAtan2(float y, float x); + float i0f(float x); + private: + // Support for FastAtan2(x,y) + float _Atan(float z) { + const float n1 = 0.97239411f; + const float n2 = -0.19194795f; + return (n1 + n2 * z * z) * z; + } +}; + +#endif diff --git a/readme.md b/readme.md index be1a39f..1daf09b 100644 --- a/readme.md +++ b/readme.md @@ -20,6 +20,12 @@ OpenAudio Library for Teensy 15-Moved Overlapped FFT AudioFormantShifter INO and related from Tympan to _OA Open Audio. Tested 3.6 and 4.0. 16-Moved AudioFrequencyShifter INO and related from Tympan to _OA Open Audio. Tested 3.6 and 4.0. 17-Working on SdFatWriter and related SdFat files. DO NOT USE. +18-Modified AudioEffectDelay for T4.0. +19-Moved in radio function AudioFilter90Deg_F32. +20-Moved in radio function RadioIQMixer_F32. +21-Added Example INO, FineFreqShift_OA.ino with Hilbert shifter and delay stereo. +22-Brought in support stuff mathDSP_F32 and sinTable512_F32.h. + **Purpose**: The purpose of this library is to build upon the [Teensy Audio Library](http://www.pjrc.com/teensy/td_libs_Audio.html) to enable new functionality for real-time audio processing. diff --git a/sinTable512_f32.h b/sinTable512_f32.h new file mode 100644 index 0000000..24e7ae2 --- /dev/null +++ b/sinTable512_f32.h @@ -0,0 +1,98 @@ +// This comes from the ARM Cortex M3, M4 sin(). Linear interpolation between points +// gives the 24-bit accuracy of float32_t +const float32_t sinTable512_f32[513] = { + 0.00000000f, 0.01227154f, 0.02454123f, 0.03680722f, 0.04906767f, 0.06132074f, + 0.07356456f, 0.08579731f, 0.09801714f, 0.11022221f, 0.12241068f, 0.13458071f, + 0.14673047f, 0.15885814f, 0.17096189f, 0.18303989f, 0.19509032f, 0.20711138f, + 0.21910124f, 0.23105811f, 0.24298018f, 0.25486566f, 0.26671276f, 0.27851969f, + 0.29028468f, 0.30200595f, 0.31368174f, 0.32531029f, 0.33688985f, 0.34841868f, + 0.35989504f, 0.37131719f, 0.38268343f, 0.39399204f, 0.40524131f, 0.41642956f, + 0.42755509f, 0.43861624f, 0.44961133f, 0.46053871f, 0.47139674f, 0.48218377f, + 0.49289819f, 0.50353838f, 0.51410274f, 0.52458968f, 0.53499762f, 0.54532499f, + 0.55557023f, 0.56573181f, 0.57580819f, 0.58579786f, 0.59569930f, 0.60551104f, + 0.61523159f, 0.62485949f, 0.63439328f, 0.64383154f, 0.65317284f, 0.66241578f, + 0.67155895f, 0.68060100f, 0.68954054f, 0.69837625f, 0.70710678f, 0.71573083f, + 0.72424708f, 0.73265427f, 0.74095113f, 0.74913639f, 0.75720885f, 0.76516727f, + 0.77301045f, 0.78073723f, 0.78834643f, 0.79583690f, 0.80320753f, 0.81045720f, + 0.81758481f, 0.82458930f, 0.83146961f, 0.83822471f, 0.84485357f, 0.85135519f, + 0.85772861f, 0.86397286f, 0.87008699f, 0.87607009f, 0.88192126f, 0.88763962f, + 0.89322430f, 0.89867447f, 0.90398929f, 0.90916798f, 0.91420976f, 0.91911385f, + 0.92387953f, 0.92850608f, 0.93299280f, 0.93733901f, 0.94154407f, 0.94560733f, + 0.94952818f, 0.95330604f, 0.95694034f, 0.96043052f, 0.96377607f, 0.96697647f, + 0.97003125f, 0.97293995f, 0.97570213f, 0.97831737f, 0.98078528f, 0.98310549f, + 0.98527764f, 0.98730142f, 0.98917651f, 0.99090264f, 0.99247953f, 0.99390697f, + 0.99518473f, 0.99631261f, 0.99729046f, 0.99811811f, 0.99879546f, 0.99932238f, + 0.99969882f, 0.99992470f, 1.00000000f, 0.99992470f, 0.99969882f, 0.99932238f, + 0.99879546f, 0.99811811f, 0.99729046f, 0.99631261f, 0.99518473f, 0.99390697f, + 0.99247953f, 0.99090264f, 0.98917651f, 0.98730142f, 0.98527764f, 0.98310549f, + 0.98078528f, 0.97831737f, 0.97570213f, 0.97293995f, 0.97003125f, 0.96697647f, + 0.96377607f, 0.96043052f, 0.95694034f, 0.95330604f, 0.94952818f, 0.94560733f, + 0.94154407f, 0.93733901f, 0.93299280f, 0.92850608f, 0.92387953f, 0.91911385f, + 0.91420976f, 0.90916798f, 0.90398929f, 0.89867447f, 0.89322430f, 0.88763962f, + 0.88192126f, 0.87607009f, 0.87008699f, 0.86397286f, 0.85772861f, 0.85135519f, + 0.84485357f, 0.83822471f, 0.83146961f, 0.82458930f, 0.81758481f, 0.81045720f, + 0.80320753f, 0.79583690f, 0.78834643f, 0.78073723f, 0.77301045f, 0.76516727f, + 0.75720885f, 0.74913639f, 0.74095113f, 0.73265427f, 0.72424708f, 0.71573083f, + 0.70710678f, 0.69837625f, 0.68954054f, 0.68060100f, 0.67155895f, 0.66241578f, + 0.65317284f, 0.64383154f, 0.63439328f, 0.62485949f, 0.61523159f, 0.60551104f, + 0.59569930f, 0.58579786f, 0.57580819f, 0.56573181f, 0.55557023f, 0.54532499f, + 0.53499762f, 0.52458968f, 0.51410274f, 0.50353838f, 0.49289819f, 0.48218377f, + 0.47139674f, 0.46053871f, 0.44961133f, 0.43861624f, 0.42755509f, 0.41642956f, + 0.40524131f, 0.39399204f, 0.38268343f, 0.37131719f, 0.35989504f, 0.34841868f, + 0.33688985f, 0.32531029f, 0.31368174f, 0.30200595f, 0.29028468f, 0.27851969f, + 0.26671276f, 0.25486566f, 0.24298018f, 0.23105811f, 0.21910124f, 0.20711138f, + 0.19509032f, 0.18303989f, 0.17096189f, 0.15885814f, 0.14673047f, 0.13458071f, + 0.12241068f, 0.11022221f, 0.09801714f, 0.08579731f, 0.07356456f, 0.06132074f, + 0.04906767f, 0.03680722f, 0.02454123f, 0.01227154f, 0.00000000f, -0.01227154f, + -0.02454123f, -0.03680722f, -0.04906767f, -0.06132074f, -0.07356456f, + -0.08579731f, -0.09801714f, -0.11022221f, -0.12241068f, -0.13458071f, + -0.14673047f, -0.15885814f, -0.17096189f, -0.18303989f, -0.19509032f, + -0.20711138f, -0.21910124f, -0.23105811f, -0.24298018f, -0.25486566f, + -0.26671276f, -0.27851969f, -0.29028468f, -0.30200595f, -0.31368174f, + -0.32531029f, -0.33688985f, -0.34841868f, -0.35989504f, -0.37131719f, + -0.38268343f, -0.39399204f, -0.40524131f, -0.41642956f, -0.42755509f, + -0.43861624f, -0.44961133f, -0.46053871f, -0.47139674f, -0.48218377f, + -0.49289819f, -0.50353838f, -0.51410274f, -0.52458968f, -0.53499762f, + -0.54532499f, -0.55557023f, -0.56573181f, -0.57580819f, -0.58579786f, + -0.59569930f, -0.60551104f, -0.61523159f, -0.62485949f, -0.63439328f, + -0.64383154f, -0.65317284f, -0.66241578f, -0.67155895f, -0.68060100f, + -0.68954054f, -0.69837625f, -0.70710678f, -0.71573083f, -0.72424708f, + -0.73265427f, -0.74095113f, -0.74913639f, -0.75720885f, -0.76516727f, + -0.77301045f, -0.78073723f, -0.78834643f, -0.79583690f, -0.80320753f, + -0.81045720f, -0.81758481f, -0.82458930f, -0.83146961f, -0.83822471f, + -0.84485357f, -0.85135519f, -0.85772861f, -0.86397286f, -0.87008699f, + -0.87607009f, -0.88192126f, -0.88763962f, -0.89322430f, -0.89867447f, + -0.90398929f, -0.90916798f, -0.91420976f, -0.91911385f, -0.92387953f, + -0.92850608f, -0.93299280f, -0.93733901f, -0.94154407f, -0.94560733f, + -0.94952818f, -0.95330604f, -0.95694034f, -0.96043052f, -0.96377607f, + -0.96697647f, -0.97003125f, -0.97293995f, -0.97570213f, -0.97831737f, + -0.98078528f, -0.98310549f, -0.98527764f, -0.98730142f, -0.98917651f, + -0.99090264f, -0.99247953f, -0.99390697f, -0.99518473f, -0.99631261f, + -0.99729046f, -0.99811811f, -0.99879546f, -0.99932238f, -0.99969882f, + -0.99992470f, -1.00000000f, -0.99992470f, -0.99969882f, -0.99932238f, + -0.99879546f, -0.99811811f, -0.99729046f, -0.99631261f, -0.99518473f, + -0.99390697f, -0.99247953f, -0.99090264f, -0.98917651f, -0.98730142f, + -0.98527764f, -0.98310549f, -0.98078528f, -0.97831737f, -0.97570213f, + -0.97293995f, -0.97003125f, -0.96697647f, -0.96377607f, -0.96043052f, + -0.95694034f, -0.95330604f, -0.94952818f, -0.94560733f, -0.94154407f, + -0.93733901f, -0.93299280f, -0.92850608f, -0.92387953f, -0.91911385f, + -0.91420976f, -0.90916798f, -0.90398929f, -0.89867447f, -0.89322430f, + -0.88763962f, -0.88192126f, -0.87607009f, -0.87008699f, -0.86397286f, + -0.85772861f, -0.85135519f, -0.84485357f, -0.83822471f, -0.83146961f, + -0.82458930f, -0.81758481f, -0.81045720f, -0.80320753f, -0.79583690f, + -0.78834643f, -0.78073723f, -0.77301045f, -0.76516727f, -0.75720885f, + -0.74913639f, -0.74095113f, -0.73265427f, -0.72424708f, -0.71573083f, + -0.70710678f, -0.69837625f, -0.68954054f, -0.68060100f, -0.67155895f, + -0.66241578f, -0.65317284f, -0.64383154f, -0.63439328f, -0.62485949f, + -0.61523159f, -0.60551104f, -0.59569930f, -0.58579786f, -0.57580819f, + -0.56573181f, -0.55557023f, -0.54532499f, -0.53499762f, -0.52458968f, + -0.51410274f, -0.50353838f, -0.49289819f, -0.48218377f, -0.47139674f, + -0.46053871f, -0.44961133f, -0.43861624f, -0.42755509f, -0.41642956f, + -0.40524131f, -0.39399204f, -0.38268343f, -0.37131719f, -0.35989504f, + -0.34841868f, -0.33688985f, -0.32531029f, -0.31368174f, -0.30200595f, + -0.29028468f, -0.27851969f, -0.26671276f, -0.25486566f, -0.24298018f, + -0.23105811f, -0.21910124f, -0.20711138f, -0.19509032f, -0.18303989f, + -0.17096189f, -0.15885814f, -0.14673047f, -0.13458071f, -0.12241068f, + -0.11022221f, -0.09801714f, -0.08579731f, -0.07356456f, -0.06132074f, + -0.04906767f, -0.03680722f, -0.02454123f, -0.01227154f, -0.00000000f}; +