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 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 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 92 static float btstack_cvsd_plc_absolute(float x){ 93 if (x < 0) x = -x; 94 return x; 95 } 96 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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