visus
/
teensy-audio
Suivre 1
Ajouter aux favoris 0
Bifurcation 0
Code Tickets 0 Demandes d'ajout 0 Versions 0 Wiki Activité
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.
1032 Révisions
2 Branches
Aborescence: 5b33918be9
Branches Tags
${ item.name }
Créer la branche ${ searchTerm }
de '5b33918be9'
${ noResults }
teensy-audio/include/audio/Resampler.h

230 lines
10KB
Brut Annotations Historique 

  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 <core/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(float attenuation=100, int32_t minHalfFilterLength=20, int32_t maxHalfFilterLength=80, 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);

  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. 		double getAttenuation() const;

  73. 		int32_t getHalfFilterLength() const;

  74.         

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

  76.         template <uint8_t NOCHANNELS>

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

  78.             outputCount=0;

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

  80.             float* ip[NOCHANNELS];

  81.             float* fPtr;

  82.         

  83.             float si0[NOCHANNELS];

  84.             float* si0Ptr;

  85.             float si1[NOCHANNELS];

  86.             float* si1Ptr;

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

  88.                 float dist=successorIndex-_cPos;

  89.                     

  90.                 float distScaled=dist*_overSamplingFactor;

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

  92.                 const int32_t indexData=successorIndex-_halfFilterLength;

  93.                 if (indexData>=0){

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

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

  96.                     }

  97.                 }  

  98.                 else {            

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

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

  101.                     }

  102.                 }       

  103.                 fPtr=filter+rightIndex;

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

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

  106.                     si1Ptr=si1;

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

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

  109.                     }

  110.                     fPtr-=_overSamplingFactor;          

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

  112.                 }

  113.                 else {

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

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

  116.                 }

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

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

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

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

  121.                         }

  122.                     }

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

  124.                     si0Ptr=si0;

  125.                     si1Ptr=si1;

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

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

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

  129.                         ++ip[i];

  130.                     }       

  131.                     fPtr-=_overSamplingFactor; 

  132.                 }

  133.                 fPtr=filter+rightIndex-1;

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

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

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

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

  138.                         }

  139.                     }

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

  141.                     si0Ptr=si0;

  142.                     si1Ptr=si1;

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

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

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

  146.                         ++ip[i];

  147.                     }

  148.                     fPtr+=_overSamplingFactor;

  149.                 }

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

  151.                 const float w1=1.-w0;

  152.                 si0Ptr=si0;

  153.                 si1Ptr=si1;

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

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

  156.                 }

  157.                 outputCount++;

  158.                 _cPos+=_stepAdapted;

  159.                 while (_cPos >successorIndex){

  160.                     successorIndex++;

  161.                 }

  162.             }

  163.             if(outputCount < outputLength){

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

  165.                 processedLength=inputLength;

  166.             }

  167.             else{

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

  169.             }

  170.             //fill _buffer

  171.             const int32_t indexData=processedLength-_filterLength;

  172.             if (indexData>=0){

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

  174.                 float** inPtr=inputs;

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

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

  177.                     ++inPtr;

  178.                 }   

  179.             }  

  180.             else {

  181.                 float** inPtr=inputs;

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

  183.                     float* b=_buffer[i];

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

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

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

  187.                             ip=*inPtr;

  188.                         }        

  189.                         *b++ = *ip++;

  190.                     } 

  191.                     ++inPtr;

  192.                 }

  193.             }

  194.             _cPos-=processedLength;

  195.             if (_cPos < -_halfFilterLength){

  196.                 _cPos=-_halfFilterLength;

  197.             }

  198.         }

  199.     private:

  200.         void getKaiserExact(float beta);

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

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

  203.         float filter[MAX_FILTER_SAMPLES];

  204.         double kaiserWindowSamples[NO_EXACT_KAISER_SAMPLES];

  205.         double tempRes[NO_EXACT_KAISER_SAMPLES-1];

  206.         double kaiserWindowXsq[NO_EXACT_KAISER_SAMPLES-1];

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

  208.         float* _endOfBuffer[MAX_NO_CHANNELS];

  209. 

  210. 		int32_t _minHalfFilterLength;

  211. 		int32_t _maxHalfFilterLength;

  212.         int32_t _overSamplingFactor;

  213.         int32_t _halfFilterLength;

  214.         int32_t _filterLength;     

  215.         bool _initialized=false;  

  216.         

  217.         const double _settledThrs = 1e-6;

  218.         StepAdaptionParameters _settings;

  219.         double _configuredStep;

  220.         double _step;

  221.         double _stepAdapted;

  222.         double _cPos;

  223.         double _sum;

  224.         double _oldDiffs[2]; 

  225. 		

  226. 		double _attenuation=0;

  227. 		float _targetAttenuation=100;

  228. };

  229. 

  230. #endif