visus
/
teensy-audio
Наблюдаван 1
Харесван 0
Разклонения 0
Код Задачи 0 Заявки за сливане 0 Версии 0 Уики Activity
Преглед на файлове

Merge pull request #330 from alex6679/master 

async-spdif-input
dds
Paul Stoffregen преди 4 години
родител
18a8d0ce79 73e562a94d
ревизия
ccc305a090
No account linked to committer's email address
променени са 9 файла, в които са добавени 1716 реда и са изтрити 0 реда
Разделен изглед Покажи статистика за разликите
  1. +1
    -0
      Audio.h
  2. +232
    -0
      Quantizer.cpp
  3. +64
    -0
      Quantizer.h
  4. +387
    -0
      Resampler.cpp
  5. +223
    -0
      Resampler.h
  6. +467
    -0
      async_input_spdif3.cpp
  7. +97
    -0
      async_input_spdif3.h
  8. +204
    -0
      biquad.h
  9. +41
    -0
      examples/HardwareTesting/PassThroughAsyncSpdif/PassThroughAsyncSpdif.ino

+ 1
- 0
Audio.h Целия файл

@@ -65,6 +65,7 @@
#include "analyze_notefreq.h"
#include "analyze_peak.h"
#include "analyze_rms.h"
#include "async_input_spdif3.h"
#include "control_sgtl5000.h"
#include "control_wm8731.h"
#include "control_ak4558.h"

+ 232
- 0
Quantizer.cpp Целия файл

@@ -0,0 +1,232 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/
#include "Quantizer.h"

#define SAMPLEINVALID(sample) (!isfinite(sample) || abs(sample) >= 1.2) //use only for floating point samples (\in [-1.,1.])

Quantizer::Quantizer(float audio_sample_rate){
#ifdef DEBUG_QUANTIZER
while(!Serial);
#endif
randomSeed(1);
if(audio_sample_rate==44100.f){
_noiseSFilter[0]=-0.06935825f;
_noiseSFilter[1]=0.52540845f;
_noiseSFilter[2]= -1.20537028f;
_noiseSFilter[3]= 2.09422811f;
_noiseSFilter[4]= -3.2177438f;
_noiseSFilter[5]= 4.04852027f;
_noiseSFilter[6]= -3.83872701f;
_noiseSFilter[7]= 3.30584589f;
_noiseSFilter[8]= -2.38682527f;
// all coefficients in correct order:
// {1. , -2.38682527, 3.30584589, -3.83872701, 4.04852027,
// -3.2177438 , 2.09422811, -1.20537028, 0.52540845, -0.06935825};
} else if(audio_sample_rate==48000.f){
_noiseSFilter[0]=0.1967454f;
_noiseSFilter[1]=-0.30086406f;
_noiseSFilter[2]= 0.09575588f;
_noiseSFilter[3]= 0.58209648f;
_noiseSFilter[4]= -1.88579617f;
_noiseSFilter[5]= 3.37325788f;
_noiseSFilter[6]= -3.88076402f;
_noiseSFilter[7]= 3.58558504f;
_noiseSFilter[8]= -2.54334066f;
// all coefficients in correct order:
// {1. , -2.54334066, 3.58558504, -3.88076402, 3.37325788,
// -1.88579617, 0.58209648, 0.09575588, -0.30086406, 0.1967454};
// }
}
else {
_noiseSFilter[0]=0.f;
_noiseSFilter[1]=0.f;
_noiseSFilter[2]=0.f;
_noiseSFilter[3]=0.f;
_noiseSFilter[4]=0.f;
_noiseSFilter[5]=0.f;
_noiseSFilter[6]=0.f;
_noiseSFilter[7]=0.f;
_noiseSFilter[8]=0.f;
}
_bufferEnd0=&_buffer0[NOISE_SHAPE_F_LENGTH];
reset();
}

void Quantizer::configure(bool noiseShaping, bool dither, float factor){
_noiseShaping=noiseShaping;
_dither=dither;
_factor=factor;
reset();
}
void Quantizer::reset(){
_bPtr0=_buffer0;
_bPtr1=_buffer1;
_fOutputLastIt0=0.;
_fOutputLastIt1=0.;
memset(_buffer0, 0, NOISE_SHAPE_F_LENGTH*sizeof(float));
memset(_buffer1, 0, NOISE_SHAPE_F_LENGTH*sizeof(float));
}
void Quantizer::quantize(float* input, int16_t* output, uint16_t length){
float xn, xnD, error;
const float f2 = 1.f/1000000.f;
#ifdef DEBUG_QUANTIZER
float debugFF=1024.f;
const float factor=(powf(2.f, 15.f)-1.f)/debugFF;
#endif

for (uint16_t i =0; i< length; i++){
xn= SAMPLEINVALID(*input) ? 0. : *input*_factor; //-_fOutputLastIt0 according to paper
++input;
if (_noiseShaping){
xn+=_fOutputLastIt0;
}
if(_dither){
const uint32_t r0=random(1000000);
const uint32_t r1=random(1000000);
xnD=xn + (r0 + r1)*f2-1.f;
}
else {
xnD=xn;
}
float xnDR=round(xnD);
if (_noiseShaping){
//compute quatization error:
error=xnDR- xn;
*_bPtr0++=error;
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
}
float* f=_noiseSFilter;
_fOutputLastIt0=(*_bPtr0++ * *f++);
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
}
for (uint16_t j =1; j< NOISE_SHAPE_F_LENGTH; j++){
_fOutputLastIt0+=(*_bPtr0++ * *f++);
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
}
}
}
#ifdef DEBUG_QUANTIZER
xnDR*=debugFF;
#endif
if (xnDR > _factor){
*output=(int16_t)_factor;
}
else if (xnDR < -_factor){
*output=(int16_t)_factor;
}
else {
*output=(int16_t)xnDR;
}

++output;
}
}

void Quantizer::quantize(float* input0, float* input1, int32_t* outputInterleaved, uint16_t length){
float xn0, xnD0, error0,xnDR0, xn1, xnD1, error1,xnDR1;
const float f2 = 1.f/1000000.f;
#ifdef DEBUG_QUANTIZER
float debugFF=1024.f;
const float factor=(powf(2.f, 15.f)-1.f)/debugFF;
#endif
for (uint16_t i =0; i< length; i++){
xn0= SAMPLEINVALID(*input0) ? 0. : *input0*_factor; //-_fOutputLastIt0 according to paper
++input0;
xn1= SAMPLEINVALID(*input1) ? 0. : *input1*_factor; //-_fOutputLastIt0 according to paper
++input1;
if (_noiseShaping){
xn0+=_fOutputLastIt0;
xn1+=_fOutputLastIt1;
}
if(_dither){
uint32_t r0=random(1000000);
uint32_t r1=random(1000000);
xnD0=xn0 + (r0 + r1)*f2-1.f;
r0=random(1000000);
r1=random(1000000);
xnD1=xn1 + (r0 + r1)*f2-1.f;
}
else {
xnD0=xn0;
xnD1=xn1;
}
xnDR0=round(xnD0);
xnDR1=round(xnD1);
if (_noiseShaping){
//compute quatization error0:
error0=xnDR0- xn0;
error1=xnDR1- xn1;
*_bPtr0++=error0;
*_bPtr1++=error1;
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
_bPtr1=_buffer1;
}
float* f=_noiseSFilter;
_fOutputLastIt0=(*_bPtr0++ * *f);
_fOutputLastIt1=(*_bPtr1++ * *f++);
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
_bPtr1=_buffer1;
}
for (uint16_t j =1 ; j< NOISE_SHAPE_F_LENGTH; j++){
_fOutputLastIt0+=(*_bPtr0++ * *f);
_fOutputLastIt1+=(*_bPtr1++ * *f++);
if (_bPtr0==_bufferEnd0){
_bPtr0=_buffer0;
_bPtr1=_buffer1;
}
}
}
#ifdef DEBUG_QUANTIZER
xnDR0*=debugFF;
#endif
if (xnDR0 > _factor){
*outputInterleaved++=(int32_t)_factor;
}
else if (xnDR0 < -_factor){
*outputInterleaved++=-(int32_t)_factor;
}
else {
*outputInterleaved++=(int32_t)xnDR0;
}
if (xnDR1 > _factor){
*outputInterleaved++=(int32_t)_factor;
}
else if (xnDR1 < -_factor){
*outputInterleaved++=-(int32_t)_factor;
}
else {
*outputInterleaved++=(int32_t)xnDR1;
}
}
}

