diff --git a/src/classic/btstack_cvsd_plc.c b/src/classic/btstack_cvsd_plc.c index 8c52ad36b..c48ba30d0 100644 --- a/src/classic/btstack_cvsd_plc.c +++ b/src/classic/btstack_cvsd_plc.c @@ -49,11 +49,13 @@ #include "btstack_cvsd_plc.h" #include "btstack_debug.h" +#define SAMPLE_FORMAT int8_t + static float rcos[CVSD_OLAL] = { - 0.99148655,0.96623611,0.92510857,0.86950446, - 0.80131732,0.72286918,0.63683150,0.54613418, - 0.45386582,0.36316850,0.27713082,0.19868268, - 0.13049554,0.07489143,0.03376389,0.00851345}; + 0.99148655f,0.96623611f,0.92510857f,0.86950446f, + 0.80131732f,0.72286918f,0.63683150f,0.54613418f, + 0.45386582f,0.36316850f,0.27713082f,0.19868268f, + 0.13049554f,0.07489143f,0.03376389f,0.00851345f}; // taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi // Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation @@ -79,7 +81,7 @@ static float absolute(float x){ return x; } -static float CrossCorrelation(int8_t *x, int8_t *y){ +static float CrossCorrelation(SAMPLE_FORMAT *x, SAMPLE_FORMAT *y){ float num = 0; float den = 0; float x2 = 0; @@ -94,7 +96,7 @@ static float CrossCorrelation(int8_t *x, int8_t *y){ return num/den; } -static int PatternMatch(int8_t *y){ +static int PatternMatch(SAMPLE_FORMAT *y){ float maxCn = -999999.0; // large negative number int bestmatch = 0; float Cn; @@ -109,7 +111,7 @@ static int PatternMatch(int8_t *y){ return bestmatch; } -static float AmplitudeMatch(int8_t *y, int8_t bestmatch) { +static float AmplitudeMatch(SAMPLE_FORMAT *y, SAMPLE_FORMAT bestmatch) { int i; float sumx = 0; float sumy = 0.000001f; @@ -126,11 +128,11 @@ static float AmplitudeMatch(int8_t *y, int8_t bestmatch) { return sf; } -static int8_t crop_to_int8(float val){ +static SAMPLE_FORMAT crop_sample(float val){ float croped_val = val; if (croped_val > 127.0) croped_val= 127.0; if (croped_val < -128.0) croped_val=-128.0; - return (int8_t) croped_val; + return (SAMPLE_FORMAT) croped_val; } @@ -138,7 +140,7 @@ void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){ memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t)); } -void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *out){ +void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, SAMPLE_FORMAT *out){ float val; int i = 0; float sf = 1; @@ -154,19 +156,19 @@ void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *out sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); for (i=0;ihist[plc_state->bestlag+i]; - plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); + plc_state->hist[CVSD_LHIST+i] = crop_sample(val); } for (;ihist[plc_state->bestlag+i]; - plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); + plc_state->hist[CVSD_LHIST+i] = crop_sample(val); } for (;ihist[plc_state->bestlag+i]; float right = plc_state->hist[plc_state->bestlag+i]; val = left*rcos[i-CVSD_FS] + right*rcos[CVSD_OLAL-1-i+CVSD_FS]; - plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); + plc_state->hist[CVSD_LHIST+i] = crop_sample(val); } for (;inbf>0){ @@ -199,7 +201,7 @@ void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *in float left = plc_state->hist[CVSD_LHIST+i]; float right = in[i]; val = left * rcos[i-CVSD_RT] + right *rcos[CVSD_OLAL+CVSD_RT-1-i]; - out[i] = (int8_t)val; + out[i] = (SAMPLE_FORMAT)val; } } @@ -217,7 +219,7 @@ void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *in plc_state->nbf=0; } -static int count_equal_bytes(int8_t * packet, uint16_t size){ +static int count_equal_bytes(SAMPLE_FORMAT * packet, uint16_t size){ int count = 0; int temp_count = 1; int i; @@ -237,11 +239,11 @@ static int count_equal_bytes(int8_t * packet, uint16_t size){ return count; } -static int bad_frame(int8_t * frame, uint16_t size){ +static int bad_frame(SAMPLE_FORMAT * frame, uint16_t size){ return count_equal_bytes(frame, size) > 20; } -void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * state, int8_t * in, uint16_t size, int8_t * out){ +void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * state, SAMPLE_FORMAT * in, uint16_t size, SAMPLE_FORMAT * out){ if (size != 24){ log_error("btstack_cvsd_plc_process_data: audio frame size is incorrect. Expected %d, got %d", CVSD_FS, size); } @@ -263,7 +265,7 @@ void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * state, int8_t * in } } -void btstack_cvsd_plc_mark_bad_frame(btstack_cvsd_plc_state_t * state, int8_t * in, uint16_t size, int8_t * out){ +void btstack_cvsd_plc_mark_bad_frame(btstack_cvsd_plc_state_t * state, SAMPLE_FORMAT * in, uint16_t size, SAMPLE_FORMAT * out){ if (size != 24){ log_error("btstack_cvsd_plc_mark_bad_frame: audio frame size is incorrect. Expected %d, got %d", CVSD_FS, size); } diff --git a/src/classic/btstack_sbc_plc.c b/src/classic/btstack_sbc_plc.c index 07b77fcb0..b6f104c7f 100644 --- a/src/classic/btstack_sbc_plc.c +++ b/src/classic/btstack_sbc_plc.c @@ -47,6 +47,8 @@ #include "btstack_sbc_plc.h" +#define SAMPLE_FORMAT int16_t + static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, @@ -84,7 +86,7 @@ static float absolute(float x){ return x; } -static float CrossCorrelation(int16_t *x, int16_t *y){ +static float CrossCorrelation(SAMPLE_FORMAT *x, SAMPLE_FORMAT *y){ float num = 0; float den = 0; float x2 = 0; @@ -99,13 +101,13 @@ static float CrossCorrelation(int16_t *x, int16_t *y){ return num/den; } -static int PatternMatch(int16_t *y){ - float maxCn = -999999.0; /* large negative number */ +static int PatternMatch(SAMPLE_FORMAT *y){ + float maxCn = -999999.0; // large negative number int bestmatch = 0; float Cn; int n; for (n=0;nmaxCn){ bestmatch=n; maxCn = Cn; @@ -114,8 +116,7 @@ static int PatternMatch(int16_t *y){ return bestmatch; } - -static float AmplitudeMatch(int16_t *y, int16_t bestmatch) { +static float AmplitudeMatch(SAMPLE_FORMAT *y, SAMPLE_FORMAT bestmatch) { int i; float sumx = 0; float sumy = 0.000001f; @@ -126,17 +127,17 @@ static float AmplitudeMatch(int16_t *y, int16_t bestmatch) { sumy += absolute(y[bestmatch+i]); } sf = sumx/sumy; - /* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */ + // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts if (sf<0.75f) sf=0.75f; if (sf>1.2f) sf=1.2f; return sf; } -static int16_t crop_to_int16(float val){ +static SAMPLE_FORMAT crop_sample(float val){ float croped_val = val; if (croped_val > 32767.0) croped_val= 32767.0; if (croped_val < -32768.0) croped_val=-32768.0; - return (int16_t) croped_val; + return (SAMPLE_FORMAT) croped_val; } uint8_t * btstack_sbc_plc_zero_signal_frame(void){ @@ -149,46 +150,44 @@ void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){ memset(plc_state->hist,0,sizeof(plc_state->hist)); } -void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){ +void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *ZIRbuf, SAMPLE_FORMAT *out){ float val; int i = 0; float sf = 1; - plc_state->nbf++; if (plc_state->nbf==1){ - /* Perform pattern matching to find where to replicate */ + // Perform pattern matching to find where to replicate plc_state->bestlag = PatternMatch(plc_state->hist); - /* the replication begins after the template match */ + // the replication begins after the template match plc_state->bestlag += SBC_M; - /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */ + // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); for (i=0;ihist[plc_state->bestlag+i]; val = left*rcos[i] + right*rcos[SBC_OLAL-1-i]; - plc_state->hist[SBC_LHIST+i] = crop_to_int16(val); + plc_state->hist[SBC_LHIST+i] = crop_sample(val); } for (;ihist[plc_state->bestlag+i]; - plc_state->hist[SBC_LHIST+i] = crop_to_int16(val); + plc_state->hist[SBC_LHIST+i] = crop_sample(val); } for (;ihist[plc_state->bestlag+i]; float right = plc_state->hist[plc_state->bestlag+i]; - val = left*rcos[i-SBC_FS]+right*rcos[SBC_FS+SBC_OLAL-1-i]; - plc_state->hist[SBC_LHIST+i] = crop_to_int16(val); + val = left*rcos[i-SBC_FS]+right*rcos[SBC_OLAL-1-i+SBC_FS]; + plc_state->hist[SBC_LHIST+i] = crop_sample(val); } for (;ihist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; } - } else { - for (i=0;ihist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; } } @@ -196,14 +195,13 @@ void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, int16_t *ZIRb out[i] = plc_state->hist[SBC_LHIST+i]; } - /* shift the history buffer */ + // shift the history buffer for (i=0;ihist[i] = plc_state->hist[i+SBC_FS]; } - } -void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){ +void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *in, SAMPLE_FORMAT *out){ float val; int i = 0; if (plc_state->nbf>0){ @@ -211,22 +209,22 @@ void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, int16_t *in, out[i] = plc_state->hist[SBC_LHIST+i]; } - for (;ihist[SBC_LHIST+i]; float right = in[i]; - val = left*rcos[i-SBC_RT] + right*rcos[SBC_OLAL-1-i+SBC_RT]; - out[i] = (int16_t)val; + val = left*rcos[i-SBC_RT] + right*rcos[SBC_OLAL+SBC_RT-1-i]; + out[i] = (SAMPLE_FORMAT)val; } } + for (;ihist[SBC_LHIST+i] = out[i]; } - /* shift the history buffer */ + // shift the history buffer for (i=0;ihist[i] = plc_state->hist[i+SBC_FS]; }