From d1550f906a6f7312c10521c9150f12effd00dedc Mon Sep 17 00:00:00 2001 From: Milanka Ringwald Date: Wed, 19 Dec 2018 19:11:16 +0100 Subject: [PATCH] cvsd_plc: add printfs for octave file to analyse history buffer --- src/classic/btstack_cvsd_plc.c | 185 ++++++++++++++++++++++++++++++++- src/classic/btstack_cvsd_plc.h | 10 +- src/classic/btstack_sbc_plc.c | 2 +- src/classic/btstack_sbc_plc.h | 2 +- test/hfp/Makefile | 11 +- test/hfp/cvsd_plc_test.c | 146 +++++++++++++------------- test/hfp/pklg_cvsd_test.c | 5 + test/sbc/pklg_msbc_test.c | 2 +- 8 files changed, 274 insertions(+), 89 deletions(-) diff --git a/src/classic/btstack_cvsd_plc.c b/src/classic/btstack_cvsd_plc.c index ee8111ddf..49c94eed0 100644 --- a/src/classic/btstack_cvsd_plc.c +++ b/src/classic/btstack_cvsd_plc.c @@ -38,7 +38,7 @@ #define __BTSTACK_FILE__ "btstack_cvsd_plc.c" /* - * btstack_sbc_plc.c + * btstack_CVSD_plc.c * */ @@ -148,6 +148,153 @@ void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){ memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t)); } +#ifdef OCTAVE_OUTPUT +typedef enum { + OCTAVE_FRAME_TYPE_UNKNOWN = 0, + OCTAVE_FRAME_TYPE_GOOD, + OCTAVE_FRAME_TYPE_BAD +} octave_frame_type_t; + +static const char * octave_frame_type_name[] = { + "unknown", + "good", + "bad" +}; + +static octave_frame_type_t octave_frame_type; +static char octave_base_name[1000]; + +const char * octave_frame_type2str(int index){ + if (index <= 0 || index >= sizeof(octave_frame_type_t)) return octave_frame_type_name[0]; + return octave_frame_type_name[index]; +} + +void btstack_cvsd_plc_octave_set_base_name(const char * base_name){ + strcpy(octave_base_name, base_name); + printf("OCTAVE: base name set to %s\n", octave_base_name); +} + +static void octave_fprintf_array_int16(FILE * oct_file, char * name, int data_len, int16_t * data){ + fprintf(oct_file, "%s = [", name); + int i; + for (i = 0; i < data_len - 1; i++){ + fprintf(oct_file, "%d, ", data[i]); + } + fprintf(oct_file, "%d", data[i]); + fprintf(oct_file, "%s", "];\n"); +} + +static FILE * open_octave_file(btstack_cvsd_plc_state_t *plc_state, octave_frame_type_t frame_type){ + char oct_file_name[1200]; + octave_frame_type = frame_type; + sprintf(oct_file_name, "%s_octave_plc_%d_%s.m", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); + + FILE * oct_file = fopen(oct_file_name, "wb"); + if (oct_file == NULL){ + printf("OCTAVE: could not open file %s\n", oct_file_name); + return NULL; + } + printf("OCTAVE: opened file %s\n", oct_file_name); + return oct_file; +} + +static void octave_fprintf_plot_history_frame(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file, int frame_nr){ + char title[100]; + char hist_name[10]; + sprintf(hist_name, "hist%d", plc_state->nbf); + + octave_fprintf_array_int16(oct_file, hist_name, CVSD_LHIST, plc_state->hist); + + fprintf(oct_file, "y = [min(%s):1000:max(%s)];\n", hist_name, hist_name); + fprintf(oct_file, "x = zeros(1, size(y,2));\n"); + fprintf(oct_file, "b = [0: %d];\n", CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL); + + int pos = CVSD_FS; + fprintf(oct_file, "shift_x = x + %d;\n", pos); + + pos = CVSD_LHIST - 1; + fprintf(oct_file, "lhist_x = x + %d;\n", pos); + pos += CVSD_OLAL; + fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos); + pos += CVSD_FS - CVSD_OLAL; + fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos); + pos += CVSD_OLAL; + fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos); + pos += CVSD_RT; + fprintf(oct_file, "lhist_rt_x = x + %d;\n", pos); + + fprintf(oct_file, "pattern_window_x = x + %d;\n", CVSD_LHIST - CVSD_M); + + fprintf(oct_file, "hf = figure();\n"); + sprintf(title, "PLC %s frame %d", octave_frame_type2str(octave_frame_type), frame_nr); + + fprintf(oct_file, "hold on;\n"); + fprintf(oct_file, "h1 = plot(%s); \n", hist_name); + + fprintf(oct_file, "title(\"%s\");\n", title); + + fprintf(oct_file, "plot(lhist_x, y, 'k'); \n"); + fprintf(oct_file, "text(max(lhist_x) - 10, max(y)+1000, 'lhist'); \n"); + + fprintf(oct_file, "plot(lhist_olal1_x, y, 'k'); \n"); + fprintf(oct_file, "text(max(lhist_olal1_x) - 10, max(y)+1000, 'OLAL'); \n"); + + fprintf(oct_file, "plot(lhist_fs_x, y, 'k'); \n"); + fprintf(oct_file, "text(max(lhist_fs_x) - 10, max(y)+1000, 'FS'); \n"); + + fprintf(oct_file, "plot(lhist_olal2_x, y, 'k'); \n"); + fprintf(oct_file, "text(max(lhist_olal2_x) - 10, max(y)+1000, 'OLAL'); \n"); + + fprintf(oct_file, "plot(lhist_rt_x, y, 'k');\n"); + fprintf(oct_file, "text(max(lhist_rt_x) - 10, max(y)+1000, 'RT'); \n"); + + if (octave_frame_type == OCTAVE_FRAME_TYPE_GOOD) return; + + int x0 = plc_state->bestlag; + int x1 = plc_state->bestlag + CVSD_M - 1; + fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); + fprintf(oct_file, "text(%d - 10, -10, 'bestlag'); \n", x0); + + x0 = plc_state->bestlag + CVSD_M ; + x1 = plc_state->bestlag + CVSD_M + CVSD_FS - 1; + fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'kd'); \n", x0, x1, hist_name, x0, x1); + + x0 = CVSD_LHIST - CVSD_M; + x1 = CVSD_LHIST - 1; + fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); + fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n"); + fprintf(oct_file, "text(max(pattern_window_x) - 10, max(y)+1000, 'M'); \n"); +} + +static void octave_fprintf_plot_output(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file){ + if (!oct_file) return; + char out_name[10]; + sprintf(out_name, "out%d", plc_state->nbf); + int x0 = CVSD_LHIST; + int x1 = x0 + CVSD_FS - 1; + octave_fprintf_array_int16(oct_file, out_name, CVSD_FS, plc_state->hist+x0); + fprintf(oct_file, "h2 = plot(b(%d:%d), %s, 'cd'); \n", x0, x1, out_name); + + char rest_hist_name[10]; + sprintf(rest_hist_name, "rest%d", plc_state->nbf); + x0 = CVSD_LHIST + CVSD_FS; + x1 = x0 + CVSD_OLAL + CVSD_RT - 1; + octave_fprintf_array_int16(oct_file, rest_hist_name, CVSD_OLAL + CVSD_RT, plc_state->hist+x0); + fprintf(oct_file, "h3 = plot(b(%d:%d), %s, 'kd'); \n", x0, x1, rest_hist_name); + + char new_hist_name[10]; + sprintf(new_hist_name, "hist%d", plc_state->nbf); + octave_fprintf_array_int16(oct_file, new_hist_name, CVSD_LHIST, plc_state->hist); + fprintf(oct_file, "h4 = plot(%s, 'r--'); \n", new_hist_name); + + fprintf(oct_file, "legend ([h1, h2, h3, h4], {\"hist\", \"out\", \"rest\", \"new hist\"}, \"location\", \"northeast\");\n "); + + char fig_name[1200]; + sprintf(fig_name, "../%s_octave_plc_%d_%s", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); + fprintf(oct_file, "print(hf, \"%s.jpg\", \"-djpg\");", fig_name); +} +#endif + void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *out){ float val; int i = 0; @@ -157,11 +304,20 @@ void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t nu if (plc_state->max_consecutive_bad_frames_nr < plc_state->nbf){ plc_state->max_consecutive_bad_frames_nr = plc_state->nbf; } - - // plc_state->cvsd_fs = CVSD_FS_MAX; if (plc_state->nbf==1){ + // printf("first bad frame\n"); // Perform pattern matching to find where to replicate plc_state->bestlag = btstack_cvsd_plc_pattern_match(plc_state->hist); + } + +#ifdef OCTAVE_OUTPUT + FILE * oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_BAD); + if (oct_file){ + octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); + } +#endif + + if (plc_state->nbf==1){ // the replication begins after the template match plc_state->bestlag += CVSD_M; @@ -201,11 +357,27 @@ void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t nu for (i=0;ihist[i] = plc_state->hist[i+num_samples]; } + +#ifdef OCTAVE_OUTPUT + if (oct_file){ + octave_fprintf_plot_output(plc_state, oct_file); + fclose(oct_file); + } +#endif } void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *in, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *out){ float val; int i = 0; +#ifdef OCTAVE_OUTPUT + FILE * oct_file = NULL; + if (plc_state->nbf>0){ + oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD); + if (oct_file){ + octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); + } + } +#endif if (plc_state->nbf>0){ for (i=0;ihist[CVSD_LHIST+i]; @@ -230,6 +402,13 @@ void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t n for (i=0;ihist[i] = plc_state->hist[i+num_samples]; } + +#ifdef OCTAVE_OUTPUT + if (oct_file){ + octave_fprintf_plot_output(plc_state, oct_file); + fclose(oct_file); + } +#endif plc_state->nbf=0; } diff --git a/src/classic/btstack_cvsd_plc.h b/src/classic/btstack_cvsd_plc.h index 1cd014269..d560783dd 100644 --- a/src/classic/btstack_cvsd_plc.h +++ b/src/classic/btstack_cvsd_plc.h @@ -51,16 +51,16 @@ extern "C" { #define BTSTACK_CVSD_PLC_SAMPLE_FORMAT int16_t -#define CVSD_FS_MAX 60 /* CVSD Frame Size */ +#define CVSD_FS 60 /* CVSD Frame Size */ #define CVSD_N 256 /* Window Length for pattern matching */ #define CVSD_M 32 /* Template for matching */ -#define CVSD_LHIST (CVSD_N+CVSD_FS_MAX-1) /* Length of history buffer required */ +#define CVSD_LHIST (CVSD_N+CVSD_FS-1) /* Length of history buffer required */ #define CVSD_RT 18 /* Reconvergence Time (samples) */ #define CVSD_OLAL 8 /* OverLap-Add Length (samples) */ /* PLC State Information */ typedef struct cvsd_plc_state { - int16_t hist[CVSD_LHIST+CVSD_FS_MAX+CVSD_RT+CVSD_OLAL]; + int16_t hist[CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL]; int16_t bestlag; int nbf; @@ -88,6 +88,10 @@ float btstack_cvsd_plc_amplitude_match(btstack_cvsd_plc_state_t *plc_state, uint BTSTACK_CVSD_PLC_SAMPLE_FORMAT btstack_cvsd_plc_crop_sample(float val); float btstack_cvsd_plc_rcos(int index); +#ifdef OCTAVE_OUTPUT +void btstack_cvsd_plc_octave_set_base_name(const char * name); +#endif + #if defined __cplusplus } #endif diff --git a/src/classic/btstack_sbc_plc.c b/src/classic/btstack_sbc_plc.c index 3b3834c9e..d12f33161 100644 --- a/src/classic/btstack_sbc_plc.c +++ b/src/classic/btstack_sbc_plc.c @@ -175,7 +175,7 @@ const char * octave_frame_type2str(int index){ return octave_frame_type_name[index]; } -void octave_set_base_name(const char * base_name){ +void btstack_sbc_plc_octave_set_base_name(const char * base_name){ strcpy(octave_base_name, base_name); printf("OCTAVE: base name set to %s\n", octave_base_name); } diff --git a/src/classic/btstack_sbc_plc.h b/src/classic/btstack_sbc_plc.h index 3d650756f..14281a4c2 100644 --- a/src/classic/btstack_sbc_plc.h +++ b/src/classic/btstack_sbc_plc.h @@ -77,7 +77,7 @@ uint8_t * btstack_sbc_plc_zero_signal_frame(void); void btstack_sbc_dump_statistics(btstack_sbc_plc_state_t * state); #ifdef OCTAVE_OUTPUT -void octave_set_base_name(const char * name); +void btstack_sbc_plc_octave_set_base_name(const char * name); #endif #if defined __cplusplus diff --git a/test/hfp/Makefile b/test/hfp/Makefile index ca725114c..b2827762d 100644 --- a/test/hfp/Makefile +++ b/test/hfp/Makefile @@ -44,17 +44,14 @@ COMMON_OBJ = $(COMMON:.c=.o) MOCK_OBJ = $(MOCK:.c=.o) # CC = gcc-fsf-4.9 -CFLAGS = -g -Wall -Wmissing-prototype -Wnarrowing \ - -I. \ - -I.. \ - -I${BTSTACK_ROOT}/src -# CFLAGS += -Werror - VPATH += ${BTSTACK_ROOT}/src VPATH += ${BTSTACK_ROOT}/src/classic VPATH += ${BTSTACK_ROOT}/platform/posix -CFLAGS = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/src/classic -I${POSIX_ROOT} -I${BTSTACK_ROOT}/include +CFLAGS = -g -Wall -Wmissing-prototypes -Wnarrowing +# CFLAGS += -Werror +CFLAGS += -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/src/classic -I${POSIX_ROOT} -I${BTSTACK_ROOT}/include +CFLAGS += -D OCTAVE_OUTPUT LDFLAGS += -lCppUTest -lCppUTestExt EXAMPLES = hfp_ag_parser_test hfp_ag_client_test hfp_hf_parser_test hfp_hf_client_test cvsd_plc_test pklg_cvsd_test diff --git a/test/hfp/cvsd_plc_test.c b/test/hfp/cvsd_plc_test.c index 5b3ff160b..92a29a029 100644 --- a/test/hfp/cvsd_plc_test.c +++ b/test/hfp/cvsd_plc_test.c @@ -206,14 +206,14 @@ static void fprintf_plot_history(FILE * oct_file, char * name, int data_len, int fprintf(oct_file, "x = zeros(1, size(y,2));\n"); fprintf(oct_file, "b = [0:500];\n"); - int pos = CVSD_FS_MAX; + int pos = CVSD_FS; fprintf(oct_file, "shift_x = x + %d;\n", pos); pos = CVSD_LHIST - 1; fprintf(oct_file, "lhist_x = x + %d;\n", pos); pos += CVSD_OLAL; fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos); - pos += CVSD_FS_MAX - CVSD_OLAL; + pos += CVSD_FS - CVSD_OLAL; fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos); pos += CVSD_OLAL; fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos); @@ -246,88 +246,88 @@ static void fprintf_plot_history(FILE * oct_file, char * name, int data_len, int fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n"); } -// TEST(CVSD_PLC, CountEqBytes){ -// // init cvsd_fs in plc_state -// float val, sf; -// int i, x0, x1; +TEST(CVSD_PLC, CountEqBytes){ + // init cvsd_fs in plc_state + float val, sf; + int i, x0, x1; -// char * name; -// BTSTACK_CVSD_PLC_SAMPLE_FORMAT out[CVSD_FS_MAX]; -// BTSTACK_CVSD_PLC_SAMPLE_FORMAT hist[CVSD_LHIST+CVSD_FS_MAX+CVSD_RT+CVSD_OLAL]; -// FILE * oct_file = fopen("/Users/mringwal/octave/plc.m", "wb"); -// if (!oct_file) return; -// fprintf(oct_file, "%s", "1;\n\n"); + char * name; + BTSTACK_CVSD_PLC_SAMPLE_FORMAT out[CVSD_FS]; + BTSTACK_CVSD_PLC_SAMPLE_FORMAT hist[CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL]; + FILE * oct_file = fopen("/Users/mringwal/octave/plc.m", "wb"); + if (!oct_file) return; + fprintf(oct_file, "%s", "1;\n\n"); -// int hist_len = sizeof(plc_state.hist)/2; -// create_sine_wave_int16_data(CVSD_LHIST, hist); -// memset(plc_state.hist, hist[CVSD_LHIST-1], sizeof(plc_state.hist)); -// memcpy(plc_state.hist, hist, CVSD_LHIST*2); + int hist_len = sizeof(plc_state.hist)/2; + create_sine_wave_int16_data(CVSD_LHIST, hist); + memset(plc_state.hist, hist[CVSD_LHIST-1], sizeof(plc_state.hist)); + memcpy(plc_state.hist, hist, CVSD_LHIST*2); -// // Perform pattern matching to find where to replicate -// plc_state.bestlag = btstack_cvsd_plc_pattern_match(plc_state.hist); -// name = (char *) "hist0"; -// fprintf_plot_history(oct_file, name, hist_len, plc_state.hist); + // Perform pattern matching to find where to replicate + plc_state.bestlag = btstack_cvsd_plc_pattern_match(plc_state.hist); + name = (char *) "hist0"; + fprintf_plot_history(oct_file, name, hist_len, plc_state.hist); -// plc_state.bestlag += CVSD_M; -// sf = btstack_cvsd_plc_amplitude_match(&plc_state, audio_samples_per_frame, plc_state.hist, plc_state.bestlag); + plc_state.bestlag += CVSD_M; + sf = btstack_cvsd_plc_amplitude_match(&plc_state, audio_samples_per_frame, plc_state.hist, plc_state.bestlag); -// for (i=0;i