+ 64
- 0
Quantizer.h Целия файл

@@ -0,0 +1,64 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/
#ifndef quantizer_h_
#define quantizer_h_

#include "Arduino.h"

//#define DEBUG_QUANTIZER

#define NOISE_SHAPE_F_LENGTH 9 //order of filter is 10, but the first coefficient equals 1 and doesn't need to be stored

class Quantizer {
public:
///@param audio_sample_rate currently only 44.1kHz and 48kHz are supported
Quantizer(float audio_sample_rate);
void configure(bool noiseShaping, bool dither, float factor);
void quantize(float* input, int16_t* output, uint16_t length);
//attention outputInterleaved must have length 2*length
void quantize(float* input0, float* input1, int32_t* outputInterleaved, uint16_t length);
void reset();
private:

bool _noiseShaping=true;
bool _dither=true;
float _fOutputLastIt0=0.f;
float _fOutputLastIt1=0.f;
float _buffer0[NOISE_SHAPE_F_LENGTH];
float _buffer1[NOISE_SHAPE_F_LENGTH];
float* _bPtr0=_buffer0;
float* _bufferEnd0;
float* _bPtr1=_buffer1;
float _noiseSFilter[NOISE_SHAPE_F_LENGTH ];
float _factor;

};

#endif

+ 387
- 0
Resampler.cpp Целия файл

@@ -0,0 +1,387 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/

#include "Resampler.h"
#include <math.h>

Resampler::Resampler(StepAdaptionParameters settings){
#ifdef DEBUG_RESAMPLER
while (!Serial);
#endif
_settings=settings;
kaiserWindowSamples[0]=1.;
double step=1./(NO_EXACT_KAISER_SAMPLES-1);
double* xSq=kaiserWindowXsq;
for (uint16_t i = 1; i <NO_EXACT_KAISER_SAMPLES; i++){
double x=(double)i*step;
*xSq++=(1.-x*x);
}
}
void Resampler::getKaiserExact(float beta){
const double thres=1e-10;
double* winS=&kaiserWindowSamples[1];
double* t=tempRes;
for (uint16_t i = 1; i <NO_EXACT_KAISER_SAMPLES; i++){
*winS++=1.;
*t++=1.;
}
double denomLastSummand=1.;
const double halfBetaSq=beta*beta/4.;
double denom=1.;
double i=1.;
while(i < 1000){
denomLastSummand*=(halfBetaSq/(i*i));
i+=1.;
denom+=denomLastSummand;
t=tempRes;
winS=&kaiserWindowSamples[1];
double* xSq=kaiserWindowXsq;
for (uint16_t j=0; j< NO_EXACT_KAISER_SAMPLES-1;j++){
(*t)*=(*xSq);
double summand=(denomLastSummand*(*t));
(*winS)+=summand;
if (summand< thres){
break;
}
++winS;
++t;
++xSq;
}
if (denomLastSummand< thres){
break;
}
}
winS=&kaiserWindowSamples[1];
for (int32_t i = 0; i <NO_EXACT_KAISER_SAMPLES-1; i++){
*winS++/=denom;
}
}
void Resampler::setKaiserWindow(float beta, int32_t noSamples){
getKaiserExact(beta);
double step=(float)(NO_EXACT_KAISER_SAMPLES-1.)/(noSamples-1.);
double xPos=step;
float* filterCoeff=filter;
*filterCoeff=1.;
++filterCoeff;
int32_t lower=(int)(xPos);
double* windowLower=&kaiserWindowSamples[lower];
double* windowUpper=&kaiserWindowSamples[lower+1];
for (int32_t i =0; i< noSamples-2; i++){
double lambda=xPos-lower;
if (lambda > 1.){
lambda-=1.;
++windowLower;
++windowUpper;
lower++;
}
*filterCoeff++=(float)(lambda*(*windowUpper)+(1.-lambda)*(*windowLower));
xPos+=step;
if (xPos>=NO_EXACT_KAISER_SAMPLES-1 || lower >=NO_EXACT_KAISER_SAMPLES-1){
break;
}
}
*filterCoeff=*windowUpper;
}

void Resampler::setFilter(int32_t halfFiltLength,int32_t overSampling, float cutOffFrequ, float kaiserBeta){

const int32_t noSamples=halfFiltLength*overSampling+1;
setKaiserWindow(kaiserBeta, noSamples);
float* filterCoeff=filter;
*filterCoeff++=cutOffFrequ;
double step=halfFiltLength/(noSamples-1.);
double xPos=step;
double factor=M_PI*cutOffFrequ;
for (int32_t i = 0; i<noSamples-1; i++ ){
*filterCoeff++*=(float)((sin(xPos*factor)/(xPos*M_PI)));
xPos+=step;
}
}

