visus
/
framework-arduinoteensy-cxx20
Tarkkaile 1
Äänestä 0
Fork 0
Koodi Ongelmat 0 Pull-pyynnöt 0 Julkaisut 0 Wiki Activity
PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
5 Commitit
1 Haara
Puu: a7a66c2f14
framework-arduinoteensy-cxx20/libraries/Audio/Quantizer.cpp

232 lines
7.7KB
Raaka Blame Historia 

  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. #include "Quantizer.h"

  30. 

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

  32. 

  33. Quantizer::Quantizer(float audio_sample_rate){

  34. #ifdef DEBUG_QUANTIZER

  35.     while(!Serial);

  36. #endif

  37.     randomSeed(1);

  38.     if(audio_sample_rate==44100.f){

  39.         _noiseSFilter[0]=-0.06935825f;

  40.         _noiseSFilter[1]=0.52540845f;

  41.         _noiseSFilter[2]= -1.20537028f;

  42.         _noiseSFilter[3]=  2.09422811f;

  43.         _noiseSFilter[4]= -3.2177438f;

  44.         _noiseSFilter[5]= 4.04852027f;

  45.         _noiseSFilter[6]= -3.83872701f;

  46.         _noiseSFilter[7]= 3.30584589f;

  47.         _noiseSFilter[8]= -2.38682527f;

  48.     //  all coefficients in correct order:

  49.     //      {1.        , -2.38682527,  3.30584589, -3.83872701,  4.04852027,

  50.     //       -3.2177438 ,  2.09422811, -1.20537028,  0.52540845, -0.06935825};

  51.     } else if(audio_sample_rate==48000.f){

  52.         _noiseSFilter[0]=0.1967454f;

  53.         _noiseSFilter[1]=-0.30086406f;

  54.         _noiseSFilter[2]= 0.09575588f;

  55.         _noiseSFilter[3]=  0.58209648f;

  56.         _noiseSFilter[4]= -1.88579617f;

  57.         _noiseSFilter[5]= 3.37325788f;

  58.         _noiseSFilter[6]= -3.88076402f;

  59.         _noiseSFilter[7]= 3.58558504f;

  60.         _noiseSFilter[8]= -2.54334066f;

  61.         //  all coefficients in correct order:

  62.     //      {1.        , -2.54334066,  3.58558504, -3.88076402,  3.37325788,

  63.     //      -1.88579617,  0.58209648,  0.09575588, -0.30086406,  0.1967454};

  64.     //    }

  65.     }

  66. 	else {

  67. 		_noiseSFilter[0]=0.f;

  68.         _noiseSFilter[1]=0.f;

  69.         _noiseSFilter[2]=0.f;

  70.         _noiseSFilter[3]=0.f;

  71.         _noiseSFilter[4]=0.f;

  72.         _noiseSFilter[5]=0.f;

  73.         _noiseSFilter[6]=0.f;

  74.         _noiseSFilter[7]=0.f;

  75.         _noiseSFilter[8]=0.f;

  76. 	}

  77.     _bufferEnd0=&_buffer0[NOISE_SHAPE_F_LENGTH];

  78.     reset();

  79. }

  80. 

  81. void Quantizer::configure(bool noiseShaping, bool dither, float factor){

  82.      _noiseShaping=noiseShaping;

  83.      _dither=dither;

  84.      _factor=factor;

  85.      reset();

  86. }

  87. void Quantizer::reset(){

  88.      _bPtr0=_buffer0;

  89.      _bPtr1=_buffer1;

  90.      _fOutputLastIt0=0.;

  91.      _fOutputLastIt1=0.;

  92.      memset(_buffer0, 0, NOISE_SHAPE_F_LENGTH*sizeof(float));

  93.      memset(_buffer1, 0, NOISE_SHAPE_F_LENGTH*sizeof(float));

  94. }

  95. void Quantizer::quantize(float* input, int16_t* output, uint16_t length){

  96.     float xn, xnD, error;

  97.     const float f2 = 1.f/1000000.f;

  98. #ifdef DEBUG_QUANTIZER

  99.     float debugFF=1024.f;

  100.     const float factor=(powf(2.f, 15.f)-1.f)/debugFF;

  101. #endif

  102. 

  103.     for (uint16_t i =0; i< length; i++){

  104.         xn= SAMPLEINVALID(*input) ? 0. : *input*_factor; //-_fOutputLastIt0 according to paper     

  105.         ++input;

  106.         if (_noiseShaping){

  107.             xn+=_fOutputLastIt0;

  108.         }

  109.         if(_dither){

  110.             const uint32_t r0=random(1000000);

  111.             const uint32_t r1=random(1000000);

  112.             xnD=xn + (r0 + r1)*f2-1.f;

  113.         }

  114.         else {

  115.             xnD=xn;

  116.         }

  117.         float xnDR=round(xnD);

  118.         if (_noiseShaping){

  119.             //compute quatization error:

  120.             error=xnDR- xn;

  121.             *_bPtr0++=error;

  122.             if (_bPtr0==_bufferEnd0){

  123.                 _bPtr0=_buffer0;

  124.             }

  125.             float* f=_noiseSFilter;

  126.             _fOutputLastIt0=(*_bPtr0++ * *f++);

  127.             if (_bPtr0==_bufferEnd0){

  128.                 _bPtr0=_buffer0;

  129.             }

  130.             for (uint16_t j =1; j< NOISE_SHAPE_F_LENGTH; j++){

  131.                 _fOutputLastIt0+=(*_bPtr0++ * *f++);

  132.                 if (_bPtr0==_bufferEnd0){

  133.                     _bPtr0=_buffer0;

  134.                 }

  135.             }

  136.         }

  137. #ifdef DEBUG_QUANTIZER

  138.         xnDR*=debugFF;

  139. #endif

  140.         if (xnDR > _factor){

  141.             *output=(int16_t)_factor;

  142.         }

  143.         else if (xnDR < -_factor){

  144.             *output=(int16_t)_factor;

  145.         }

  146.         else {

  147.             *output=(int16_t)xnDR;

  148.         }

  149. 

  150.         ++output;

  151.     }

  152. }

  153. 

  154. void Quantizer::quantize(float* input0, float* input1, int32_t* outputInterleaved, uint16_t length){

  155.     float xn0, xnD0, error0,xnDR0, xn1, xnD1, error1,xnDR1;

  156.     const float f2 = 1.f/1000000.f;

  157. #ifdef DEBUG_QUANTIZER

  158.     float debugFF=1024.f;

  159.     const float factor=(powf(2.f, 15.f)-1.f)/debugFF;

  160. #endif

  161.     for (uint16_t i =0; i< length; i++){

  162.         xn0= SAMPLEINVALID(*input0) ? 0. : *input0*_factor; //-_fOutputLastIt0 according to paper        

  163.         ++input0;

  164.         xn1= SAMPLEINVALID(*input1) ? 0. : *input1*_factor; //-_fOutputLastIt0 according to paper  

  165.         ++input1;

  166.         if (_noiseShaping){

  167.             xn0+=_fOutputLastIt0;

  168.             xn1+=_fOutputLastIt1;

  169.         }

  170.         if(_dither){

  171.             uint32_t r0=random(1000000);

  172.             uint32_t r1=random(1000000);

  173.             xnD0=xn0 + (r0 + r1)*f2-1.f;

  174.             r0=random(1000000);

  175.             r1=random(1000000);

  176.             xnD1=xn1 + (r0 + r1)*f2-1.f;

  177.         }

  178.         else {

  179.             xnD0=xn0;

  180.             xnD1=xn1;

  181.         }

  182.         xnDR0=round(xnD0);

  183.         xnDR1=round(xnD1);

  184.         if (_noiseShaping){

  185.             //compute quatization error0:

  186.             error0=xnDR0- xn0;

  187.             error1=xnDR1- xn1;

  188.             *_bPtr0++=error0;

  189.             *_bPtr1++=error1;

  190.             if (_bPtr0==_bufferEnd0){

  191.                 _bPtr0=_buffer0;

  192.                 _bPtr1=_buffer1;

  193.             }

  194.             float* f=_noiseSFilter;

  195.             _fOutputLastIt0=(*_bPtr0++ * *f);

  196.             _fOutputLastIt1=(*_bPtr1++ * *f++);

  197.             if (_bPtr0==_bufferEnd0){

  198.                 _bPtr0=_buffer0;

  199.                 _bPtr1=_buffer1;

  200.             }

  201.             for (uint16_t j =1 ; j< NOISE_SHAPE_F_LENGTH; j++){

  202.                 _fOutputLastIt0+=(*_bPtr0++ * *f);

  203.                 _fOutputLastIt1+=(*_bPtr1++ * *f++);

  204.                 if (_bPtr0==_bufferEnd0){

  205.                     _bPtr0=_buffer0;

  206.                     _bPtr1=_buffer1;

  207.                 }

  208.             }

  209.         }

  210. #ifdef DEBUG_QUANTIZER

  211.         xnDR0*=debugFF;

  212. #endif

  213.         if (xnDR0 > _factor){

  214.             *outputInterleaved++=(int32_t)_factor;

  215.         }

  216.         else if (xnDR0 < -_factor){

  217.             *outputInterleaved++=-(int32_t)_factor;

  218.         }

  219.         else {

  220.             *outputInterleaved++=(int32_t)xnDR0;

  221.         }

  222.         if (xnDR1 > _factor){

  223.             *outputInterleaved++=(int32_t)_factor;

  224.         }

  225.         else if (xnDR1 < -_factor){

  226.             *outputInterleaved++=-(int32_t)_factor;

  227.         }

  228.         else {

  229.             *outputInterleaved++=(int32_t)xnDR1;

  230.         }

  231.     }

  232. }