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 MATTHIAS 24 * RINGWALD 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 <stdio.h> 47 #include <stdlib.h> 48 #include <string.h> 49 50 #include "btstack_cvsd_plc.h" 51 #include "btstack_debug.h" 52 53 // static float rcos[CVSD_OLAL] = { 54 // 0.99148655f,0.96623611f,0.92510857f,0.86950446f, 55 // 0.80131732f,0.72286918f,0.63683150f,0.54613418f, 56 // 0.45386582f,0.36316850f,0.27713082f,0.19868268f, 57 // 0.13049554f,0.07489143f,0.03376389f,0.00851345f}; 58 59 static float rcos[CVSD_OLAL] = { 60 0.99148655f,0.92510857f, 61 0.80131732f,0.63683150f, 62 0.45386582f,0.27713082f, 63 0.13049554f,0.03376389f}; 64 65 float btstack_cvsd_plc_rcos(int index){ 66 if (index > CVSD_OLAL) return 0; 67 return rcos[index]; 68 } 69 // taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi 70 // Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation 71 static float sqrt3(const float x){ 72 union { 73 int i; 74 float x; 75 } u; 76 u.x = x; 77 u.i = (1<<29) + (u.i >> 1) - (1<<22); 78 79 // Two Babylonian Steps (simplified from:) 80 // u.x = 0.5f * (u.x + x/u.x); 81 // u.x = 0.5f * (u.x + x/u.x); 82 u.x = u.x + (x/u.x); 83 u.x = (0.25f*u.x) + (x/u.x); 84 85 return u.x; 86 } 87 88 static float btstack_cvsd_plc_absolute(float x){ 89 if (x < 0) x = -x; 90 return x; 91 } 92 93 static float btstack_cvsd_plc_cross_correlation(BTSTACK_CVSD_PLC_SAMPLE_FORMAT *x, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *y){ 94 float num = 0; 95 float den = 0; 96 float x2 = 0; 97 float y2 = 0; 98 int m; 99 for (m=0;m<CVSD_M;m++){ 100 num+=((float)x[m])*y[m]; 101 x2+=((float)x[m])*x[m]; 102 y2+=((float)y[m])*y[m]; 103 } 104 den = (float)sqrt3(x2*y2); 105 return num/den; 106 } 107 108 int btstack_cvsd_plc_pattern_match(BTSTACK_CVSD_PLC_SAMPLE_FORMAT *y){ 109 float maxCn = -999999.0; // large negative number 110 int bestmatch = 0; 111 float Cn; 112 int n; 113 for (n=0;n<CVSD_N;n++){ 114 Cn = btstack_cvsd_plc_cross_correlation(&y[CVSD_LHIST-CVSD_M], &y[n]); 115 if (Cn>maxCn){ 116 bestmatch=n; 117 maxCn = Cn; 118 } 119 } 120 return bestmatch; 121 } 122 123 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){ 124 UNUSED(plc_state); 125 int i; 126 float sumx = 0; 127 float sumy = 0.000001f; 128 float sf; 129 130 for (i=0;i<num_samples;i++){ 131 sumx += btstack_cvsd_plc_absolute(y[CVSD_LHIST-num_samples+i]); 132 sumy += btstack_cvsd_plc_absolute(y[bestmatch+i]); 133 } 134 sf = sumx/sumy; 135 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts 136 if (sf<0.75f) sf=0.75f; 137 if (sf>1.0) sf=1.0f; 138 return sf; 139 } 140 141 BTSTACK_CVSD_PLC_SAMPLE_FORMAT btstack_cvsd_plc_crop_sample(float val){ 142 float croped_val = val; 143 if (croped_val > 32767.0) croped_val= 32767.0; 144 if (croped_val < -32768.0) croped_val=-32768.0; 145 return (BTSTACK_CVSD_PLC_SAMPLE_FORMAT) croped_val; 146 } 147 148 void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){ 149 memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t)); 150 } 151 152 #ifdef OCTAVE_OUTPUT 153 typedef enum { 154 OCTAVE_FRAME_TYPE_UNKNOWN = 0, 155 OCTAVE_FRAME_TYPE_GOOD, 156 OCTAVE_FRAME_TYPE_BAD 157 } octave_frame_type_t; 158 159 static const char * octave_frame_type_name[] = { 160 "unknown", 161 "good", 162 "bad" 163 }; 164 165 static octave_frame_type_t octave_frame_type; 166 static char octave_base_name[1000]; 167 168 const char * octave_frame_type2str(int index){ 169 if (index <= 0 || index >= sizeof(octave_frame_type_t)) return octave_frame_type_name[0]; 170 return octave_frame_type_name[index]; 171 } 172 173 void btstack_cvsd_plc_octave_set_base_name(const char * base_name){ 174 strcpy(octave_base_name, base_name); 175 printf("OCTAVE: base name set to %s\n", octave_base_name); 176 } 177 178 static void octave_fprintf_array_int16(FILE * oct_file, char * name, int data_len, int16_t * data){ 179 fprintf(oct_file, "%s = [", name); 180 int i; 181 for (i = 0; i < data_len - 1; i++){ 182 fprintf(oct_file, "%d, ", data[i]); 183 } 184 fprintf(oct_file, "%d", data[i]); 185 fprintf(oct_file, "%s", "];\n"); 186 } 187 188 static FILE * open_octave_file(btstack_cvsd_plc_state_t *plc_state, octave_frame_type_t frame_type){ 189 char oct_file_name[1200]; 190 octave_frame_type = frame_type; 191 sprintf(oct_file_name, "%s_octave_plc_%d_%s.m", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); 192 193 FILE * oct_file = fopen(oct_file_name, "wb"); 194 if (oct_file == NULL){ 195 printf("OCTAVE: could not open file %s\n", oct_file_name); 196 return NULL; 197 } 198 printf("OCTAVE: opened file %s\n", oct_file_name); 199 return oct_file; 200 } 201 202 static void octave_fprintf_plot_history_frame(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file, int frame_nr){ 203 char title[100]; 204 char hist_name[10]; 205 sprintf(hist_name, "hist%d", plc_state->nbf); 206 207 octave_fprintf_array_int16(oct_file, hist_name, CVSD_LHIST, plc_state->hist); 208 209 fprintf(oct_file, "y = [min(%s):1000:max(%s)];\n", hist_name, hist_name); 210 fprintf(oct_file, "x = zeros(1, size(y,2));\n"); 211 fprintf(oct_file, "b = [0: %d];\n", CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL); 212 213 int pos = CVSD_FS; 214 fprintf(oct_file, "shift_x = x + %d;\n", pos); 215 216 pos = CVSD_LHIST - 1; 217 fprintf(oct_file, "lhist_x = x + %d;\n", pos); 218 pos += CVSD_OLAL; 219 fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos); 220 pos += CVSD_FS - CVSD_OLAL; 221 fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos); 222 pos += CVSD_OLAL; 223 fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos); 224 pos += CVSD_RT; 225 fprintf(oct_file, "lhist_rt_x = x + %d;\n", pos); 226 227 fprintf(oct_file, "pattern_window_x = x + %d;\n", CVSD_LHIST - CVSD_M); 228 229 fprintf(oct_file, "hf = figure();\n"); 230 sprintf(title, "PLC %s frame %d", octave_frame_type2str(octave_frame_type), frame_nr); 231 232 fprintf(oct_file, "hold on;\n"); 233 fprintf(oct_file, "h1 = plot(%s); \n", hist_name); 234 235 fprintf(oct_file, "title(\"%s\");\n", title); 236 237 fprintf(oct_file, "plot(lhist_x, y, 'k'); \n"); 238 fprintf(oct_file, "text(max(lhist_x) - 10, max(y)+1000, 'lhist'); \n"); 239 240 fprintf(oct_file, "plot(lhist_olal1_x, y, 'k'); \n"); 241 fprintf(oct_file, "text(max(lhist_olal1_x) - 10, max(y)+1000, 'OLAL'); \n"); 242 243 fprintf(oct_file, "plot(lhist_fs_x, y, 'k'); \n"); 244 fprintf(oct_file, "text(max(lhist_fs_x) - 10, max(y)+1000, 'FS'); \n"); 245 246 fprintf(oct_file, "plot(lhist_olal2_x, y, 'k'); \n"); 247 fprintf(oct_file, "text(max(lhist_olal2_x) - 10, max(y)+1000, 'OLAL'); \n"); 248 249 fprintf(oct_file, "plot(lhist_rt_x, y, 'k');\n"); 250 fprintf(oct_file, "text(max(lhist_rt_x) - 10, max(y)+1000, 'RT'); \n"); 251 252 if (octave_frame_type == OCTAVE_FRAME_TYPE_GOOD) return; 253 254 int x0 = plc_state->bestlag; 255 int x1 = plc_state->bestlag + CVSD_M - 1; 256 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); 257 fprintf(oct_file, "text(%d - 10, -10, 'bestlag'); \n", x0); 258 259 x0 = plc_state->bestlag + CVSD_M ; 260 x1 = plc_state->bestlag + CVSD_M + CVSD_FS - 1; 261 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'kd'); \n", x0, x1, hist_name, x0, x1); 262 263 x0 = CVSD_LHIST - CVSD_M; 264 x1 = CVSD_LHIST - 1; 265 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); 266 fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n"); 267 fprintf(oct_file, "text(max(pattern_window_x) - 10, max(y)+1000, 'M'); \n"); 268 } 269 270 static void octave_fprintf_plot_output(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file){ 271 if (!oct_file) return; 272 char out_name[10]; 273 sprintf(out_name, "out%d", plc_state->nbf); 274 int x0 = CVSD_LHIST; 275 int x1 = x0 + CVSD_FS - 1; 276 octave_fprintf_array_int16(oct_file, out_name, CVSD_FS, plc_state->hist+x0); 277 fprintf(oct_file, "h2 = plot(b(%d:%d), %s, 'cd'); \n", x0, x1, out_name); 278 279 char rest_hist_name[10]; 280 sprintf(rest_hist_name, "rest%d", plc_state->nbf); 281 x0 = CVSD_LHIST + CVSD_FS; 282 x1 = x0 + CVSD_OLAL + CVSD_RT - 1; 283 octave_fprintf_array_int16(oct_file, rest_hist_name, CVSD_OLAL + CVSD_RT, plc_state->hist+x0); 284 fprintf(oct_file, "h3 = plot(b(%d:%d), %s, 'kd'); \n", x0, x1, rest_hist_name); 285 286 char new_hist_name[10]; 287 sprintf(new_hist_name, "hist%d", plc_state->nbf); 288 octave_fprintf_array_int16(oct_file, new_hist_name, CVSD_LHIST, plc_state->hist); 289 fprintf(oct_file, "h4 = plot(%s, 'r--'); \n", new_hist_name); 290 291 fprintf(oct_file, "legend ([h1, h2, h3, h4], {\"hist\", \"out\", \"rest\", \"new hist\"}, \"location\", \"northeast\");\n "); 292 293 char fig_name[1200]; 294 sprintf(fig_name, "../%s_octave_plc_%d_%s", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); 295 fprintf(oct_file, "print(hf, \"%s.jpg\", \"-djpg\");", fig_name); 296 } 297 #endif 298 299 void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *out){ 300 float val; 301 int i = 0; 302 float sf = 1; 303 plc_state->nbf++; 304 305 if (plc_state->max_consecutive_bad_frames_nr < plc_state->nbf){ 306 plc_state->max_consecutive_bad_frames_nr = plc_state->nbf; 307 } 308 if (plc_state->nbf==1){ 309 // printf("first bad frame\n"); 310 // Perform pattern matching to find where to replicate 311 plc_state->bestlag = btstack_cvsd_plc_pattern_match(plc_state->hist); 312 } 313 314 #ifdef OCTAVE_OUTPUT 315 FILE * oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_BAD); 316 if (oct_file){ 317 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); 318 } 319 #endif 320 321 if (plc_state->nbf==1){ 322 // the replication begins after the template match 323 plc_state->bestlag += CVSD_M; 324 325 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet 326 sf = btstack_cvsd_plc_amplitude_match(plc_state, num_samples, plc_state->hist, plc_state->bestlag); 327 for (i=0;i<CVSD_OLAL;i++){ 328 val = sf*plc_state->hist[plc_state->bestlag+i]; 329 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 330 } 331 332 for (;i<num_samples;i++){ 333 val = sf*plc_state->hist[plc_state->bestlag+i]; 334 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 335 } 336 337 for (;i<(num_samples+CVSD_OLAL);i++){ 338 float left = sf*plc_state->hist[plc_state->bestlag+i]; 339 float right = plc_state->hist[plc_state->bestlag+i]; 340 val = (left*rcos[i-num_samples]) + (right*rcos[CVSD_OLAL-1-i+num_samples]); 341 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 342 } 343 344 for (;i<(num_samples+CVSD_RT+CVSD_OLAL);i++){ 345 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 346 } 347 } else { 348 for (;i<(num_samples+CVSD_RT+CVSD_OLAL);i++){ 349 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 350 } 351 } 352 353 for (i=0;i<num_samples;i++){ 354 out[i] = plc_state->hist[CVSD_LHIST+i]; 355 } 356 357 // shift the history buffer 358 for (i=0;i<(CVSD_LHIST+CVSD_RT+CVSD_OLAL);i++){ 359 plc_state->hist[i] = plc_state->hist[i+num_samples]; 360 } 361 362 #ifdef OCTAVE_OUTPUT 363 if (oct_file){ 364 octave_fprintf_plot_output(plc_state, oct_file); 365 fclose(oct_file); 366 } 367 #endif 368 } 369 370 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){ 371 float val; 372 int i = 0; 373 #ifdef OCTAVE_OUTPUT 374 FILE * oct_file = NULL; 375 if (plc_state->nbf>0){ 376 oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD); 377 if (oct_file){ 378 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); 379 } 380 } 381 #endif 382 if (plc_state->nbf>0){ 383 for (i=0;i<CVSD_RT;i++){ 384 out[i] = plc_state->hist[CVSD_LHIST+i]; 385 } 386 387 for (i=CVSD_RT;i<(CVSD_RT+CVSD_OLAL);i++){ 388 float left = plc_state->hist[CVSD_LHIST+i]; 389 float right = in[i]; 390 val = (left * rcos[i-CVSD_RT]) + (right *rcos[CVSD_OLAL+CVSD_RT-1-i]); 391 out[i] = btstack_cvsd_plc_crop_sample((BTSTACK_CVSD_PLC_SAMPLE_FORMAT)val); 392 } 393 } 394 395 for (;i<num_samples;i++){ 396 out[i] = in[i]; 397 } 398 // Copy the output to the history buffer 399 for (i=0;i<num_samples;i++){ 400 plc_state->hist[CVSD_LHIST+i] = out[i]; 401 } 402 // shift the history buffer 403 for (i=0;i<CVSD_LHIST;i++){ 404 plc_state->hist[i] = plc_state->hist[i+num_samples]; 405 } 406 407 #ifdef OCTAVE_OUTPUT 408 if (oct_file){ 409 octave_fprintf_plot_output(plc_state, oct_file); 410 fclose(oct_file); 411 } 412 #endif 413 plc_state->nbf=0; 414 } 415 416 static int count_equal_samples(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * packet, uint16_t size){ 417 int count = 0; 418 int temp_count = 1; 419 int i; 420 for (i = 0; i < (size-1); i++){ 421 if (packet[i] == packet[i+1]){ 422 temp_count++; 423 continue; 424 } 425 if (count < temp_count){ 426 count = temp_count; 427 } 428 temp_count = 1; 429 } 430 if (temp_count > (count + 1)){ 431 count = temp_count; 432 } 433 return count; 434 } 435 436 static int count_zeros(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){ 437 int nr_zeros = 0; 438 int i; 439 for (i = 0; i < (size-1); i++){ 440 if (frame[i] == 0){ 441 nr_zeros++; 442 } 443 } 444 return nr_zeros; 445 } 446 447 static int zero_frame(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){ 448 return count_zeros(frame, size) == size; 449 } 450 451 // more than half the samples are the same -> bad frame 452 static int bad_frame(btstack_cvsd_plc_state_t *plc_state, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){ 453 UNUSED(plc_state); 454 return count_equal_samples(frame, size) >= (size / 2); 455 } 456 457 458 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){ 459 if (num_samples == 0) return; 460 461 plc_state->frame_count++; 462 463 if (!is_bad_frame) { 464 bool is_zero_frame = zero_frame(in, num_samples); 465 if (is_zero_frame){ 466 plc_state->zero_frames_nr++; 467 } else { 468 is_bad_frame = bad_frame(plc_state, in, num_samples); 469 } 470 } 471 472 if (is_bad_frame){ 473 (void)memcpy(out, in, num_samples * 2); 474 if (plc_state->good_samples > CVSD_LHIST){ 475 btstack_cvsd_plc_bad_frame(plc_state, num_samples, out); 476 plc_state->bad_frames_nr++; 477 } else { 478 memset(out, 0, num_samples * 2); 479 } 480 } else { 481 btstack_cvsd_plc_good_frame(plc_state, num_samples, in, out); 482 plc_state->good_frames_nr++; 483 if (plc_state->good_frames_nr == 1){ 484 log_info("First good frame at index %d\n", plc_state->frame_count-1); 485 } 486 plc_state->good_samples += num_samples; 487 } 488 } 489 490 void btstack_cvsd_dump_statistics(btstack_cvsd_plc_state_t * state){ 491 log_info("Good frames: %d\n", state->good_frames_nr); 492 log_info("Bad frames: %d\n", state->bad_frames_nr); 493 log_info("Zero frames: %d\n", state->zero_frames_nr); 494 log_info("Max Consecutive bad frames: %d\n", state->max_consecutive_bad_frames_nr); 495 } 496