double Resampler::getStep() const {
return _stepAdapted;
}
void Resampler::reset(){
_initialized=false;
}
void Resampler::configure(float fs, float newFs, float attenuation, int32_t minHalfFilterLength){
// Serial.print("configure, fs: ");
// Serial.println(fs);
if (fs<=0. || newFs <=0.){
_initialized=false;
return;
}
_step=(double)fs/newFs;
_configuredStep=_step;
_stepAdapted=_step;
_sum=0.;
_oldDiffs[0]=0.;
_oldDiffs[1]=0.;
for (uint8_t i =0; i< MAX_NO_CHANNELS; i++){
memset(_buffer[i], 0, sizeof(float)*MAX_HALF_FILTER_LENGTH*2);
}

float cutOffFrequ, kaiserBeta;
_overSamplingFactor=1024;
if (fs <= newFs){
cutOffFrequ=1.;
kaiserBeta=10;
_halfFilterLength=minHalfFilterLength;
}
else{
cutOffFrequ=newFs/fs;
double b=2.*(0.5*newFs-20000)/fs; //this transition band width causes aliasing. However the generated frequencies are above 20kHz
#ifdef DEBUG_RESAMPLER
Serial.print("b: ");
Serial.println(b);
#endif
double hfl=(int32_t)((attenuation-8)/(2.*2.285*TWO_PI*b)+0.5);
if (hfl >= minHalfFilterLength && hfl <= MAX_HALF_FILTER_LENGTH){
_halfFilterLength=hfl;
#ifdef DEBUG_RESAMPLER
Serial.print("Attenuation: ");
#endif
}
else if (hfl < minHalfFilterLength){
_halfFilterLength=minHalfFilterLength;
attenuation=((2*_halfFilterLength+1)-1)*(2.285*TWO_PI*b)+8;
#ifdef DEBUG_RESAMPLER
Serial.println("Resmapler: sinc filter length increased");
Serial.print("Attenuation increased to ");
#endif
}
else{
_halfFilterLength=MAX_HALF_FILTER_LENGTH;
attenuation=((2*_halfFilterLength+1)-1)*(2.285*TWO_PI*b)+8;
#ifdef DEBUG_RESAMPLER
Serial.println("Resmapler: needed sinc filter length too long");
Serial.print("Attenuation decreased to ");
#endif
}
#ifdef DEBUG_RESAMPLER
Serial.print(attenuation);
Serial.println("dB");
#endif
if (attenuation>50.){
kaiserBeta=0.1102*(attenuation-8.7);
}
else if (21<=attenuation && attenuation<=50){
kaiserBeta=0.5842*(float)pow(attenuation-21.,0.4)+0.07886*(attenuation-21.);
}
else{
kaiserBeta=0.;
}
int32_t noSamples=_halfFilterLength*_overSamplingFactor+1;
if (noSamples > MAX_FILTER_SAMPLES){
int32_t f = (noSamples-1)/(MAX_FILTER_SAMPLES-1)+1;
_overSamplingFactor/=f;
}
}

#ifdef DEBUG_RESAMPLER
Serial.print("fs: ");
Serial.println(fs);
Serial.print("cutOffFrequ: ");
Serial.println(cutOffFrequ);
Serial.print("filter length: ");
Serial.println(2*_halfFilterLength+1);
Serial.print("overSampling: ");
Serial.println(_overSamplingFactor);
Serial.print("kaiserBeta: ");
Serial.println(kaiserBeta, 12);
Serial.print("_step: ");
Serial.println(_step, 12);
#endif
setFilter(_halfFilterLength, _overSamplingFactor, cutOffFrequ, kaiserBeta);
_filterLength=_halfFilterLength*2;
for (uint8_t i =0; i< MAX_NO_CHANNELS; i++){
_endOfBuffer[i]=&_buffer[i][_filterLength];
}
_cPos=-_halfFilterLength; //marks the current center position of the filter
_initialized=true;
}
bool Resampler::initialized() const {
return _initialized;
}
void Resampler::resample(float* input0, float* input1, uint16_t inputLength, uint16_t& processedLength, float* output0, float* output1,uint16_t outputLength, uint16_t& outputCount) {
outputCount=0;
int32_t successorIndex=(int32_t)(ceil(_cPos)); //negative number -> currently the _buffer0 of the last iteration is used
float* ip0, *ip1, *fPtr;
float filterC;
float si0[2];
float si1[2];
while (floor(_cPos + _halfFilterLength) < inputLength && outputCount < outputLength){
float dist=successorIndex-_cPos;
const float distScaled=dist*_overSamplingFactor;
int32_t rightIndex=abs((int32_t)(ceil(distScaled))-_overSamplingFactor*_halfFilterLength);
const int32_t indexData=successorIndex-_halfFilterLength;
if (indexData>=0){
ip0=input0+indexData;
ip1=input1+indexData;
}
else {
ip0=_buffer[0]+indexData+_filterLength;
ip1=_buffer[1]+indexData+_filterLength;
}
fPtr=filter+rightIndex;
if (rightIndex==_overSamplingFactor*_halfFilterLength){
si1[0]=*ip0++**fPtr;
si1[1]=*ip1++**fPtr;
memset(si0, 0, 2*sizeof(float));
fPtr-=_overSamplingFactor;
rightIndex=(int32_t)(ceil(distScaled))+_overSamplingFactor; //needed below
}
else {
memset(si0, 0, 2*sizeof(float));
memset(si1, 0, 2*sizeof(float));
rightIndex=(int32_t)(ceil(distScaled)); //needed below
}
for (uint16_t i =0 ; i<_halfFilterLength; i++){
if(ip0==_endOfBuffer[0]){
ip0=input0;
ip1=input1;
}
si1[0]+=*ip0**fPtr;
si1[1]+=*ip1**fPtr;
filterC=*(fPtr+1);
si0[0]+=*ip0*filterC;
si0[1]+=*ip1*filterC;
fPtr-=_overSamplingFactor;
++ip0;
++ip1;
}
fPtr=filter+rightIndex-1;
for (uint16_t i =0 ; i<_halfFilterLength; i++){
if(ip0==_endOfBuffer[0]){
ip0=input0;
ip1=input1;
}
si0[0]+=*ip0**fPtr;
si0[1]+=*ip1**fPtr;
filterC=*(fPtr+1);
si1[0]+=*ip0*filterC;
si1[1]+=*ip1*filterC;
fPtr+=_overSamplingFactor;
++ip0;
++ip1;
}
const float w0=ceil(distScaled)-distScaled;
const float w1=1.-w0;
*output0++=si0[0]*w0 + si1[0]*w1;
*output1++=si0[1]*w0 + si1[1]*w1;

outputCount++;

_cPos+=_stepAdapted;
while (_cPos >successorIndex){
successorIndex++;
}
}
if(outputCount < outputLength){
//ouput vector not full -> we ran out of input samples
processedLength=inputLength;
}
else{
processedLength=min(inputLength, (int16_t)floor(_cPos + _halfFilterLength));
}
//fill _buffer
const int32_t indexData=processedLength-_filterLength;
if (indexData>=0){
ip0=input0+indexData;
ip1=input1+indexData;
const unsigned long long bytesToCopy= _filterLength*sizeof(float);
memcpy((void *)_buffer[0], (void *)ip0, bytesToCopy);
memcpy((void *)_buffer[1], (void *)ip1, bytesToCopy);
}
else {
float* b0=_buffer[0];
float* b1=_buffer[1];
ip0=_buffer[0]+indexData+_filterLength;
ip1=_buffer[1]+indexData+_filterLength;
for (uint16_t i =0; i< _filterLength; i++){
if(ip0==_endOfBuffer[0]){
ip0=input0;
ip1=input1;
}
*b0++ = *ip0++;
*b1++ = *ip1++;
}
}
_cPos-=processedLength;
if (_cPos < -_halfFilterLength){
_cPos=-_halfFilterLength;
}
}

void Resampler::fixStep(){
if (!_initialized){
return;
}
_step=_stepAdapted;
_sum=0.;
_oldDiffs[0]=0.;
_oldDiffs[1]=0.;
}
void Resampler::addToPos(double val){
if(val < 0){
return;
}
_cPos+=val;
}

bool Resampler::addToSampleDiff(double diff){
_oldDiffs[0]=_oldDiffs[1];
_oldDiffs[1]=(1.-_settings.alpha)*_oldDiffs[1]+_settings.alpha*diff;
const double slope=_oldDiffs[1]-_oldDiffs[0];
_sum+=diff;
double correction=_settings.kp*diff+_settings.kd*slope+_settings.ki*_sum;
const double oldStepAdapted=_stepAdapted;
_stepAdapted=_step+correction;
if (abs(_stepAdapted/_configuredStep-1.) > _settings.maxAdaption){
_initialized=false;
return false;
}
bool settled=false;
if ((abs(oldStepAdapted- _stepAdapted)/_stepAdapted < _settledThrs*abs(diff) && abs(diff) > 1.5*1e-6)) {
settled=true;
}
return settled;
}

