You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
116 lines
3.8 KiB
116 lines
3.8 KiB
/*
|
|
* AudioLMSDenoiseNotch_F32
|
|
*
|
|
* 22 January 2022 copyright (c)Robert Larkin 2022
|
|
*
|
|
* 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 "AudioStream_F32.h"
|
|
#include "AudioLMSDenoiseNotch_F32.h"
|
|
|
|
void AudioLMSDenoiseNotch_F32::update(void)
|
|
{
|
|
uint16_t j, k;
|
|
float32_t blockDataIn, error, firOut;
|
|
audio_block_f32_t *block;
|
|
|
|
block = AudioStream_F32::receiveWritable_f32();
|
|
if (!block) return;
|
|
|
|
if(!doLMS)
|
|
{
|
|
AudioStream_F32::transmit(block);
|
|
AudioStream_F32::release(block);
|
|
return;
|
|
}
|
|
|
|
audio_block_f32_t *blockOut;
|
|
blockOut = AudioStream_F32::allocate_f32(); // Output block
|
|
if (!blockOut)
|
|
{
|
|
AudioStream_F32::transmit(block);
|
|
AudioStream_F32::release(block);
|
|
return;
|
|
}
|
|
|
|
for(int i=0; i<128; i++)
|
|
{
|
|
blockDataIn = block->data[i];
|
|
|
|
// Leakage on one coefficient
|
|
coeff[numLeak] *= decay; // Decay one coefficient
|
|
if(++numLeak >= lengthDataF) // Wrap around, if needed
|
|
numLeak = 0;
|
|
|
|
// Circular delay line to find correlated components
|
|
dataD[kNextD] = blockDataIn; // Get a new data point from block
|
|
|
|
#ifdef LMS_NORMALIZE
|
|
powerNorm[i] = blockDataIn*blockDataIn;
|
|
pNorm += powerNorm[i];
|
|
if(i==127)
|
|
pNorm -= powerNorm[0];
|
|
else
|
|
pNorm -= powerNorm[i+1];
|
|
#endif
|
|
|
|
if(++kNextD >= lengthDataD) // Next spot in delay line
|
|
kNextD = 0;
|
|
|
|
// Update the FIR.
|
|
dataF[kOffsetF] = dataD[kNextD]; // Input FIR is output Delay
|
|
firOut = 0.0f;
|
|
for(j=0; j<lengthDataF; j++) // Over all coefficients
|
|
{
|
|
k = (j + kOffsetF) & kMask; // Data circular buffer
|
|
firOut +=coeff[j]*dataF[k];
|
|
}
|
|
|
|
// Compute the error, the difference between the data point
|
|
// just received and the FIR output.
|
|
error = blockDataIn - firOut;
|
|
|
|
// Update the coefficients
|
|
#ifdef LMS_NORMALIZE
|
|
float32_t kcf = error*beta/pNorm;
|
|
#else
|
|
float32_t kcf = error*beta;
|
|
#endif
|
|
for(j=0; j<lengthDataF; j++)
|
|
{
|
|
k = (j + kOffsetF) & kMask;
|
|
coeff[j] = coeff[j] + kcf*dataF[k];
|
|
}
|
|
|
|
// Move to next positions in circular data buffer via kOffsetF
|
|
if(++kOffsetF >= lengthDataF)
|
|
kOffsetF = 0; // Wrap the FIR circular buffer
|
|
|
|
// fir out to output block
|
|
if(what == DENOISE)
|
|
blockOut->data[i] = firOut;
|
|
else
|
|
blockOut->data[i] = error; // Auto-Notch
|
|
}
|
|
//transmit the block and be done
|
|
AudioStream_F32::transmit(blockOut);
|
|
AudioStream_F32::release(block);
|
|
AudioStream_F32::release(blockOut);
|
|
}
|
|
|