mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-28 08:37:22 +00:00
Code Issues Packages Projects Releases Wiki Activity

sbc plc: renamed vars

Browse Source
This commit is contained in:
Milanka Ringwald 2016-09-13 10:35:08 +02:00
parent a3b51b7f14
commit 7e6b1e83d2
5 changed files with 89 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
test/hfp/btstack_cvsd_plc.c → src/classic/btstack_cvsd_plc.c
View File

0
test/hfp/btstack_cvsd_plc.h → src/classic/btstack_cvsd_plc.h
View File

156
src/classic/btstack_sbc_plc.c
View File

@ -57,7 +57,7 @@ static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x
0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c}; 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};
/* Raised COSine table for OLA */ /* Raised COSine table for OLA */
static float rcos[OLAL] = { static float rcos[SBC_OLAL] = {
0.99148655f,0.96623611f,0.92510857f,0.86950446f, 0.99148655f,0.96623611f,0.92510857f,0.86950446f,
0.80131732f,0.72286918f,0.63683150f,0.54613418f, 0.80131732f,0.72286918f,0.63683150f,0.54613418f,
0.45386582f,0.36316850f,0.27713082f,0.19868268f, 0.45386582f,0.36316850f,0.27713082f,0.19868268f,
@ -67,146 +67,152 @@ static float CrossCorrelation(int16_t *x, int16_t *y);
static int PatternMatch(int16_t *y); static int PatternMatch(int16_t *y);
static float AmplitudeMatch(int16_t *y, int16_t bestmatch); static float AmplitudeMatch(int16_t *y, int16_t bestmatch);
static int16_t crop_to_int16(float val){
float croped_val = 0;
if (val > 32767.0) croped_val= 32767.0;
if (val < -32768.0) croped_val=-32768.0;
return (int16_t) croped_val;
}
uint8_t * btstack_sbc_plc_zero_signal_frame(void){ uint8_t * btstack_sbc_plc_zero_signal_frame(void){
return (uint8_t *)&indices0; return (uint8_t *)&indices0;
} }
void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){ void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){
int i;
plc_state->nbf=0; plc_state->nbf=0;
plc_state->bestlag=0; plc_state->bestlag=0;
// for (i=0;i<LHIST+FS+CVSDRT+OLAL;i++){ memset(plc_state->hist,0,sizeof(plc_state->hist));
// plc_state->hist[i] = 0;
// }
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, int16_t *ZIRbuf, int16_t *out){
int i;
float val; float val;
float sf; int i = 0;
float sf = 1;
plc_state->nbf++; plc_state->nbf++;
sf=1.0f;
i=0;
if (plc_state->nbf==1){ 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); plc_state->bestlag = PatternMatch(plc_state->hist);
/* the replication begins after the template match */ /* the replication begins after the template match */
plc_state->bestlag += M; 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); sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
for (i=0;i<OLAL;i++){ for (i=0;i<SBC_OLAL;i++){
val = ZIRbuf[i]*rcos[i] + sf*plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-i-1]; float left = ZIRbuf[i];
if (val > 32767.0) val= 32767.0; float right = sf*plc_state->hist[plc_state->bestlag+i];
if (val < -32768.0) val=-32768.0; val = left*rcos[i] + right*rcos[SBC_OLAL-i-1];
plc_state->hist[LHIST+i] = (int16_t)val; plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
} }
for (;i<FS;i++){ for (;i<SBC_FS;i++){
val = sf*plc_state->hist[plc_state->bestlag+i]; if (val > 32767.0) val= 32767.0; val = sf*plc_state->hist[plc_state->bestlag+i];
if (val < -32768.0) val=-32768.0; plc_state->hist[LHIST+i] = (int16_t)val; plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
} }
for (;i<FS+OLAL;i++){ for (;i<SBC_FS+SBC_OLAL;i++){
val = sf*plc_state->hist[plc_state->bestlag+i]*rcos[i-FS]+plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-1-i+FS]; float left = sf*plc_state->hist[plc_state->bestlag+i];
if (val > 32767.0) val= 32767.0; float right = plc_state->hist[plc_state->bestlag+i];
if (val < -32768.0) val=-32768.0; val = left*rcos[i-SBC_FS]+right*rcos[SBC_OLAL-1-i+SBC_FS];
plc_state->hist[LHIST+i] = (int16_t)val; plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
} }
for (;i<FS+SBCRT+OLAL;i++) for (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
}
} else { } else {
for (;i<FS;i++) for (i=0;i<SBC_FS+SBC_RT+SBC_OLAL;i++)
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
for (;i<FS+SBCRT+OLAL;i++) }
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; for (i=0;i<SBC_FS;i++){
out[i] = plc_state->hist[SBC_LHIST+i];
}
/* 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];
} }
for (i=0;i<FS;i++)
out[i] = plc_state->hist[LHIST+i];
/* shift the history buffer */
for (i=0;i<LHIST+SBCRT+OLAL;i++)
plc_state->hist[i] = plc_state->hist[i+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, int16_t *in, int16_t *out){
int i; float val;
i=0; int i = 0;
if (plc_state->nbf>0){ if (plc_state->nbf>0){
for (i=0;i<SBCRT;i++) for (i=0;i<SBC_RT;i++){
out[i] = plc_state->hist[LHIST+i]; out[i] = plc_state->hist[SBC_LHIST+i];
for (;i<SBCRT+OLAL;i++) }
out[i] = (int16_t)(plc_state->hist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]);
for (;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] = crop_to_int16(val);
}
} }
for (;i<FS;i++) for (;i<SBC_FS;i++){
out[i] = in[i]; out[i] = in[i];
}
/*Copy the output to the history buffer */ /*Copy the output to the history buffer */
for (i=0;i<FS;i++) for (i=0;i<SBC_FS;i++){
plc_state->hist[LHIST+i] = out[i]; plc_state->hist[SBC_LHIST+i] = out[i];
}
/* shift the history buffer */ /* shift the history buffer */
for (i=0;i<LHIST;i++) for (i=0;i<SBC_LHIST;i++){
plc_state->hist[i] = plc_state->hist[i+FS]; plc_state->hist[i] = plc_state->hist[i+SBC_FS];
}
plc_state->nbf=0; plc_state->nbf=0;
} }
float CrossCorrelation(int16_t *x, int16_t *y){ float CrossCorrelation(int16_t *x, int16_t *y){
float num = 0;
float den = 0;
float x2 = 0;
float y2 = 0;
int m; int m;
float num; for (m=0;m<SBC_M;m++){
float den;
float Cn;
float x2, y2;
num=0;
den=0;
x2=0.0;
y2=0.0;
for (m=0;m<M;m++){
num+=((float)x[m])*y[m]; num+=((float)x[m])*y[m];
x2+=((float)x[m])*x[m]; x2+=((float)x[m])*x[m];
y2+=((float)y[m])*y[m]; y2+=((float)y[m])*y[m];
} }
den = (float)sqrt(x2*y2); den = (float)sqrt(x2*y2);
Cn = num/den; return num/den;
return(Cn);
} }
int PatternMatch(int16_t *y){ int PatternMatch(int16_t *y){
int n; float maxCn = -999999.0; /* large negative number */
float maxCn; int bestmatch = 0;
float Cn; float Cn;
int bestmatch; int n;
maxCn=-999999.0; /* large negative number */ for (n=0;n<SBC_N;n++){
bestmatch=0; Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M] /* x */, &y[n]);
for (n=0;n<N;n++){
Cn = CrossCorrelation(&y[LHIST-M] /* x */, &y[n]);
if (Cn>maxCn){ if (Cn>maxCn){
bestmatch=n; bestmatch=n;
maxCn = Cn; maxCn = Cn;
} }
} }
return(bestmatch); return bestmatch;
} }
float AmplitudeMatch(int16_t *y, int16_t bestmatch) { float AmplitudeMatch(int16_t *y, int16_t bestmatch) {
int i; int i;
float sumx; float sumx = 0;
float sumy; float sumy = 0.000001f;
float sf; float sf;
sumx = 0.0;
sumy = 0.000001f; for (i=0;i<SBC_FS;i++){
for (i=0;i<FS;i++){ sumx += abs(y[SBC_LHIST-SBC_FS+i]);
sumx += abs(y[LHIST-FS+i]);
sumy += abs(y[bestmatch+i]); sumy += abs(y[bestmatch+i]);
} }
sf = sumx/sumy; 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<0.75f) sf=0.75f;
if (sf>1.2f) sf=1.2f; if (sf>1.2f) sf=1.2f;
return(sf); return sf;
} }

