visus
/
teensy-audio
Sledovat 1
Oblíbit 0
Rozštěpit 0
Zdrojový kód Úkoly 0 Požadavky na natažení 0 Vydání 0 Wiki Aktivita
894 Revize
2 Větve
Strom: 4d2ffa31d4
teensy-audio/Resampler.h

223 lines
9.8KB
Surový Blame Historie 

  1. /* Audio Library for Teensy 3.X

  2.  * Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com

  3.  *

  4.  * Development of this audio library was funded by PJRC.COM, LLC by sales of

  5.  * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop

  6.  * open source software by purchasing Teensy or other PJRC products.

  7.  *

  8.  * Permission is hereby granted, free of charge, to any person obtaining a copy

  9.  * of this software and associated documentation files (the "Software"), to deal

  10.  * in the Software without restriction, including without limitation the rights

  11.  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

  12.  * copies of the Software, and to permit persons to whom the Software is

  13.  * furnished to do so, subject to the following conditions:

  14.  *

  15.  * The above copyright notice, development funding notice, and this permission

  16.  * notice shall be included in all copies or substantial portions of the Software.

  17.  *

  18.  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

  19.  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

  20.  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

  21.  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

  22.  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

  23.  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

  24.  * THE SOFTWARE.

  25.  */

  26. /*

  27.  by Alexander Walch

  28.  */

  29. 

  30. #ifndef resampler_h_

  31. #define resampler_h_

  32. 

  33. 

  34. #include "Arduino.h"

  35. //#define DEBUG_RESAMPLER  //activates debug output

  36. 

  37. #define MAX_FILTER_SAMPLES 40961 //=1024*20 +1

  38. #define NO_EXACT_KAISER_SAMPLES 1025

  39. #define MAX_HALF_FILTER_LENGTH 80

  40. #define MAX_NO_CHANNELS 8

  41. class Resampler {

  42.     public:

  43. 

  44.         struct StepAdaptionParameters {

  45.             StepAdaptionParameters(){}

  46.             double alpha =0.2;  //exponential smoothing parameter

  47.             double maxAdaption = 0.01; //maximum relative allowed adaption of resampler step 0.01 = 1%

  48.             double kp= 0.6;

  49.             double ki=0.00012;

  50.             double kd= 1.8;

  51.         };

  52.         Resampler(StepAdaptionParameters settings=StepAdaptionParameters());

  53.         void reset();

  54.         ///@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

  55.         ///@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

  56.         void configure(float fs, float newFs, float attenuation=100, int32_t minHalfFilterLength=20);

  57.         ///@param input0 first input array/ channel

  58.         ///@param input1 second input array/ channel

  59.         ///@param inputLength length of each input array

  60.         ///@param processedLength number of samples of the input that were resampled to fill the output array

  61.         ///@param output0 first output array/ channel

  62.         ///@param output1 second output array/ channel

  63.         ///@param outputLength length of each output array

  64.         ///@param outputCount number of samples of each output array, that were filled with data

  65.         void resample(float* input0, float* input1, uint16_t inputLength, uint16_t& processedLength, float* output0, float* output1,uint16_t outputLength, uint16_t& outputCount);

  66.         bool addToSampleDiff(double diff);

  67.         double getXPos() const;

  68.         double getStep() const;

  69.         void addToPos(double val);

  70.         void fixStep();

  71.         bool initialized() const;

  72.         

  73.         //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

  74.         template <uint8_t NOCHANNELS>

  75.         inline void resample(float** inputs, uint16_t inputLength, uint16_t& processedLength, float** outputs, uint16_t outputLength, uint16_t& outputCount){

  76.             outputCount=0;

  77.             int32_t successorIndex=(int32_t)(ceil(_cPos));  //negative number -> currently the _buffer0 of the last iteration is used

  78.             float* ip[NOCHANNELS];

  79.             float* fPtr;

  80.         

  81.             float si0[NOCHANNELS];

  82.             float* si0Ptr;

  83.             float si1[NOCHANNELS];

  84.             float* si1Ptr;

  85.             while (floor(_cPos + _halfFilterLength) < inputLength && outputCount < outputLength){

  86.                 float dist=successorIndex-_cPos;

  87.                     

  88.                 float distScaled=dist*_overSamplingFactor;

  89.                 int32_t rightIndex=abs((int32_t)(ceil(distScaled))-_overSamplingFactor*_halfFilterLength);   

  90.                 const int32_t indexData=successorIndex-_halfFilterLength;

  91.                 if (indexData>=0){

  92.                     for (uint8_t i =0; i< NOCHANNELS; i++){

  93.                         ip[i]=inputs[i]+indexData;

  94.                     }

  95.                 }  

  96.                 else {            

  97.                     for (uint8_t i =0; i< NOCHANNELS; i++){

  98.                         ip[i]=_buffer[i]+indexData+_filterLength;

  99.                     }

  100.                 }       

  101.                 fPtr=filter+rightIndex;

  102.                 memset(si0, 0, NOCHANNELS*sizeof(float));

  103.                 if (rightIndex==_overSamplingFactor*_halfFilterLength){

  104.                     si1Ptr=si1;

  105.                     for (uint8_t i=0; i< NOCHANNELS; i++){

  106.                         *(si1Ptr++)=*ip[i]++**fPtr;

  107.                     }

  108.                     fPtr-=_overSamplingFactor;          

  109.                     rightIndex=(int32_t)(ceil(distScaled))+_overSamplingFactor;     //needed below  

  110.                 }

  111.                 else {

  112.                     memset(si1, 0, NOCHANNELS*sizeof(float));

  113.                     rightIndex=(int32_t)(ceil(distScaled));     //needed below

  114.                 }

  115.                 for (uint16_t i =0 ; i<_halfFilterLength; i++){

  116.                     if(ip[0]==_endOfBuffer[0]){

  117.                         for (uint8_t i =0; i< NOCHANNELS; i++){

  118.                             ip[i]=inputs[i];

  119.                         }

  120.                     }

  121.                     const float fPtrSucc=*(fPtr+1);

  122.                     si0Ptr=si0;

  123.                     si1Ptr=si1;

  124.                     for (uint8_t i =0; i< NOCHANNELS; i++){

  125.                         *(si0Ptr++)+=*ip[i]*fPtrSucc; 

  126.                         *(si1Ptr++)+=*ip[i]**fPtr; 

  127.                         ++ip[i];

  128.                     }       

  129.                     fPtr-=_overSamplingFactor; 

  130.                 }

  131.                 fPtr=filter+rightIndex-1;

  132.                 for (uint16_t i =0 ; i<_halfFilterLength; i++){  

  133.                     if(ip[0]==_endOfBuffer[0]){

  134.                         for (uint8_t i =0; i< NOCHANNELS; i++){

  135.                             ip[i]=inputs[i];

  136.                         }

  137.                     }

  138.                     const float fPtrSucc=*(fPtr+1);

  139.                     si0Ptr=si0;

  140.                     si1Ptr=si1;

  141.                     for (uint8_t i =0; i< NOCHANNELS; i++){

  142.                         *(si0Ptr++)+=*ip[i]**fPtr; 

  143.                         *(si1Ptr++)+=*ip[i]*fPtrSucc;  

  144.                         ++ip[i];

  145.                     }

  146.                     fPtr+=_overSamplingFactor;

  147.                 }

  148.                 const float w0=ceil(distScaled)-distScaled;

  149.                 const float w1=1.-w0;

  150.                 si0Ptr=si0;

  151.                 si1Ptr=si1;

  152.                 for (uint8_t i =0; i< NOCHANNELS; i++){

  153.                     *outputs[i]++=*(si0Ptr++)*w0 + *(si1Ptr++)*w1;

  154.                 }

  155.                 outputCount++;

  156.                 _cPos+=_stepAdapted;

  157.                 while (_cPos >successorIndex){

  158.                     successorIndex++;

  159.                 }

  160.             }

  161.             if(outputCount < outputLength){

  162.                 //ouput vector not full -> we ran out of input samples

  163.                 processedLength=inputLength;

  164.             }

  165.             else{

  166.                 processedLength=min(inputLength, (int16_t)floor(_cPos + _halfFilterLength));

  167.             }

  168.             //fill _buffer

  169.             const int32_t indexData=processedLength-_filterLength;

  170.             if (indexData>=0){

  171.                 const unsigned long long bytesToCopy= _filterLength*sizeof(float);

  172.                 float** inPtr=inputs;

  173.                 for (uint8_t i =0; i< NOCHANNELS; i++){

  174.                     memcpy((void *)_buffer[i], (void *)((*inPtr)+indexData), bytesToCopy);

  175.                     ++inPtr;

  176.                 }   

  177.             }  

  178.             else {

  179.                 float** inPtr=inputs;

  180.                 for (uint8_t i =0; i< NOCHANNELS; i++){

  181.                     float* b=_buffer[i];

  182.                     float* ip=b+indexData+_filterLength; 

  183.                     for (uint16_t j =0; j< _filterLength; j++){

  184.                         if(ip==_endOfBuffer[i]){

  185.                             ip=*inPtr;

  186.                         }        

  187.                         *b++ = *ip++;

  188.                     } 

  189.                     ++inPtr;

  190.                 }

  191.             }

  192.             _cPos-=processedLength;

  193.             if (_cPos < -_halfFilterLength){

  194.                 _cPos=-_halfFilterLength;

  195.             }

  196.         }

  197.     private:

  198.         void getKaiserExact(float beta);

  199.         void setKaiserWindow(float beta, int32_t noSamples);

  200.         void setFilter(int32_t halfFiltLength,int32_t overSampling, float cutOffFrequ, float kaiserBeta);

  201.         float filter[MAX_FILTER_SAMPLES];

  202.         double kaiserWindowSamples[NO_EXACT_KAISER_SAMPLES];

  203.         double tempRes[NO_EXACT_KAISER_SAMPLES-1];

  204.         double kaiserWindowXsq[NO_EXACT_KAISER_SAMPLES-1];

  205.         float _buffer[MAX_NO_CHANNELS][MAX_HALF_FILTER_LENGTH*2];

  206.         float* _endOfBuffer[MAX_NO_CHANNELS];

  207. 

  208.         int32_t _overSamplingFactor;

  209.         int32_t _halfFilterLength;

  210.         int32_t _filterLength;     

  211.         bool _initialized=false;  

  212.         

  213.         const double _settledThrs = 1e-6;

  214.         StepAdaptionParameters _settings;

  215.         double _configuredStep;

  216.         double _step;

  217.         double _stepAdapted;

  218.         double _cPos;

  219.         double _sum;

  220.         double _oldDiffs[2]; 

  221. };

  222. 

  223. #endif