double Resampler::getXPos() const{
return _cPos+(double)_halfFilterLength;
}

+ 223
- 0
Resampler.h Целия файл

@@ -0,0 +1,223 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/

#ifndef resampler_h_
#define resampler_h_


#include "Arduino.h"
//#define DEBUG_RESAMPLER //activates debug output

#define MAX_FILTER_SAMPLES 40961 //=1024*20 +1
#define NO_EXACT_KAISER_SAMPLES 1025
#define MAX_HALF_FILTER_LENGTH 80
#define MAX_NO_CHANNELS 8
class Resampler {
public:

struct StepAdaptionParameters {
StepAdaptionParameters(){}
double alpha =0.2; //exponential smoothing parameter
double maxAdaption = 0.01; //maximum relative allowed adaption of resampler step 0.01 = 1%
double kp= 0.6;
double ki=0.00012;
double kd= 1.8;
};
Resampler(StepAdaptionParameters settings=StepAdaptionParameters());
void reset();
///@param attenuation target attenuation [dB] of the anti-aliasing filter. Only used if newFs<fs. The attenuation can't be reached if the needed filter length exceeds 2*MAX_FILTER_SAMPLES+1
///@param minHalfFilterLength If newFs >= fs, the filter length of the resampling filter is 2*minHalfFilterLength+1. If fs y newFs the filter is maybe longer to reach the desired attenuation
void configure(float fs, float newFs, float attenuation=100, int32_t minHalfFilterLength=20);
///@param input0 first input array/ channel
///@param input1 second input array/ channel
///@param inputLength length of each input array
///@param processedLength number of samples of the input that were resampled to fill the output array
///@param output0 first output array/ channel
///@param output1 second output array/ channel
///@param outputLength length of each output array
///@param outputCount number of samples of each output array, that were filled with data
void resample(float* input0, float* input1, uint16_t inputLength, uint16_t& processedLength, float* output0, float* output1,uint16_t outputLength, uint16_t& outputCount);
bool addToSampleDiff(double diff);
double getXPos() const;
double getStep() const;
void addToPos(double val);
void fixStep();
bool initialized() const;
//resampling NOCHANNELS channels. Performance is increased a lot if the number of channels is known at compile time -> the number of channels is a template argument
template <uint8_t NOCHANNELS>
inline void resample(float** inputs, uint16_t inputLength, uint16_t& processedLength, float** outputs, uint16_t outputLength, uint16_t& outputCount){
outputCount=0;
int32_t successorIndex=(int32_t)(ceil(_cPos)); //negative number -> currently the _buffer0 of the last iteration is used
float* ip[NOCHANNELS];
float* fPtr;
float si0[NOCHANNELS];
float* si0Ptr;
float si1[NOCHANNELS];
float* si1Ptr;
while (floor(_cPos + _halfFilterLength) < inputLength && outputCount < outputLength){
float dist=successorIndex-_cPos;
float distScaled=dist*_overSamplingFactor;
int32_t rightIndex=abs((int32_t)(ceil(distScaled))-_overSamplingFactor*_halfFilterLength);
const int32_t indexData=successorIndex-_halfFilterLength;
if (indexData>=0){
for (uint8_t i =0; i< NOCHANNELS; i++){
ip[i]=inputs[i]+indexData;
}
}
else {
for (uint8_t i =0; i< NOCHANNELS; i++){
ip[i]=_buffer[i]+indexData+_filterLength;
}
}
fPtr=filter+rightIndex;
memset(si0, 0, NOCHANNELS*sizeof(float));
if (rightIndex==_overSamplingFactor*_halfFilterLength){
si1Ptr=si1;
for (uint8_t i=0; i< NOCHANNELS; i++){
*(si1Ptr++)=*ip[i]++**fPtr;
}
fPtr-=_overSamplingFactor;
rightIndex=(int32_t)(ceil(distScaled))+_overSamplingFactor; //needed below
}
else {
memset(si1, 0, NOCHANNELS*sizeof(float));
rightIndex=(int32_t)(ceil(distScaled)); //needed below
}
for (uint16_t i =0 ; i<_halfFilterLength; i++){
if(ip[0]==_endOfBuffer[0]){
for (uint8_t i =0; i< NOCHANNELS; i++){
ip[i]=inputs[i];
}
}
const float fPtrSucc=*(fPtr+1);
si0Ptr=si0;
si1Ptr=si1;
for (uint8_t i =0; i< NOCHANNELS; i++){
*(si0Ptr++)+=*ip[i]*fPtrSucc;
*(si1Ptr++)+=*ip[i]**fPtr;
++ip[i];
}
fPtr-=_overSamplingFactor;
}
fPtr=filter+rightIndex-1;
for (uint16_t i =0 ; i<_halfFilterLength; i++){
if(ip[0]==_endOfBuffer[0]){
for (uint8_t i =0; i< NOCHANNELS; i++){
ip[i]=inputs[i];
}
}
const float fPtrSucc=*(fPtr+1);
si0Ptr=si0;
si1Ptr=si1;
for (uint8_t i =0; i< NOCHANNELS; i++){
*(si0Ptr++)+=*ip[i]**fPtr;
*(si1Ptr++)+=*ip[i]*fPtrSucc;
++ip[i];
}
fPtr+=_overSamplingFactor;
}
const float w0=ceil(distScaled)-distScaled;
const float w1=1.-w0;
si0Ptr=si0;
si1Ptr=si1;
for (uint8_t i =0; i< NOCHANNELS; i++){
*outputs[i]++=*(si0Ptr++)*w0 + *(si1Ptr++)*w1;
}
outputCount++;
_cPos+=_stepAdapted;
while (_cPos >successorIndex){
successorIndex++;
}
}
if(outputCount < outputLength){
//ouput vector not full -> we ran out of input samples
processedLength=inputLength;
}
else{
processedLength=min(inputLength, (int16_t)floor(_cPos + _halfFilterLength));
}
//fill _buffer
const int32_t indexData=processedLength-_filterLength;
if (indexData>=0){
const unsigned long long bytesToCopy= _filterLength*sizeof(float);
float** inPtr=inputs;
for (uint8_t i =0; i< NOCHANNELS; i++){
memcpy((void *)_buffer[i], (void *)((*inPtr)+indexData), bytesToCopy);
++inPtr;
}
}
else {
float** inPtr=inputs;
for (uint8_t i =0; i< NOCHANNELS; i++){
float* b=_buffer[i];
float* ip=b+indexData+_filterLength;
for (uint16_t j =0; j< _filterLength; j++){
if(ip==_endOfBuffer[i]){
ip=*inPtr;
}
*b++ = *ip++;
}
++inPtr;
}
}
_cPos-=processedLength;
if (_cPos < -_halfFilterLength){
_cPos=-_halfFilterLength;
}
}
private:
void getKaiserExact(float beta);
void setKaiserWindow(float beta, int32_t noSamples);
void setFilter(int32_t halfFiltLength,int32_t overSampling, float cutOffFrequ, float kaiserBeta);
float filter[MAX_FILTER_SAMPLES];
double kaiserWindowSamples[NO_EXACT_KAISER_SAMPLES];
double tempRes[NO_EXACT_KAISER_SAMPLES-1];
double kaiserWindowXsq[NO_EXACT_KAISER_SAMPLES-1];
float _buffer[MAX_NO_CHANNELS][MAX_HALF_FILTER_LENGTH*2];
float* _endOfBuffer[MAX_NO_CHANNELS];

int32_t _overSamplingFactor;
int32_t _halfFilterLength;
int32_t _filterLength;
bool _initialized=false;
const double _settledThrs = 1e-6;
StepAdaptionParameters _settings;
double _configuredStep;
double _step;
double _stepAdapted;
double _cPos;
double _sum;
double _oldDiffs[2];
};