14
src/classic/btstack_sbc_plc.h
View File

@ -49,16 +49,16 @@
extern "C" { extern "C" {
#endif #endif
#define FS 120 /* Frame Size */ #define SBC_FS 120 /* SBC Frame Size */
#define N 256 /* 16ms - Window Length for pattern matching */ #define SBC_N 256 /* 16ms - Window Length for pattern matching */
#define M 64 /* 4ms - Template for matching */ #define SBC_M 64 /* 4ms - Template for matching */
#define LHIST (N+FS-1) /* Length of history buffer required */ #define SBC_LHIST (SBC_N+SBC_FS-1) /* Length of history buffer required */
#define SBCRT 36 /* SBC Reconvergence Time (samples) */ #define SBC_RT 36 /* SBC Reconvergence Time (samples) */
#define OLAL 16 /* OverLap-Add Length (samples) */ #define SBC_OLAL 16 /* OverLap-Add Length (samples) */
/* PLC State Information */ /* PLC State Information */
typedef struct sbc_plc_state { typedef struct sbc_plc_state {
int16_t hist[LHIST+FS+SBCRT+OLAL]; int16_t hist[SBC_LHIST+SBC_FS+SBC_RT+SBC_OLAL];
int16_t bestlag; int16_t bestlag;
int nbf; int nbf;
} btstack_sbc_plc_state_t; } btstack_sbc_plc_state_t;

2
test/hfp/Makefile
View File

@ -53,7 +53,7 @@ VPATH += ${BTSTACK_ROOT}/src
VPATH += ${BTSTACK_ROOT}/src/classic VPATH += ${BTSTACK_ROOT}/src/classic
VPATH += ${BTSTACK_ROOT}/platform/posix VPATH += ${BTSTACK_ROOT}/platform/posix
CFLAGS = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/include -I${BTSTACK_ROOT}/ble CFLAGS = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/src/classic -I${BTSTACK_ROOT}/include -I${BTSTACK_ROOT}/ble
LDFLAGS += -lCppUTest -lCppUTestExt LDFLAGS += -lCppUTest -lCppUTestExt
EXAMPLES = hfp_ag_parser_test hfp_ag_client_test hfp_hf_parser_test hfp_hf_client_test cvsd_plc_test EXAMPLES = hfp_ag_parser_test hfp_ag_client_test hfp_hf_parser_test hfp_hf_client_test cvsd_plc_test