parent
825f50668b
commit
ae99656d29
@ -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 <Arduino.h> |
||||
#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<end_loop; i++) dest[dest_count++] = source[src_count++]; |
||||
|
||||
//finish writing taking data from the next queue buffer
|
||||
if (src_count < n_copy) { // there's still more input data to copy...but we need to roll-over to a new input queue
|
||||
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) { |
||||
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<<channel))) continue; |
||||
output = allocate_f32(); |
||||
if (!output) continue; |
||||
|
||||
//figure out where to start pulling the data samples from
|
||||
uint32_t ref_samp_long = (head*AUDIO_BLOCK_SIZE_F32) + writeposition; //note that writepoisition has already been
|
||||
//incremented by the block length by the
|
||||
//receiveIncomingData method. We'll adjust it next
|
||||
uint32_t offset_samp = delay_samps[channel]+output->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); |
||||
} |
||||
} |
||||
|
||||
|
@ -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<<channel))) { |
||||
// enabling a previously disabled channel
|
||||
delay_samps[channel] = n; |
||||
if (blks > maxblocks) maxblocks = blks; |
||||
activemask |= (1<<channel); |
||||
} else { |
||||
if (n > 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<<channel); |
||||
// recompute maxblocks for remaining enabled channels
|
||||
recompute_maxblocks(); |
||||
} |
||||
virtual void update(void); |
||||
private: |
||||
void recompute_maxblocks(void) { |
||||
uint32_t max=0; |
||||
uint32_t channel = 0; |
||||
do { |
||||
if (activemask & (1<<channel)) { |
||||
uint32_t n = delay_samps[channel]; |
||||
n = (n + (AUDIO_BLOCK_SIZE_F32-1)) / AUDIO_BLOCK_SIZE_F32 + 1; |
||||
if (n > 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 |
@ -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; j<fir_length; j+=2) {sum += coef[j] * data[j+nn];}
|
||||
arm_fir_f32(&Ph90Deg_inst, block_i->data, 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; i<block_size; i++) { // Let it wrap around at 256
|
||||
in_index++; |
||||
in_index = delayBufferMask & in_index; // Index into delay buffer, wrapping
|
||||
delayData[in_index] = *pin++; // Put ith q data to circular delay buffer
|
||||
} |
||||
|
||||
// And similarly, output with a delay, i.e., a buffer offset
|
||||
pout = &(block_q->data[0]); // Re-use the input block
|
||||
for(i=0; i<block_size; i++) { |
||||
out_index++; |
||||
out_index = delayBufferMask & out_index; // Index into delay buffer, wrapping
|
||||
*pout++ = delayData[out_index]; |
||||
} |
||||
|
||||
//transmit the data
|
||||
AudioStream_F32::transmit(blockOut_i, 0); // send the FIR outputs
|
||||
AudioStream_F32::release (blockOut_i); |
||||
AudioStream_F32::transmit(block_q, 1); // and the delayed outputs
|
||||
AudioStream_F32::release (block_q); |
||||
} |
||||
|
@ -0,0 +1,153 @@ |
||||
/*
|
||||
* AudioFilter90Deg_F32.h |
||||
* 22 March 2020 Bob Larkin |
||||
* Parts are based on Open Audio FIR filter by Chip Audette: |
||||
*
|
||||
* Chip Audette (OpenAudio) Feb 2017 |
||||
* - Building from AudioFilterFIR from Teensy Audio Library |
||||
* (AudioFilterFIR credited to Pete (El Supremo)) |
||||
* Copyright (c) 2020 Bob Larkin |
||||
* |
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy |
||||
* of this software and associated documentation files (the "Software"), to deal |
||||
* in the Software without restriction, including without limitation the rights |
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
||||
* copies of the Software, and to permit persons to whom the Software is |
||||
* furnished to do so, subject to the following conditions: |
||||
* |
||||
* The above copyright notice, 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. |
||||
*/ |
||||
|
||||
/*
|
||||
* This consists of two uncoupled paths that almost have the same amplitude gain |
||||
* but differ in phase by exactly 90 degrees. See AudioFilter90Deg_F32.cpp |
||||
* The number of coefficients is an odd number for the FIR Hilbert transform |
||||
* as that produces an easily achievable integer sample period delay. In |
||||
* float, the ARM FIR library routine will handle odd numbers.\, so no zero padding |
||||
* is needed. |
||||
*
|
||||
* No default Hilbert Transform is provided, as it is highly application dependent. |
||||
* The number of coefficients is an odd number with a maximum of 250. The Iowa |
||||
* Hills program can design a Hilbert Transform filter. Use begin(*pCoeff, nCoeff)
|
||||
* in the .INO to initialize this block. |
||||
* |
||||
* Status: Tested T3.6 and T4.0. No known bugs. |
||||
* Functions: |
||||
* begin(*pCoeff, nCoeff); Initializes this block, with: |
||||
* pCoeff = pointer to array of F32 Hilbert Transform coefficients |
||||
* nCoeff = uint16_t number of Hilbert transform coefficients |
||||
* showError(e); Turns error printing in update() on (e=1) and off (e=0). For debug. |
||||
* Examples:
|
||||
* ReceiverPart1.ino |
||||
* ReceiverPart2.ino |
||||
* Time: Depends on size of Hilbert FIR. Time for main body of update() including |
||||
* Hilbert FIR and compensating delay, 128 data block, running on Teensy 3.6 is: |
||||
* 19 tap Hilbert (including 0's) 74 microseconds |
||||
* 121 tap Hilbert (including 0's) 324 microseconds |
||||
* 251 tap Hilbert (including 0's) 646 microseconds |
||||
* Same 121 tap Hilbert on T4.0 is 57 microseconds per update() |
||||
* Same 251 tap Hilbert on T4.0 is 114 microseconds per update() |
||||
*/ |
||||
|
||||
#ifndef _filter_90deg_f32_h |
||||
#define _filter_90deg_f32_h |
||||
|
||||
#include "AudioStream_F32.h" |
||||
#include "arm_math.h" |
||||
|
||||
#define TEST_TIME_90D 1 |
||||
|
||||
// Following supports a maximum FIR Hilbert Transform of 251
|
||||
#define HILBERT_MAX_COEFFS 251 |
||||
|
||||
class AudioFilter90Deg_F32 : public AudioStream_F32 { |
||||
//GUI: inputs:2, outputs:2 //this line used for automatic generation of GUI node
|
||||
//GUI: shortName: 90DegPhase
|
||||
public: |
||||
// Option of AudioSettings_F32 change to block size (no sample rate dependent variables here):
|
||||
AudioFilter90Deg_F32(void) : AudioStream_F32(2, inputQueueArray_f32) { |
||||
block_size = AUDIO_BLOCK_SAMPLES; |
||||
} |
||||
AudioFilter90Deg_F32(const AudioSettings_F32 &settings) : AudioStream_F32(2, inputQueueArray_f32) { |
||||
block_size = settings.audio_block_samples; |
||||
} |
||||
|
||||
// Initialize the 90Deg by giving it the filter coefficients and number of coefficients
|
||||
// Then the delay line for the q (Right) channel is initialized
|
||||
void begin(const float32_t *cp, const int _n_coeffs) { |
||||
coeff_p = cp; |
||||
n_coeffs = _n_coeffs; |
||||
|
||||
// Initialize FIR instance (ARM DSP Math Library) (for f32 the return is always void)
|
||||
if (coeff_p!=NULL && n_coeffs<252) { |
||||
arm_fir_init_f32(&Ph90Deg_inst, n_coeffs, (float32_t *)coeff_p, &StateF32[0], block_size); |
||||
} |
||||
else { |
||||
coeff_p = NULL; // Stops further FIR filtering for Hilbert
|
||||
// Serial.println("Hilbert: Missing FIR Coefficients or number > 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 |
@ -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); |
||||
} |
@ -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 |
||||
|
@ -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<enter>", 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"); |
||||
} |
@ -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}; |
@ -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}; |
@ -0,0 +1,114 @@ |
||||
|
||||
#include "mathDSP_F32.h" |
||||
#include <math.h> |
||||
|
||||
/* 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; |
||||
} |
||||
|
@ -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 |
@ -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}; |
||||
|
Loading…
Reference in new issue