#endif

+ 467
- 0
async_input_spdif3.cpp Целия файл

@@ -0,0 +1,467 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/
#if defined(__IMXRT1052__) || defined(__IMXRT1062__)

#include "async_input_spdif3.h"
#include "biquad.h"
#include <utility/imxrt_hw.h>
//Parameters
namespace {
#define SPDIF_RX_BUFFER_LENGTH AUDIO_BLOCK_SAMPLES
const int32_t bufferLength=8*AUDIO_BLOCK_SAMPLES;
const uint16_t noSamplerPerIsr=SPDIF_RX_BUFFER_LENGTH/4;
}
volatile bool AsyncAudioInputSPDIF3::resetResampler=true;

#ifdef DEBUG_SPDIF_IN
volatile bool AsyncAudioInputSPDIF3::bufferOverflow=false;
#endif

volatile uint32_t AsyncAudioInputSPDIF3::microsLast;

DMAMEM __attribute__((aligned(32)))
static int32_t spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
static float bufferR[bufferLength];
static float bufferL[bufferLength];

volatile int32_t AsyncAudioInputSPDIF3::buffer_offset = 0; // read by resample/ written in spdif input isr -> copied at the beginning of 'resmaple' protected by __disable_irq() in resample
int32_t AsyncAudioInputSPDIF3::resample_offset = 0; // read/written by resample/ read in spdif input isr -> no protection needed?

volatile bool AsyncAudioInputSPDIF3::lockChanged=false;
volatile bool AsyncAudioInputSPDIF3::locked=false;
DMAChannel AsyncAudioInputSPDIF3::dma(false);

AsyncAudioInputSPDIF3::~AsyncAudioInputSPDIF3(){
delete [] _bufferLPFilter.pCoeffs;
delete [] _bufferLPFilter.pState;
delete quantizer[0];
delete quantizer[1];
}

PROGMEM
AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength) : AudioStream(0, NULL) {
_attenuation=attenuation;
_minHalfFilterLength=minHalfFilterLength;
const float factor = powf(2, 15)-1.f; // to 16 bit audio
quantizer[0]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
quantizer[0]->configure(noiseshaping, dither, factor);
quantizer[1]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
quantizer[1]->configure(noiseshaping, dither, factor);
begin();
}
PROGMEM
void AsyncAudioInputSPDIF3::begin()
{
dma.begin(true); // Allocate the DMA channel first
const uint32_t noByteMinorLoop=2*4;
dma.TCD->SOFF = 4;
dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(2) | DMA_TCD_ATTR_DSIZE(2);
dma.TCD->NBYTES_MLNO = DMA_TCD_NBYTES_MLOFFYES_NBYTES(noByteMinorLoop) | DMA_TCD_NBYTES_SMLOE |
DMA_TCD_NBYTES_MLOFFYES_MLOFF(-8);
dma.TCD->SLAST = -8;
dma.TCD->DOFF = 4;
dma.TCD->CITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
dma.TCD->DLASTSGA = -sizeof(spdif_rx_buffer);
dma.TCD->BITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR;
dma.TCD->SADDR = (void *)((uint32_t)&SPDIF_SRL);
dma.TCD->DADDR = spdif_rx_buffer;
dma.triggerAtHardwareEvent(DMAMUX_SOURCE_SPDIF_RX);

SPDIF_SCR |=SPDIF_SCR_DMA_RX_EN; //DMA Receive Request Enable
dma.enable();
dma.attachInterrupt(isr);
config_spdifIn();
#ifdef DEBUG_SPDIF_IN
while (!Serial);
#endif
_bufferLPFilter.pCoeffs=new float[5];
_bufferLPFilter.numStages=1;
_bufferLPFilter.pState=new float[2];
getCoefficients(_bufferLPFilter.pCoeffs, BiquadType::LOW_PASS, 0., 5., AUDIO_SAMPLE_RATE_EXACT/AUDIO_BLOCK_SAMPLES, 0.5);
}
bool AsyncAudioInputSPDIF3::isLocked() const {
__disable_irq();
bool l=locked;
__enable_irq();
return l;
}
void AsyncAudioInputSPDIF3::spdif_interrupt(){
if(SPDIF_SIS & SPDIF_SIS_LOCK){
if (!locked){
locked=true;
lockChanged=true;
}
}
else if(SPDIF_SIS & SPDIF_SIS_LOCKLOSS){
if (locked){
locked=false;
lockChanged=true;
resetResampler=true;
}
}
SPDIF_SIC |= SPDIF_SIC_LOCKLOSS;//clear SPDIF_SIC_LOCKLOSS interrupt
SPDIF_SIC |= SPDIF_SIC_LOCK; //clear SPDIF_SIC_LOCK interrupt
}

void AsyncAudioInputSPDIF3::resample(int16_t* data_left, int16_t* data_right, int32_t& block_offset){
block_offset=0;
if(!_resampler.initialized()){
return;
}
__disable_irq();
if(!locked){
__enable_irq();
return;
}
int32_t bOffset=buffer_offset;
int32_t resOffset=resample_offset;
__enable_irq();
uint16_t inputBufferStop = bOffset >= resOffset ? bOffset-resOffset : bufferLength-resOffset;
if (inputBufferStop==0){
return;
}
uint16_t processedLength;
uint16_t outputCount=0;
uint16_t outputLength=AUDIO_BLOCK_SAMPLES;

float resampledBufferL[AUDIO_BLOCK_SAMPLES];
float resampledBufferR[AUDIO_BLOCK_SAMPLES];
_resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL, resampledBufferR, outputLength, outputCount);
resOffset=(resOffset+processedLength)%bufferLength;
block_offset=outputCount;

if (bOffset > resOffset && block_offset< AUDIO_BLOCK_SAMPLES){
inputBufferStop= bOffset-resOffset;
outputLength=AUDIO_BLOCK_SAMPLES-block_offset;
_resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL+block_offset, resampledBufferR+block_offset, outputLength, outputCount);
resOffset=(resOffset+processedLength)%bufferLength;
block_offset+=outputCount;
}
quantizer[0]->quantize(resampledBufferL, data_left, block_offset);
quantizer[1]->quantize(resampledBufferR, data_right, block_offset);
__disable_irq();
resample_offset=resOffset;
__enable_irq();
}

