1 /*
2 * Copyright (C) 2016 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 3. Neither the name of the copyright holders nor the names of
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 * 4. Any redistribution, use, or modification is done solely for
17 * personal benefit and not for any commercial purpose or for
18 * monetary gain.
19 *
20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 *
33 * Please inquire about commercial licensing options at
34 * [email protected]
35 *
36 */
37
38 #define BTSTACK_FILE__ "btstack_cvsd_plc.c"
39
40 /*
41 * btstack_CVSD_plc.c
42 *
43 */
44
45 #include <stdint.h>
46 #include <stdlib.h>
47 #include <string.h>
48
49 #ifdef OCTAVE_OUTPUT
50 #include <stdio.h>
51 #endif
52
53 #include "btstack_cvsd_plc.h"
54 #include "btstack_debug.h"
55
56 // static float rcos[CVSD_OLAL] = {
57 // 0.99148655f,0.96623611f,0.92510857f,0.86950446f,
58 // 0.80131732f,0.72286918f,0.63683150f,0.54613418f,
59 // 0.45386582f,0.36316850f,0.27713082f,0.19868268f,
60 // 0.13049554f,0.07489143f,0.03376389f,0.00851345f};
61
62 static float rcos[CVSD_OLAL] = {
63 0.99148655f,0.92510857f,
64 0.80131732f,0.63683150f,
65 0.45386582f,0.27713082f,
66 0.13049554f,0.03376389f};
67
btstack_cvsd_plc_rcos(int index)68 float btstack_cvsd_plc_rcos(int index){
69 if (index >= CVSD_OLAL) return 0;
70 return rcos[index];
71 }
72
73 // taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
74 // Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation
sqrt3(const float x)75 static float sqrt3(const float x){
76 union {
77 int i;
78 float x;
79 } u;
80 u.x = x;
81 u.i = (1<<29) + (u.i >> 1) - (1<<22);
82
83 // Two Babylonian Steps (simplified from:)
84 // u.x = 0.5f * (u.x + x/u.x);
85 // u.x = 0.5f * (u.x + x/u.x);
86 u.x = u.x + (x/u.x);
87 u.x = (0.25f*u.x) + (x/u.x);
88
89 return u.x;
90 }
91
btstack_cvsd_plc_absolute(float x)92 static float btstack_cvsd_plc_absolute(float x){
93 if (x < 0) x = -x;
94 return x;
95 }
96
btstack_cvsd_plc_cross_correlation(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * x,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * y)97 static float btstack_cvsd_plc_cross_correlation(BTSTACK_CVSD_PLC_SAMPLE_FORMAT *x, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *y){
98 float num = 0.f;
99 float den = 0.f;
100 float x2 = 0.f;
101 float y2 = 0.f;
102 int m;
103 for (m=0;m<CVSD_M;m++){
104 num+=((float)x[m])*y[m];
105 x2+=((float)x[m])*x[m];
106 y2+=((float)y[m])*y[m];
107 }
108 den = (float)sqrt3(x2*y2);
109 return num/den;
110 }
111
btstack_cvsd_plc_pattern_match(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * y)112 int btstack_cvsd_plc_pattern_match(BTSTACK_CVSD_PLC_SAMPLE_FORMAT *y){
113 float maxCn = -999999.f; // large negative number
114 int bestmatch = 0;
115 float Cn;
116 int n;
117 for (n=0;n<CVSD_N;n++){
118 Cn = btstack_cvsd_plc_cross_correlation(&y[CVSD_LHIST-CVSD_M], &y[n]);
119 if (Cn>maxCn){
120 bestmatch=n;
121 maxCn = Cn;
122 }
123 }
124 return bestmatch;
125 }
126
btstack_cvsd_plc_amplitude_match(btstack_cvsd_plc_state_t * plc_state,uint16_t num_samples,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * y,BTSTACK_CVSD_PLC_SAMPLE_FORMAT bestmatch)127 float btstack_cvsd_plc_amplitude_match(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *y, BTSTACK_CVSD_PLC_SAMPLE_FORMAT bestmatch){
128 UNUSED(plc_state);
129 int i;
130 float sumx = 0.f;
131 float sumy = 0.000001f;
132 float sf;
133
134 for (i=0;i<num_samples;i++){
135 sumx += btstack_cvsd_plc_absolute(y[CVSD_LHIST-num_samples+i]);
136 sumy += btstack_cvsd_plc_absolute(y[bestmatch+i]);
137 }
138 sf = sumx/sumy;
139 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts
140 if (sf<0.75f) sf=0.75f;
141 if (sf>1.f) sf=1.f;
142 return sf;
143 }
144
btstack_cvsd_plc_crop_sample(float val)145 BTSTACK_CVSD_PLC_SAMPLE_FORMAT btstack_cvsd_plc_crop_sample(float val){
146 float croped_val = val;
147 if (croped_val > 32767.f) croped_val= 32767.f;
148 if (croped_val < -32768.f) croped_val=-32768.f;
149 return (BTSTACK_CVSD_PLC_SAMPLE_FORMAT) croped_val;
150 }
151
btstack_cvsd_plc_init(btstack_cvsd_plc_state_t * plc_state)152 void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){
153 memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t));
154 }
155
156 #ifdef OCTAVE_OUTPUT
157 typedef enum {
158 OCTAVE_FRAME_TYPE_UNKNOWN = 0,
159 OCTAVE_FRAME_TYPE_GOOD,
160 OCTAVE_FRAME_TYPE_BAD
161 } octave_frame_type_t;
162
163 static const char * octave_frame_type_name[] = {
164 "unknown",
165 "good",
166 "bad"
167 };
168
169 static octave_frame_type_t octave_frame_type;
170 static char octave_base_name[1000];
171
octave_frame_type2str(int index)172 const char * octave_frame_type2str(int index){
173 if (index <= 0 || index >= sizeof(octave_frame_type_t)) return octave_frame_type_name[0];
174 return octave_frame_type_name[index];
175 }
176
btstack_cvsd_plc_octave_set_base_name(const char * base_name)177 void btstack_cvsd_plc_octave_set_base_name(const char * base_name){
178 strcpy(octave_base_name, base_name);
179 printf("OCTAVE: base name set to %s\n", octave_base_name);
180 }
181
octave_fprintf_array_int16(FILE * oct_file,char * name,int data_len,int16_t * data)182 static void octave_fprintf_array_int16(FILE * oct_file, char * name, int data_len, int16_t * data){
183 fprintf(oct_file, "%s = [", name);
184 int i;
185 for (i = 0; i < data_len - 1; i++){
186 fprintf(oct_file, "%d, ", data[i]);
187 }
188 fprintf(oct_file, "%d", data[i]);
189 fprintf(oct_file, "%s", "];\n");
190 }
191
open_octave_file(btstack_cvsd_plc_state_t * plc_state,octave_frame_type_t frame_type)192 static FILE * open_octave_file(btstack_cvsd_plc_state_t *plc_state, octave_frame_type_t frame_type){
193 char oct_file_name[1200];
194 octave_frame_type = frame_type;
195 snprintf(oct_file_name, sizeof(oct_file_name), "%s_octave_plc_%d_%s.m",
196 octave_base_name, plc_state->frame_count,
197 octave_frame_type2str(octave_frame_type));
198 oct_file_name[sizeof(oct_file_name) - 1] = 0;
199
200 FILE * oct_file = fopen(oct_file_name, "wb");
201 if (oct_file == NULL){
202 printf("OCTAVE: could not open file %s\n", oct_file_name);
203 return NULL;
204 }
205 printf("OCTAVE: opened file %s\n", oct_file_name);
206 return oct_file;
207 }
208
octave_fprintf_plot_history_frame(btstack_cvsd_plc_state_t * plc_state,FILE * oct_file,int frame_nr)209 static void octave_fprintf_plot_history_frame(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file, int frame_nr){
210 char title[100];
211 char hist_name[10];
212 snprintf(hist_name, sizeof(hist_name), "hist%d", plc_state->nbf);
213 hist_name[sizeof(hist_name) - 1] = 0;
214
215 octave_fprintf_array_int16(oct_file, hist_name, CVSD_LHIST, plc_state->hist);
216
217 fprintf(oct_file, "y = [min(%s):1000:max(%s)];\n", hist_name, hist_name);
218 fprintf(oct_file, "x = zeros(1, size(y,2));\n");
219 fprintf(oct_file, "b = [0: %d];\n", CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL);
220
221 int pos = CVSD_FS;
222 fprintf(oct_file, "shift_x = x + %d;\n", pos);
223
224 pos = CVSD_LHIST - 1;
225 fprintf(oct_file, "lhist_x = x + %d;\n", pos);
226 pos += CVSD_OLAL;
227 fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos);
228 pos += CVSD_FS - CVSD_OLAL;
229 fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos);
230 pos += CVSD_OLAL;
231 fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos);
232 pos += CVSD_RT;
233 fprintf(oct_file, "lhist_rt_x = x + %d;\n", pos);
234
235 fprintf(oct_file, "pattern_window_x = x + %d;\n", CVSD_LHIST - CVSD_M);
236
237 fprintf(oct_file, "hf = figure();\n");
238 snprintf(title, sizeof(title), "PLC %s frame %d",
239 octave_frame_type2str(octave_frame_type), frame_nr);
240 title[sizeof(title) - 1] = 0;
241
242 fprintf(oct_file, "hold on;\n");
243 fprintf(oct_file, "h1 = plot(%s); \n", hist_name);
244
245 fprintf(oct_file, "title(\"%s\");\n", title);
246
247 fprintf(oct_file, "plot(lhist_x, y, 'k'); \n");
248 fprintf(oct_file, "text(max(lhist_x) - 10, max(y)+1000, 'lhist'); \n");
249
250 fprintf(oct_file, "plot(lhist_olal1_x, y, 'k'); \n");
251 fprintf(oct_file, "text(max(lhist_olal1_x) - 10, max(y)+1000, 'OLAL'); \n");
252
253 fprintf(oct_file, "plot(lhist_fs_x, y, 'k'); \n");
254 fprintf(oct_file, "text(max(lhist_fs_x) - 10, max(y)+1000, 'FS'); \n");
255
256 fprintf(oct_file, "plot(lhist_olal2_x, y, 'k'); \n");
257 fprintf(oct_file, "text(max(lhist_olal2_x) - 10, max(y)+1000, 'OLAL'); \n");
258
259 fprintf(oct_file, "plot(lhist_rt_x, y, 'k');\n");
260 fprintf(oct_file, "text(max(lhist_rt_x) - 10, max(y)+1000, 'RT'); \n");
261
262 if (octave_frame_type == OCTAVE_FRAME_TYPE_GOOD) return;
263
264 int x0 = plc_state->bestlag;
265 int x1 = plc_state->bestlag + CVSD_M - 1;
266 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1);
267 fprintf(oct_file, "text(%d - 10, -10, 'bestlag'); \n", x0);
268
269 x0 = plc_state->bestlag + CVSD_M ;
270 x1 = plc_state->bestlag + CVSD_M + CVSD_FS - 1;
271 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'kd'); \n", x0, x1, hist_name, x0, x1);
272
273 x0 = CVSD_LHIST - CVSD_M;
274 x1 = CVSD_LHIST - 1;
275 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1);
276 fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n");
277 fprintf(oct_file, "text(max(pattern_window_x) - 10, max(y)+1000, 'M'); \n");
278 }
279
octave_fprintf_plot_output(btstack_cvsd_plc_state_t * plc_state,FILE * oct_file)280 static void octave_fprintf_plot_output(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file){
281 if (!oct_file) return;
282 char out_name[10];
283 snprintf(out_name, sizeof(out_name), "out%d", plc_state->nbf);
284 out_name[sizeof(out_name) - 1] = 0;
285 int x0 = CVSD_LHIST;
286 int x1 = x0 + CVSD_FS - 1;
287 octave_fprintf_array_int16(oct_file, out_name, CVSD_FS, plc_state->hist+x0);
288 fprintf(oct_file, "h2 = plot(b(%d:%d), %s, 'cd'); \n", x0, x1, out_name);
289
290 char rest_hist_name[10];
291 snprintf(rest_hist_name, sizeof(rest_hist_name), "rest%d", plc_state->nbf);
292 rest_hist_name[sizeof(rest_hist_name) - 1] = 0;
293 x0 = CVSD_LHIST + CVSD_FS;
294 x1 = x0 + CVSD_OLAL + CVSD_RT - 1;
295 octave_fprintf_array_int16(oct_file, rest_hist_name, CVSD_OLAL + CVSD_RT, plc_state->hist+x0);
296 fprintf(oct_file, "h3 = plot(b(%d:%d), %s, 'kd'); \n", x0, x1, rest_hist_name);
297
298 char new_hist_name[10];
299 snprintf(new_hist_name, sizeof(new_hist_name), "hist%d", plc_state->nbf);
300 new_hist_name[sizeof(new_hist_name) - 1] = 0;
301 octave_fprintf_array_int16(oct_file, new_hist_name, CVSD_LHIST, plc_state->hist);
302 fprintf(oct_file, "h4 = plot(%s, 'r--'); \n", new_hist_name);
303
304 fprintf(oct_file, "legend ([h1, h2, h3, h4], {\"hist\", \"out\", \"rest\", \"new hist\"}, \"location\", \"northeast\");\n ");
305
306 char fig_name[1200];
307 snprintf(fig_name, sizeof(fig_name), "../%s_octave_plc_%d_%s",
308 octave_base_name, plc_state->frame_count,
309 octave_frame_type2str(octave_frame_type));
310 fig_name[sizeof(fig_name) - 1] = 0;
311 fprintf(oct_file, "print(hf, \"%s.jpg\", \"-djpg\");", fig_name);
312 }
313 #endif
314
btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t * plc_state,uint16_t num_samples,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * out)315 void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *out){
316 float val;
317 int i;
318 float sf = 1;
319 plc_state->nbf++;
320
321 if (plc_state->max_consecutive_bad_frames_nr < plc_state->nbf){
322 plc_state->max_consecutive_bad_frames_nr = plc_state->nbf;
323 }
324 if (plc_state->nbf==1){
325 // printf("first bad frame\n");
326 // Perform pattern matching to find where to replicate
327 plc_state->bestlag = btstack_cvsd_plc_pattern_match(plc_state->hist);
328 }
329
330 #ifdef OCTAVE_OUTPUT
331 FILE * oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_BAD);
332 if (oct_file){
333 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
334 }
335 #endif
336
337 if (plc_state->nbf==1){
338 // the replication begins after the template match
339 plc_state->bestlag += CVSD_M;
340
341 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet
342 sf = btstack_cvsd_plc_amplitude_match(plc_state, num_samples, plc_state->hist, plc_state->bestlag);
343 for (i=0; i<CVSD_OLAL; i++){
344 val = sf*plc_state->hist[plc_state->bestlag+i];
345 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val);
346 }
347
348 for (i=CVSD_OLAL; i<num_samples; i++){
349 val = sf*plc_state->hist[plc_state->bestlag+i];
350 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val);
351 }
352
353 for (i=num_samples; i<(num_samples+CVSD_OLAL); i++){
354 float left = sf*plc_state->hist[plc_state->bestlag+i];
355 float right = plc_state->hist[plc_state->bestlag+i];
356 val = (left*rcos[i-num_samples]) + (right*rcos[CVSD_OLAL-1-i+num_samples]);
357 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val);
358 }
359
360 for (i=(num_samples+CVSD_OLAL); i<(num_samples+CVSD_RT+CVSD_OLAL); i++){
361 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
362 }
363 } else {
364 for (i=0; i<(num_samples+CVSD_RT+CVSD_OLAL); i++){
365 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
366 }
367 }
368
369 for (i=0; i<num_samples; i++){
370 out[i] = plc_state->hist[CVSD_LHIST+i];
371 }
372
373 // shift the history buffer
374 for (i=0; i<(CVSD_LHIST+CVSD_RT+CVSD_OLAL); i++){
375 plc_state->hist[i] = plc_state->hist[i+num_samples];
376 }
377
378 #ifdef OCTAVE_OUTPUT
379 if (oct_file){
380 octave_fprintf_plot_output(plc_state, oct_file);
381 fclose(oct_file);
382 }
383 #endif
384 }
385
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)386 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){
387 float val;
388 int i = 0;
389 #ifdef OCTAVE_OUTPUT
390 FILE * oct_file = NULL;
391 if (plc_state->nbf>0){
392 oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD);
393 if (oct_file){
394 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
395 }
396 }
397 #endif
398 if (plc_state->nbf>0){
399 for (i=0;i<CVSD_RT;i++){
400 out[i] = plc_state->hist[CVSD_LHIST+i];
401 }
402
403 for (i=CVSD_RT;i<(CVSD_RT+CVSD_OLAL);i++){
404 float left = plc_state->hist[CVSD_LHIST+i];
405 float right = in[i];
406 val = (left * rcos[i-CVSD_RT]) + (right *rcos[CVSD_OLAL+CVSD_RT-1-i]);
407 out[i] = btstack_cvsd_plc_crop_sample((BTSTACK_CVSD_PLC_SAMPLE_FORMAT)val);
408 }
409 }
410
411 for (;i<num_samples;i++){
412 out[i] = in[i];
413 }
414 // Copy the output to the history buffer
415 for (i=0;i<num_samples;i++){
416 plc_state->hist[CVSD_LHIST+i] = out[i];
417 }
418 // shift the history buffer
419 for (i=0;i<CVSD_LHIST;i++){
420 plc_state->hist[i] = plc_state->hist[i+num_samples];
421 }
422
423 #ifdef OCTAVE_OUTPUT
424 if (oct_file){
425 octave_fprintf_plot_output(plc_state, oct_file);
426 fclose(oct_file);
427 }
428 #endif
429 plc_state->nbf=0;
430 }
431
count_equal_samples(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * packet,uint16_t size)432 static int count_equal_samples(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * packet, uint16_t size){
433 int count = 0;
434 int temp_count = 1;
435 int i;
436 for (i = 0; i < (size-1); i++){
437 if (packet[i] == packet[i+1]){
438 temp_count++;
439 continue;
440 }
441 if (count < temp_count){
442 count = temp_count;
443 }
444 temp_count = 1;
445 }
446 if (temp_count > (count + 1)){
447 count = temp_count;
448 }
449 return count;
450 }
451
count_zeros(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame,uint16_t size)452 static int count_zeros(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){
453 int nr_zeros = 0;
454 int i;
455 for (i = 0; i < (size-1); i++){
456 if (frame[i] == 0){
457 nr_zeros++;
458 }
459 }
460 return nr_zeros;
461 }
462
zero_frame(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame,uint16_t size)463 static int zero_frame(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){
464 return count_zeros(frame, size) == size;
465 }
466
467 // more than half the samples are the same -> bad frame
bad_frame(btstack_cvsd_plc_state_t * plc_state,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame,uint16_t size)468 static int bad_frame(btstack_cvsd_plc_state_t *plc_state, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){
469 UNUSED(plc_state);
470 return count_equal_samples(frame, size) >= (size / 2);
471 }
472
473
btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * plc_state,bool is_bad_frame,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * in,uint16_t num_samples,BTSTACK_CVSD_PLC_SAMPLE_FORMAT * out)474 void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * plc_state, bool is_bad_frame, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * in, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * out){
475 if (num_samples == 0) return;
476
477 plc_state->frame_count++;
478
479 if (!is_bad_frame) {
480 bool is_zero_frame = zero_frame(in, num_samples);
481 if (is_zero_frame){
482 plc_state->zero_frames_nr++;
483 } else {
484 is_bad_frame = bad_frame(plc_state, in, num_samples);
485 }
486 }
487
488 if (is_bad_frame){
489 (void)memcpy(out, in, num_samples * 2);
490 if (plc_state->good_samples > CVSD_LHIST){
491 btstack_cvsd_plc_bad_frame(plc_state, num_samples, out);
492 plc_state->bad_frames_nr++;
493 } else {
494 memset(out, 0, num_samples * 2);
495 }
496 } else {
497 btstack_cvsd_plc_good_frame(plc_state, num_samples, in, out);
498 plc_state->good_frames_nr++;
499 if (plc_state->good_frames_nr == 1){
500 log_info("First good frame at index %d\n", plc_state->frame_count-1);
501 }
502 plc_state->good_samples += num_samples;
503 }
504 }
505
btstack_cvsd_dump_statistics(btstack_cvsd_plc_state_t * state)506 void btstack_cvsd_dump_statistics(btstack_cvsd_plc_state_t * state){
507 UNUSED(state);
508 log_info("Good frames: %d\n", state->good_frames_nr);
509 log_info("Bad frames: %d\n", state->bad_frames_nr);
510 log_info("Zero frames: %d\n", state->zero_frames_nr);
511 log_info("Max Consecutive bad frames: %d\n", state->max_consecutive_bad_frames_nr);
512 }
513