mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-01-26 12:35:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

hfp: streamline code in sbc and cvsd plc

Browse Source
This commit is contained in:
Matthias Ringwald 2017-01-23 11:57:07 +01:00
parent c0857b5e79
commit 4e4f6f264b
2 changed files with 48 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/classic/btstack_cvsd_plc.c
View File

@ -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;i<CVSD_OLAL;i++){
val = sf*plc_state->hist[plc_state->bestlag+i];
plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val);
plc_state->hist[CVSD_LHIST+i] = crop_sample(val);
}
for (;i<CVSD_FS;i++){
val = sf*plc_state->hist[plc_state->bestlag+i];
plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val);
plc_state->hist[CVSD_LHIST+i] = crop_sample(val);
}
for (;i<CVSD_FS+CVSD_OLAL;i++){
float left = sf*plc_state->hist[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 (;i<CVSD_FS+CVSD_RT+CVSD_OLAL;i++){
@ -187,7 +189,7 @@ void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *out
}
}
void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *in, int8_t *out){
void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, SAMPLE_FORMAT *in, SAMPLE_FORMAT *out){
float val;
int i = 0;
if (plc_state->nbf>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);
}

56
src/classic/btstack_sbc_plc.c
View File

@ -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;n<SBC_N;n++){
Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M] /* x */, &y[n]);
Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M], &y[n]);
if (Cn>maxCn){
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;i<SBC_OLAL;i++){
float left = ZIRbuf[i];
float right = sf*plc_state->hist[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 (;i<SBC_FS;i++){
val = sf*plc_state->hist[plc_state->bestlag+i];
plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
plc_state->hist[SBC_LHIST+i] = crop_sample(val);
}
for (;i<SBC_FS+SBC_OLAL;i++){
float left = sf*plc_state->hist[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 (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){
plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
}
} else {
for (i=0;i<SBC_FS+SBC_RT+SBC_OLAL;i++){
for (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){
plc_state->hist[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;i<SBC_LHIST+SBC_RT+SBC_OLAL;i++){
plc_state->hist[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 (;i<SBC_RT+SBC_OLAL;i++){
for (i = SBC_RT;i<SBC_RT+SBC_OLAL;i++){
float left = plc_state->hist[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 (;i<SBC_FS;i++){
out[i] = in[i];
}
/*Copy the output to the history buffer */
// Copy the output to the history buffer
for (i=0;i<SBC_FS;i++){
plc_state->hist[SBC_LHIST+i] = out[i];
}
/* shift the history buffer */
// shift the history buffer
for (i=0;i<SBC_LHIST;i++){
plc_state->hist[i] = plc_state->hist[i+SBC_FS];
}