void AsyncAudioInputSPDIF3::isr(void)
{
dma.clearInterrupt();
microsLast=micros();
const int32_t *src, *end;
uint32_t daddr = (uint32_t)(dma.TCD->DADDR);

if (daddr < (uint32_t)spdif_rx_buffer + sizeof(spdif_rx_buffer) / 2) {
// DMA is receiving to the first half of the buffer
// need to remove data from the second half
src = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
//if (AsyncAudioInputSPDIF3::update_responsibility) AudioStream::update_all();
} else {
// DMA is receiving to the second half of the buffer
// need to remove data from the first half
src = (int32_t *)&spdif_rx_buffer[0];
end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
}
if (buffer_offset >=resample_offset ||
(buffer_offset + SPDIF_RX_BUFFER_LENGTH/4) < resample_offset) {
#if IMXRT_CACHE_ENABLED >=1
arm_dcache_delete((void*)src, sizeof(spdif_rx_buffer) / 2);
#endif
float *destR = &(bufferR[buffer_offset]);
float *destL = &(bufferL[buffer_offset]);
const float factor= pow(2., 23.)+1;
do {
int32_t n=(*src) & 0x800000 ? (*src)|0xFF800000 : (*src) & 0xFFFFFF;
*destL++ = (float)(n)/factor;
++src;

n=(*src) & 0x800000 ? (*src)|0xFF800000 : (*src) & 0xFFFFFF;
*destR++ = (float)(n)/factor;
++src;
} while (src < end);
buffer_offset=(buffer_offset+SPDIF_RX_BUFFER_LENGTH/4)%bufferLength;
}
#ifdef DEBUG_SPDIF_IN
else {
bufferOverflow=true;
}
#endif
}
double AsyncAudioInputSPDIF3::getNewValidInputFrequ(){
//page 2129: FrequMeas[23:0]=FreqMeas_CLK / BUS_CLK * 2^10 * GAIN
if (SPDIF_SRPC & SPDIF_SRPC_LOCK){
const double f=(float)F_BUS_ACTUAL/(1024.*1024.*24.*128.);// bit clock = 128 * sampling frequency
const double freqMeas=(SPDIF_SRFM & 0xFFFFFF)*f;
if (_lastValidInputFrequ != freqMeas){//frequency not stable yet;
_lastValidInputFrequ=freqMeas;
return -1.;
}
return _lastValidInputFrequ;
}
return -1.;
}

double AsyncAudioInputSPDIF3::getBufferedTime() const{
__disable_irq();
double n=_bufferedTime;
__enable_irq();
return n;
}

void AsyncAudioInputSPDIF3::configure(){
__disable_irq();
if(resetResampler){
_resampler.reset();
resetResampler=false;
}
if(!locked){
__enable_irq();
#ifdef DEBUG_SPDIF_IN
Serial.println("lock lost");
#endif
return;
}
#ifdef DEBUG_SPDIF_IN
const bool bOverf=bufferOverflow;
bufferOverflow=false;
#endif
const bool lc=lockChanged;
__enable_irq();

#ifdef DEBUG_SPDIF_IN
if (bOverf){
Serial.print("buffer overflow, buffer offset: ");
Serial.print(buffer_offset);
Serial.print(", resample_offset: ");
Serial.println(resample_offset);
if (!_resampler.initialized()){
Serial.println("_resampler not initialized. ");
}
}
#endif
if (lc || !_resampler.initialized()){
const double inputF=getNewValidInputFrequ(); //returns: -1 ... invalid frequency
if (inputF > 0.){
__disable_irq();
lockChanged=false; //only reset lockChanged if a valid frequency was received (inputFrequ > 0.)
__enable_irq();
//we got a valid sample frequency
const double frequDiff=inputF/_inputFrequency-1.;
if (abs(frequDiff) > 0.01 || !_resampler.initialized()){
//the new sample frequency differs from the last one -> configure the _resampler again
_inputFrequency=inputF;
const int32_t targetLatency=round(_targetLatencyS*inputF);
_targetLatencyS=max(0.001,(noSamplerPerIsr*3./2./_inputFrequency));
__disable_irq();
resample_offset = targetLatency <= buffer_offset ? buffer_offset - targetLatency : bufferLength -(targetLatency-buffer_offset);
__enable_irq();
_resampler.configure(inputF, AUDIO_SAMPLE_RATE_EXACT, _attenuation, _minHalfFilterLength);
#ifdef DEBUG_SPDIF_IN
Serial.print("_maxLatency: ");
Serial.println(_maxLatency);
Serial.print("targetLatency: ");
Serial.println(targetLatency);
Serial.print("relative frequ diff: ");
Serial.println(frequDiff, 8);
Serial.print("configure _resampler with frequency ");
Serial.println(inputF,8);
#endif
}
}
}
}

