| #include "Resampler.h" | #include "Resampler.h" | ||||
| #include <math.h> | #include <math.h> | ||||
| Resampler::Resampler(StepAdaptionParameters settings){ | |||||
| Resampler::Resampler(float attenuation, int32_t minHalfFilterLength, int32_t maxHalfFilterLength, StepAdaptionParameters settings): _targetAttenuation(attenuation) | |||||
| { | |||||
| _maxHalfFilterLength=max(1, min(MAX_HALF_FILTER_LENGTH, maxHalfFilterLength)); | |||||
| _minHalfFilterLength=max(1, min(maxHalfFilterLength, minHalfFilterLength)); | |||||
| #ifdef DEBUG_RESAMPLER | #ifdef DEBUG_RESAMPLER | ||||
| while (!Serial); | while (!Serial); | ||||
| #endif | #endif | ||||
| void Resampler::reset(){ | void Resampler::reset(){ | ||||
| _initialized=false; | _initialized=false; | ||||
| } | } | ||||
| void Resampler::configure(float fs, float newFs, float attenuation, int32_t minHalfFilterLength){ | |||||
| void Resampler::configure(float fs, float newFs){ | |||||
| // Serial.print("configure, fs: "); | // Serial.print("configure, fs: "); | ||||
| // Serial.println(fs); | // Serial.println(fs); | ||||
| if (fs<=0. || newFs <=0.){ | if (fs<=0. || newFs <=0.){ | ||||
| _initialized=false; | _initialized=false; | ||||
| return; | return; | ||||
| } | } | ||||
| _attenuation=_targetAttenuation; | |||||
| _step=(double)fs/newFs; | _step=(double)fs/newFs; | ||||
| _configuredStep=_step; | _configuredStep=_step; | ||||
| _stepAdapted=_step; | _stepAdapted=_step; | ||||
| _oldDiffs[0]=0.; | _oldDiffs[0]=0.; | ||||
| _oldDiffs[1]=0.; | _oldDiffs[1]=0.; | ||||
| for (uint8_t i =0; i< MAX_NO_CHANNELS; i++){ | for (uint8_t i =0; i< MAX_NO_CHANNELS; i++){ | ||||
| memset(_buffer[i], 0, sizeof(float)*MAX_HALF_FILTER_LENGTH*2); | |||||
| memset(_buffer[i], 0, sizeof(float)*_maxHalfFilterLength*2); | |||||
| } | } | ||||
| float cutOffFrequ, kaiserBeta; | float cutOffFrequ, kaiserBeta; | ||||
| _attenuation=0; | _attenuation=0; | ||||
| cutOffFrequ=1.; | cutOffFrequ=1.; | ||||
| kaiserBeta=10; | kaiserBeta=10; | ||||
| _halfFilterLength=min(minHalfFilterLength,MAX_HALF_FILTER_LENGTH); | |||||
| _halfFilterLength=min(_minHalfFilterLength,_maxHalfFilterLength); | |||||
| } | } | ||||
| else{ | else{ | ||||
| cutOffFrequ=newFs/fs; | cutOffFrequ=newFs/fs; | ||||
| Serial.print("b: "); | Serial.print("b: "); | ||||
| Serial.println(b); | Serial.println(b); | ||||
| #endif | #endif | ||||
| double hfl=(int32_t)((attenuation-8)/(2.*2.285*TWO_PI*b)+0.5); | |||||
| if (hfl >= minHalfFilterLength && hfl <= MAX_HALF_FILTER_LENGTH){ | |||||
| double hfl=(int32_t)((_attenuation-8)/(2.*2.285*TWO_PI*b)+0.5); | |||||
| if (hfl >= _minHalfFilterLength && hfl <= _maxHalfFilterLength){ | |||||
| _halfFilterLength=hfl; | _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 if (hfl < _minHalfFilterLength){ | |||||
| _halfFilterLength=_minHalfFilterLength; | |||||
| _attenuation=((2*_halfFilterLength+1)-1)*(2.285*TWO_PI*b)+8; | |||||
| } | } | ||||
| else{ | 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 | |||||
| _halfFilterLength=_maxHalfFilterLength; | |||||
| _attenuation=((2*_halfFilterLength+1)-1)*(2.285*TWO_PI*b)+8; | |||||
| } | } | ||||
| #ifdef DEBUG_RESAMPLER | |||||
| Serial.print(attenuation); | |||||
| Serial.println("dB"); | |||||
| #endif | |||||
| if (attenuation>50.){ | |||||
| kaiserBeta=0.1102*(attenuation-8.7); | |||||
| 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 if (21<=_attenuation && _attenuation<=50){ | |||||
| kaiserBeta=0.5842*(float)pow(_attenuation-21.,0.4)+0.07886*(_attenuation-21.); | |||||
| } | } | ||||
| else{ | else{ | ||||
| kaiserBeta=0.; | kaiserBeta=0.; | ||||
| int32_t f = (noSamples-1)/(MAX_FILTER_SAMPLES-1)+1; | int32_t f = (noSamples-1)/(MAX_FILTER_SAMPLES-1)+1; | ||||
| _overSamplingFactor/=f; | _overSamplingFactor/=f; | ||||
| } | } | ||||
| _attenuation=attenuation; | |||||
| } | } | ||||
| #ifdef DEBUG_RESAMPLER | #ifdef DEBUG_RESAMPLER |
| double ki=0.00012; | double ki=0.00012; | ||||
| double kd= 1.8; | double kd= 1.8; | ||||
| }; | }; | ||||
| Resampler(StepAdaptionParameters settings=StepAdaptionParameters()); | |||||
| Resampler(float attenuation=100, int32_t minHalfFilterLength=20, int32_t maxHalfFilterLength=80, StepAdaptionParameters settings=StepAdaptionParameters()); | |||||
| void reset(); | 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 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 | ///@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); | |||||
| void configure(float fs, float newFs); | |||||
| ///@param input0 first input array/ channel | ///@param input0 first input array/ channel | ||||
| ///@param input1 second input array/ channel | ///@param input1 second input array/ channel | ||||
| ///@param inputLength length of each input array | ///@param inputLength length of each input array | ||||
| float _buffer[MAX_NO_CHANNELS][MAX_HALF_FILTER_LENGTH*2]; | float _buffer[MAX_NO_CHANNELS][MAX_HALF_FILTER_LENGTH*2]; | ||||
| float* _endOfBuffer[MAX_NO_CHANNELS]; | float* _endOfBuffer[MAX_NO_CHANNELS]; | ||||
| int32_t _minHalfFilterLength; | |||||
| int32_t _maxHalfFilterLength; | |||||
| int32_t _overSamplingFactor; | int32_t _overSamplingFactor; | ||||
| int32_t _halfFilterLength; | int32_t _halfFilterLength; | ||||
| int32_t _filterLength; | int32_t _filterLength; | ||||
| double _oldDiffs[2]; | double _oldDiffs[2]; | ||||
| double _attenuation=0; | double _attenuation=0; | ||||
| float _targetAttenuation=100; | |||||
| }; | }; | ||||
| #endif | #endif |
| } | } | ||||
| PROGMEM | PROGMEM | ||||
| AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength) : AudioStream(0, NULL) { | |||||
| _attenuation=attenuation; | |||||
| _minHalfFilterLength=minHalfFilterLength; | |||||
| AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength, int32_t maxHalfFilterLength): | |||||
| AudioStream(0, NULL), | |||||
| _resampler(attenuation, minHalfFilterLength, maxHalfFilterLength) | |||||
| { | |||||
| const float factor = powf(2, 15)-1.f; // to 16 bit audio | const float factor = powf(2, 15)-1.f; // to 16 bit audio | ||||
| quantizer[0]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT); | quantizer[0]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT); | ||||
| quantizer[0]->configure(noiseshaping, dither, factor); | quantizer[0]->configure(noiseshaping, dither, factor); | ||||
| __disable_irq(); | __disable_irq(); | ||||
| resample_offset = targetLatency <= buffer_offset ? buffer_offset - targetLatency : bufferLength -(targetLatency-buffer_offset); | resample_offset = targetLatency <= buffer_offset ? buffer_offset - targetLatency : bufferLength -(targetLatency-buffer_offset); | ||||
| __enable_irq(); | __enable_irq(); | ||||
| _resampler.configure(inputF, AUDIO_SAMPLE_RATE_EXACT, _attenuation, _minHalfFilterLength); | |||||
| _resampler.configure(inputF, AUDIO_SAMPLE_RATE_EXACT); | |||||
| #ifdef DEBUG_SPDIF_IN | #ifdef DEBUG_SPDIF_IN | ||||
| Serial.print("_maxLatency: "); | Serial.print("_maxLatency: "); | ||||
| Serial.println(_maxLatency); | Serial.println(_maxLatency); |
| class AsyncAudioInputSPDIF3 : public AudioStream | class AsyncAudioInputSPDIF3 : public AudioStream | ||||
| { | { | ||||
| public: | 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=true, bool noiseshaping=true,float attenuation=100, int32_t minHalfFilterLength=20); | |||||
| ///@param attenuation target attenuation [dB] of the anti-aliasing filter. Only used if AUDIO_SAMPLE_RATE_EXACT < input sample rate (input fs). The attenuation can't be reached if the needed filter length exceeds 2*MAX_FILTER_SAMPLES+1 | |||||
| ///@param minHalfFilterLength If AUDIO_SAMPLE_RATE_EXACT >= input fs), the filter length of the resampling filter is 2*minHalfFilterLength+1. If AUDIO_SAMPLE_RATE_EXACT < input fs the filter is maybe longer to reach the desired attenuation | |||||
| ///@param maxHalfFilterLength Can be used to restrict the maximum filter length at the cost of a lower attenuation | |||||
| AsyncAudioInputSPDIF3(bool dither=true, bool noiseshaping=true,float attenuation=100, int32_t minHalfFilterLength=20, int32_t maxHalfFilterLength=80); | |||||
| ~AsyncAudioInputSPDIF3(); | ~AsyncAudioInputSPDIF3(); | ||||
| virtual void update(void); | virtual void update(void); | ||||
| void begin(); | void begin(); | ||||
| static volatile uint32_t microsLast; | static volatile uint32_t microsLast; | ||||
| //==================== | //==================== | ||||
| float _attenuation; | |||||
| int32_t _minHalfFilterLength; | |||||
| Resampler _resampler; | Resampler _resampler; | ||||
| Quantizer* quantizer[2]; | Quantizer* quantizer[2]; | ||||
| arm_biquad_cascade_df2T_instance_f32 _bufferLPFilter; | arm_biquad_cascade_df2T_instance_f32 _bufferLPFilter; |
| #include <Audio.h> | #include <Audio.h> | ||||
| AsyncAudioInputSPDIF3 spdifIn(false, false, 100, 20); //dither = false, noiseshaping = false, anti-aliasing attenuation=100dB, minimum resampling filter length=20 | |||||
| AsyncAudioInputSPDIF3 spdifIn(false, false, 100, 20, 80); //dither = true, noiseshaping = true, anti-aliasing attenuation=100dB, minimum half resampling filter length=20, maximum half resampling filter length=80 | |||||
| AudioOutputSPDIF3 spdifOut; | AudioOutputSPDIF3 spdifOut; | ||||
| AudioConnection patchCord1(spdifIn, 0, spdifOut, 0); | AudioConnection patchCord1(spdifIn, 0, spdifOut, 0); | ||||
| Serial.print("resampling goup delay [milli seconds]: "); | Serial.print("resampling goup delay [milli seconds]: "); | ||||
| Serial.println(spdifIn.getHalfFilterLength()/inputFrequency*1e3,2); | Serial.println(spdifIn.getHalfFilterLength()/inputFrequency*1e3,2); | ||||
| Serial.print("half filter length: "); | |||||
| Serial.println(spdifIn.getHalfFilterLength()); | |||||
| double pUsageIn=spdifIn.processorUsage(); | double pUsageIn=spdifIn.processorUsage(); | ||||
| Serial.print("processor usage [%]: "); | Serial.print("processor usage [%]: "); |