void AsyncAudioInputSPDIF3::monitorResampleBuffer(){
if(!_resampler.initialized()){
return;
}
__disable_irq();
const double dmaOffset=(micros()-microsLast)*1e-6; //[seconds]
double bTime = resample_offset <= buffer_offset ? (buffer_offset-resample_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset : (bufferLength-resample_offset +buffer_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset; //[seconds]
double diff = bTime- (_blockDuration+ _targetLatencyS); //seconds

biquad_cascade_df2T<double, arm_biquad_cascade_df2T_instance_f32, float>(&_bufferLPFilter, &diff, &diff, 1);
bool settled=_resampler.addToSampleDiff(diff);
if (bTime > _maxLatency || bTime-dmaOffset<= _blockDuration || settled) {
double distance=(_blockDuration+_targetLatencyS-dmaOffset)*_lastValidInputFrequ+_resampler.getXPos();
diff=0.;
if (distance > bufferLength-noSamplerPerIsr){
diff=bufferLength-noSamplerPerIsr-distance;
distance=bufferLength-noSamplerPerIsr;
}
if (distance < 0.){
distance=0.;
diff=- (_blockDuration+ _targetLatencyS);
}
double resample_offsetF=buffer_offset-distance;
resample_offset=(int32_t)floor(resample_offsetF);
_resampler.addToPos(resample_offsetF-resample_offset);
while (resample_offset<0){
resample_offset+=bufferLength;
}
//int32_t b_offset=buffer_offset;
#ifdef DEBUG_SPDIF_IN
double bTimeFixed = resample_offset <= buffer_offset ? (buffer_offset-resample_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset : (bufferLength-resample_offset +buffer_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset; //[seconds]
#endif
__enable_irq();
#ifdef DEBUG_SPDIF_IN
// Serial.print("settled: ");
// Serial.println(settled);
Serial.print("bTime: ");
Serial.print(bTime*1e6,3);
Serial.print("_maxLatency: ");
Serial.println(_maxLatency*1e6,3);
Serial.print("bTime-dmaOffset: ");
Serial.print((bTime-dmaOffset)*1e6,3);
Serial.print(", _blockDuration: ");
Serial.print(_blockDuration*1e6,3);
Serial.print("bTimeFixed: ");
Serial.print(bTimeFixed*1e6,3);

#endif
preload(&_bufferLPFilter, diff);
_resampler.fixStep();
}
else {
__enable_irq();
}
_bufferedTime=_targetLatencyS+diff;
}

void AsyncAudioInputSPDIF3::update(void)
{
configure();
monitorResampleBuffer(); //important first call 'monitorResampleBuffer' then 'resample'
audio_block_t *block_left =allocate();
audio_block_t *block_right =nullptr;
if (block_left!= nullptr) {
block_right = allocate();
if (block_right == nullptr) {
release(block_left);
block_left = nullptr;
}
}
if (block_left && block_right) {
int32_t block_offset;
resample(block_left->data, block_right->data,block_offset);
if(block_offset < AUDIO_BLOCK_SAMPLES){
memset(block_left->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t));
memset(block_right->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t));
#ifdef DEBUG_SPDIF_IN
Serial.print("filled only ");
Serial.print(block_offset);
Serial.println(" samples.");
#endif
}
transmit(block_left, 0);
release(block_left);
block_left=nullptr;
transmit(block_right, 1);
release(block_right);
block_right=nullptr;
}
#ifdef DEBUG_SPDIF_IN
else {
Serial.println("Not enough blocks available. Too few audio memory?");
}
#endif
}
double AsyncAudioInputSPDIF3::getInputFrequency() const{
__disable_irq();
double f=_lastValidInputFrequ;
__enable_irq();
return f;
}
double AsyncAudioInputSPDIF3::getTargetLantency() const {
__disable_irq();
double l=_targetLatencyS;
__enable_irq();
return l ;
}
void AsyncAudioInputSPDIF3::config_spdifIn(){
//CCM Clock Gating Register 5, imxrt1060_rev1.pdf page 1145
CCM_CCGR5 |=CCM_CCGR5_SPDIF(CCM_CCGR_ON); //turn spdif clock on - necessary for receiver!
SPDIF_SCR |=SPDIF_SCR_RXFIFO_OFF_ON; //turn receive fifo off 1->off, 0->on

SPDIF_SCR&=~(SPDIF_SCR_RXFIFO_CTR); //reset rx fifo control: normal opertation

SPDIF_SCR&=~(SPDIF_SCR_RXFIFOFULL_SEL(3)); //reset rx full select
SPDIF_SCR|=SPDIF_SCR_RXFIFOFULL_SEL(2); //full interrupt if at least 8 sample in Rx left and right FIFOs

SPDIF_SCR|=SPDIF_SCR_RXAUTOSYNC; //Rx FIFO auto sync on

SPDIF_SCR&=(~SPDIF_SCR_USRC_SEL(3)); //No embedded U channel

CORE_PIN15_CONFIG = 3; //pin 15 set to alt3 -> spdif input

/// from eval board sample code
// IOMUXC_SetPinConfig(
// IOMUXC_GPIO_AD_B1_03_SPDIF_IN, /* GPIO_AD_B1_03 PAD functional properties : */
// 0x10B0u); /* Slew Rate Field: Slow Slew Rate
// Drive Strength Field: R0/6
// Speed Field: medium(100MHz)
// Open Drain Enable Field: Open Drain Disabled
// Pull / Keep Enable Field: Pull/Keeper Enabled
// Pull / Keep Select Field: Keeper
// Pull Up / Down Config. Field: 100K Ohm Pull Down
// Hyst. Enable Field: Hysteresis Disabled */
CORE_PIN15_PADCONFIG=0x10B0;
SPDIF_SCR &=(~SPDIF_SCR_RXFIFO_OFF_ON); //receive fifo is turned on again


SPDIF_SRPC &= ~SPDIF_SRPC_CLKSRC_SEL(15); //reset clock selection page 2136
//SPDIF_SRPC |=SPDIF_SRPC_CLKSRC_SEL(6); //if (DPLL Locked) SPDIF_RxClk else tx_clk (SPDIF0_CLK_ROOT)
//page 2129: FrequMeas[23:0]=FreqMeas_CLK / BUS_CLK * 2^10 * GAIN
SPDIF_SRPC &=~SPDIF_SRPC_GAINSEL(7); //reset gain select 0 -> gain = 24*2^10
//SPDIF_SRPC |= SPDIF_SRPC_GAINSEL(3); //gain select: 8*2^10
//==============================================

//interrupts
SPDIF_SIE |= SPDIF_SIE_LOCK; //enable spdif receiver lock interrupt
SPDIF_SIE |=SPDIF_SIE_LOCKLOSS;

lockChanged=true;
attachInterruptVector(IRQ_SPDIF, spdif_interrupt);
NVIC_SET_PRIORITY(IRQ_SPDIF, 208); // 255 = lowest priority, 208 = priority of update
NVIC_ENABLE_IRQ(IRQ_SPDIF);

SPDIF_SIC |= SPDIF_SIC_LOCK; //clear SPDIF_SIC_LOCK interrupt
SPDIF_SIC |= SPDIF_SIC_LOCKLOSS;//clear SPDIF_SIC_LOCKLOSS interrupt
locked=(SPDIF_SRPC & SPDIF_SRPC_LOCK);
}
#endif


+ 97
- 0
async_input_spdif3.h Целия файл

@@ -0,0 +1,97 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/
#ifndef async_input_spdif3_h_
#define async_input_spdif3_h_
#include "Resampler.h"
#include "Quantizer.h"
#include "Arduino.h"
#include "AudioStream.h"
#include "DMAChannel.h"
#include <arm_math.h>

//#define DEBUG_SPDIF_IN //activates debug output

class AsyncAudioInputSPDIF3 : public AudioStream
{
public:
///@param attenuation target attenuation [dB] of the anti-aliasing filter. Only used if newFs<fs. The attenuation can't be reached if the needed filter length exceeds 2*MAX_FILTER_SAMPLES+1
///@param minHalfFilterLength If newFs >= fs, the filter length of the resampling filter is 2*minHalfFilterLength+1. If fs y newFs the filter is maybe longer to reach the desired attenuation
AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength);
~AsyncAudioInputSPDIF3();
virtual void update(void);
void begin();
void stop();
double getBufferedTime() const;
double getInputFrequency() const;
bool isLocked() const;
double getTargetLantency() const;
protected:
static DMAChannel dma;
static void isr(void);
private:
void resample(int16_t* data_left, int16_t* data_right, int32_t& block_offset);
void monitorResampleBuffer();
void configure();
double getNewValidInputFrequ();
void config_spdifIn();

//accessed in isr ====
static volatile int32_t buffer_offset;
static int32_t resample_offset;
static volatile uint32_t microsLast;
//====================

// spdif lock-changed interrupt
static volatile bool locked;
static volatile bool lockChanged;
static volatile bool resetResampler;
static void spdif_interrupt();
#ifdef MEASURE_FREQ
static FrequencyMeasurement frequMeasure;
#endif
//=============================
float _attenuation;
int32_t _minHalfFilterLength;
Resampler _resampler;
Quantizer* quantizer[2];
arm_biquad_cascade_df2T_instance_f32 _bufferLPFilter;
volatile double _bufferedTime;
volatile double _lastValidInputFrequ;
double _inputFrequency;
double _targetLatencyS; //target latency [seconds]
const double _blockDuration=AUDIO_BLOCK_SAMPLES/AUDIO_SAMPLE_RATE; //[seconds]
const double _maxLatency=2.*_blockDuration;

#ifdef DEBUG_SPDIF_IN
static volatile bool bufferOverflow;
#endif
};

#endif

+ 204
- 0
biquad.h Целия файл

@@ -0,0 +1,204 @@
/* Audio Library for Teensy 3.X
* Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
*
* 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.
*/
/*
by Alexander Walch
*/
#ifndef biquad_coeffs_h_
#define biquad_coeffs_h_

#include "Arduino.h"
#include <arm_math.h>

enum class BiquadType {
LOW_PASS, HIGH_PASS, BAND_PASS, NOTCH, ALL_PASS, PEAKING, LOW_SHELF, HIGH_SHELF
};

template <typename T>
void getCoefficients(T* coeffs, BiquadType type, double dbGain, double freq, double srate, double bandwidthOrQOrS, bool isBandwidthOrS=false){
const double A =(type == BiquadType::PEAKING || type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) ? pow(10., dbGain / 40.) : pow(10, dbGain / 20);
const double omega = 2 * M_PI * freq / srate;
const double sn = sin(omega);
const double cs = cos(omega);
double alpha;

if (!isBandwidthOrS) // Q
alpha = sn / (2 * bandwidthOrQOrS);
else if (type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) // S
alpha = sn / 2 * sqrt((A + 1 / A) * (1 / bandwidthOrQOrS - 1) + 2);
else // BW
alpha = sn * sinh(_M_LN2 / 2 * bandwidthOrQOrS * omega / sn);

const double beta = 2 * sqrt(A) * alpha;

double b0, b1, b2, a0Inv, a1, a2;

switch (type)
{
case BiquadType::LOW_PASS:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0Inv = 1/(1 + alpha);
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case BiquadType::HIGH_PASS:
b0 = (1 + cs) / 2;
b1 = -(1 + cs);
b2 = (1 + cs) / 2;
a0Inv = 1/(1 + alpha);
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case BiquadType::BAND_PASS:
b0 = alpha;
b1 = 0;
b2 = -alpha;
a0Inv = 1/(1 + alpha);
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case BiquadType::NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
a0Inv = 1/(1 + alpha);
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case BiquadType::ALL_PASS:
b0 = 1 - alpha;
b1 = -2 * cs;
b2 = 1 + alpha;
a0Inv = 1/(1 + alpha);
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case BiquadType::PEAKING:
b0 = 1 + (alpha * A);
b1 = -2 * cs;
b2 = 1 - (alpha * A);
a0Inv = 1/(1 + (alpha / A));
a1 = -2 * cs;
a2 = 1 - (alpha / A);
break;
case BiquadType::LOW_SHELF:
b0 = A * ((A + 1) - (A - 1) * cs + beta);
b1 = 2 * A * ((A - 1) - (A + 1) * cs);
b2 = A * ((A + 1) - (A - 1) * cs - beta);
a0Inv = (A + 1) + (A - 1) * cs + beta;
a1 = -2 * ((A - 1) + (A + 1) * cs);
a2 = (A + 1) + (A - 1) * cs - beta;
break;
case BiquadType::HIGH_SHELF:
b0 = A * ((A + 1) + (A - 1) * cs + beta);
b1 = -2 * A * ((A - 1) + (A + 1) * cs);
b2 = A * ((A + 1) + (A - 1) * cs - beta);
a0Inv = 1/((A + 1) - (A - 1) * cs + beta);
a1 = 2 * ((A - 1) - (A + 1) * cs);
a2 = (A + 1) - (A - 1) * cs - beta;
break;
}

*coeffs++=(T)(b0 * a0Inv);
*coeffs++=(T)(b1 * a0Inv);
*coeffs++=(T)(b2 * a0Inv);
*coeffs++=(T)(-a1 * a0Inv);
*coeffs=(T)(-a2 * a0Inv);
}

template <typename T, typename BIQUAD, typename BTYPE>
void biquad_cascade_df2T(const BIQUAD* S, T* pSrc, T* pDst, uint32_t blockSize) {
BTYPE* b0 =S->pCoeffs;
BTYPE* b1=S->pCoeffs+1;
BTYPE* b2=S->pCoeffs+2;
BTYPE* a1Neg=S->pCoeffs+3;
BTYPE* a2Neg=S->pCoeffs+4;

BTYPE* state=S->pState;
if(S->numStages==1){
BTYPE yn;
for (uint32_t j=0; j<blockSize; j++ ){
yn = *b0 * *pSrc + *state;
*state = *b1 * *pSrc + *a1Neg * yn + *(state+1);
*(state+1) = *b2 * *pSrc++ + *a2Neg * yn;
*pDst++=(T)yn;
}
}
else {

BTYPE pDstD[blockSize];
BTYPE* pDstDP=pDstD;
for (uint32_t j=0; j<blockSize; j++ ){
*pDstDP = *b0 * *pSrc + *state;
*state = *b1 * *pSrc + *a1Neg * *pDstDP + *(state+1);
*(state+1) = *b2 * *pSrc++ + *a2Neg * *pDstDP++;
}
b0+=5;
b1+=5;
b2+=5;
a1Neg+=5;
a2Neg+=5;
state+=2;
for (uint8_t i =0; i< S->numStages - 2;i++){
pDstDP=pDstD;
BTYPE xn;
for (uint32_t j=0; j<blockSize; j++ ){
xn=*pDstDP;
*pDstDP = *b0 * xn + *state;
*state = *b1 * xn + *a1Neg * *pDstDP + *(state+1);
*(state+1) = *b2 * xn + *a2Neg * *pDstDP++;
}
b0+=5;
b1+=5;
b2+=5;
a1Neg+=5;
a2Neg+=5;
state+=2;
}
BTYPE yn;
pDstDP=pDstD;
for (uint32_t j=0; j<blockSize; j++ ){
yn = *b0 * *pDstDP + *state;
*state = *b1 * *pDstDP + *a1Neg * yn + *(state+1);
*(state+1) = *b2 * *pDstDP++ + *a2Neg * yn;
*pDst++=(T)yn;
}
}
}

template <typename B>
void preload(const B* S, double val=0.){
// double* b1=B->pCoeffs+1;
// double* b2=B->pCoeffs+2;
// double* a1Neg=B->pCoeffs+3;
// double* a2Neg=B->pCoeffs+4;
*(S->pState+1) = (*(S->pCoeffs+2) + *(S->pCoeffs+4)) * val;
*(S->pState) = (*(S->pCoeffs+1) + *(S->pCoeffs+3)) * val + *(S->pState+1);
}

#endif

+ 41
- 0
examples/HardwareTesting/PassThroughAsyncSpdif/PassThroughAsyncSpdif.ino Целия файл

@@ -0,0 +1,41 @@

#include <Audio.h>

AudioOutputSPDIF3 spdifOut;
AsyncAudioInputSPDIF3 spdifIn(true, true, 100, 20); //dither = true, noiseshaping = true, anti-aliasing attenuation=100dB, minimum resampling filter length=20
//

AudioConnection patchCord1(spdifIn, 0, spdifOut, 0);
AudioConnection patchCord2(spdifIn, 1, spdifOut, 1);

void setup() {

// put your setup code here, to run once:
AudioMemory(12);
while (!Serial);

}

void loop() {
double bufferedTine=spdifIn.getBufferedTime();
//double targetLatency = spdifIn.getTargetLantency();
Serial.print("buffered time [micro seconds]: ");
Serial.println(bufferedTine*1e6,2);
// Serial.print(", target: ");
// Serial.println(targetLatency*1e6,2);
double pUsageIn=spdifIn.processorUsage();
Serial.print("processor usage [%]: ");
Serial.println(pUsageIn);

// bool islocked=spdifIn.isLocked();
// Serial.print("isLocked: ");
// Serial.println(islocked);

// double f=spdifIn.getInputFrequency();
// Serial.print("frequency: ");
// Serial.println(f);
// Serial.print("Memory max: ");
// Serial.println(AudioMemoryUsageMax());
delay(500);
}

Write Preview
Loading…
Отказ
Запис