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 int i; 125 float sumx = 0; 126 float sumy = 0.000001f; 127 float sf; 128 129 for (i=0;i<num_samples;i++){ 130 sumx += btstack_cvsd_plc_absolute(y[CVSD_LHIST-num_samples+i]); 131 sumy += btstack_cvsd_plc_absolute(y[bestmatch+i]); 132 } 133 sf = sumx/sumy; 134 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts 135 if (sf<0.75f) sf=0.75f; 136 if (sf>1.0) sf=1.0f; 137 return sf; 138 } 139 140 BTSTACK_CVSD_PLC_SAMPLE_FORMAT btstack_cvsd_plc_crop_sample(float val){ 141 float croped_val = val; 142 if (croped_val > 32767.0) croped_val= 32767.0; 143 if (croped_val < -32768.0) croped_val=-32768.0; 144 return (BTSTACK_CVSD_PLC_SAMPLE_FORMAT) croped_val; 145 } 146 147 void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){ 148 memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t)); 149 } 150 151 #ifdef OCTAVE_OUTPUT 152 typedef enum { 153 OCTAVE_FRAME_TYPE_UNKNOWN = 0, 154 OCTAVE_FRAME_TYPE_GOOD, 155 OCTAVE_FRAME_TYPE_BAD 156 } octave_frame_type_t; 157 158 static const char * octave_frame_type_name[] = { 159 "unknown", 160 "good", 161 "bad" 162 }; 163 164 static octave_frame_type_t octave_frame_type; 165 static char octave_base_name[1000]; 166 167 const char * octave_frame_type2str(int index){ 168 if (index <= 0 || index >= sizeof(octave_frame_type_t)) return octave_frame_type_name[0]; 169 return octave_frame_type_name[index]; 170 } 171 172 void btstack_cvsd_plc_octave_set_base_name(const char * base_name){ 173 strcpy(octave_base_name, base_name); 174 printf("OCTAVE: base name set to %s\n", octave_base_name); 175 } 176 177 static void octave_fprintf_array_int16(FILE * oct_file, char * name, int data_len, int16_t * data){ 178 fprintf(oct_file, "%s = [", name); 179 int i; 180 for (i = 0; i < data_len - 1; i++){ 181 fprintf(oct_file, "%d, ", data[i]); 182 } 183 fprintf(oct_file, "%d", data[i]); 184 fprintf(oct_file, "%s", "];\n"); 185 } 186 187 static FILE * open_octave_file(btstack_cvsd_plc_state_t *plc_state, octave_frame_type_t frame_type){ 188 char oct_file_name[1200]; 189 octave_frame_type = frame_type; 190 sprintf(oct_file_name, "%s_octave_plc_%d_%s.m", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); 191 192 FILE * oct_file = fopen(oct_file_name, "wb"); 193 if (oct_file == NULL){ 194 printf("OCTAVE: could not open file %s\n", oct_file_name); 195 return NULL; 196 } 197 printf("OCTAVE: opened file %s\n", oct_file_name); 198 return oct_file; 199 } 200 201 static void octave_fprintf_plot_history_frame(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file, int frame_nr){ 202 char title[100]; 203 char hist_name[10]; 204 sprintf(hist_name, "hist%d", plc_state->nbf); 205 206 octave_fprintf_array_int16(oct_file, hist_name, CVSD_LHIST, plc_state->hist); 207 208 fprintf(oct_file, "y = [min(%s):1000:max(%s)];\n", hist_name, hist_name); 209 fprintf(oct_file, "x = zeros(1, size(y,2));\n"); 210 fprintf(oct_file, "b = [0: %d];\n", CVSD_LHIST+CVSD_FS+CVSD_RT+CVSD_OLAL); 211 212 int pos = CVSD_FS; 213 fprintf(oct_file, "shift_x = x + %d;\n", pos); 214 215 pos = CVSD_LHIST - 1; 216 fprintf(oct_file, "lhist_x = x + %d;\n", pos); 217 pos += CVSD_OLAL; 218 fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos); 219 pos += CVSD_FS - CVSD_OLAL; 220 fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos); 221 pos += CVSD_OLAL; 222 fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos); 223 pos += CVSD_RT; 224 fprintf(oct_file, "lhist_rt_x = x + %d;\n", pos); 225 226 fprintf(oct_file, "pattern_window_x = x + %d;\n", CVSD_LHIST - CVSD_M); 227 228 fprintf(oct_file, "hf = figure();\n"); 229 sprintf(title, "PLC %s frame %d", octave_frame_type2str(octave_frame_type), frame_nr); 230 231 fprintf(oct_file, "hold on;\n"); 232 fprintf(oct_file, "h1 = plot(%s); \n", hist_name); 233 234 fprintf(oct_file, "title(\"%s\");\n", title); 235 236 fprintf(oct_file, "plot(lhist_x, y, 'k'); \n"); 237 fprintf(oct_file, "text(max(lhist_x) - 10, max(y)+1000, 'lhist'); \n"); 238 239 fprintf(oct_file, "plot(lhist_olal1_x, y, 'k'); \n"); 240 fprintf(oct_file, "text(max(lhist_olal1_x) - 10, max(y)+1000, 'OLAL'); \n"); 241 242 fprintf(oct_file, "plot(lhist_fs_x, y, 'k'); \n"); 243 fprintf(oct_file, "text(max(lhist_fs_x) - 10, max(y)+1000, 'FS'); \n"); 244 245 fprintf(oct_file, "plot(lhist_olal2_x, y, 'k'); \n"); 246 fprintf(oct_file, "text(max(lhist_olal2_x) - 10, max(y)+1000, 'OLAL'); \n"); 247 248 fprintf(oct_file, "plot(lhist_rt_x, y, 'k');\n"); 249 fprintf(oct_file, "text(max(lhist_rt_x) - 10, max(y)+1000, 'RT'); \n"); 250 251 if (octave_frame_type == OCTAVE_FRAME_TYPE_GOOD) return; 252 253 int x0 = plc_state->bestlag; 254 int x1 = plc_state->bestlag + CVSD_M - 1; 255 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); 256 fprintf(oct_file, "text(%d - 10, -10, 'bestlag'); \n", x0); 257 258 x0 = plc_state->bestlag + CVSD_M ; 259 x1 = plc_state->bestlag + CVSD_M + CVSD_FS - 1; 260 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'kd'); \n", x0, x1, hist_name, x0, x1); 261 262 x0 = CVSD_LHIST - CVSD_M; 263 x1 = CVSD_LHIST - 1; 264 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1); 265 fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n"); 266 fprintf(oct_file, "text(max(pattern_window_x) - 10, max(y)+1000, 'M'); \n"); 267 } 268 269 static void octave_fprintf_plot_output(btstack_cvsd_plc_state_t *plc_state, FILE * oct_file){ 270 if (!oct_file) return; 271 char out_name[10]; 272 sprintf(out_name, "out%d", plc_state->nbf); 273 int x0 = CVSD_LHIST; 274 int x1 = x0 + CVSD_FS - 1; 275 octave_fprintf_array_int16(oct_file, out_name, CVSD_FS, plc_state->hist+x0); 276 fprintf(oct_file, "h2 = plot(b(%d:%d), %s, 'cd'); \n", x0, x1, out_name); 277 278 char rest_hist_name[10]; 279 sprintf(rest_hist_name, "rest%d", plc_state->nbf); 280 x0 = CVSD_LHIST + CVSD_FS; 281 x1 = x0 + CVSD_OLAL + CVSD_RT - 1; 282 octave_fprintf_array_int16(oct_file, rest_hist_name, CVSD_OLAL + CVSD_RT, plc_state->hist+x0); 283 fprintf(oct_file, "h3 = plot(b(%d:%d), %s, 'kd'); \n", x0, x1, rest_hist_name); 284 285 char new_hist_name[10]; 286 sprintf(new_hist_name, "hist%d", plc_state->nbf); 287 octave_fprintf_array_int16(oct_file, new_hist_name, CVSD_LHIST, plc_state->hist); 288 fprintf(oct_file, "h4 = plot(%s, 'r--'); \n", new_hist_name); 289 290 fprintf(oct_file, "legend ([h1, h2, h3, h4], {\"hist\", \"out\", \"rest\", \"new hist\"}, \"location\", \"northeast\");\n "); 291 292 char fig_name[1200]; 293 sprintf(fig_name, "../%s_octave_plc_%d_%s", octave_base_name, plc_state->frame_count, octave_frame_type2str(octave_frame_type)); 294 fprintf(oct_file, "print(hf, \"%s.jpg\", \"-djpg\");", fig_name); 295 } 296 #endif 297 298 void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT *out){ 299 float val; 300 int i = 0; 301 float sf = 1; 302 plc_state->nbf++; 303 304 if (plc_state->max_consecutive_bad_frames_nr < plc_state->nbf){ 305 plc_state->max_consecutive_bad_frames_nr = plc_state->nbf; 306 } 307 if (plc_state->nbf==1){ 308 // printf("first bad frame\n"); 309 // Perform pattern matching to find where to replicate 310 plc_state->bestlag = btstack_cvsd_plc_pattern_match(plc_state->hist); 311 } 312 313 #ifdef OCTAVE_OUTPUT 314 FILE * oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_BAD); 315 if (oct_file){ 316 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); 317 } 318 #endif 319 320 if (plc_state->nbf==1){ 321 // the replication begins after the template match 322 plc_state->bestlag += CVSD_M; 323 324 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet 325 sf = btstack_cvsd_plc_amplitude_match(plc_state, num_samples, plc_state->hist, plc_state->bestlag); 326 for (i=0;i<CVSD_OLAL;i++){ 327 val = sf*plc_state->hist[plc_state->bestlag+i]; 328 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 329 } 330 331 for (;i<num_samples;i++){ 332 val = sf*plc_state->hist[plc_state->bestlag+i]; 333 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 334 } 335 336 for (;i<num_samples+CVSD_OLAL;i++){ 337 float left = sf*plc_state->hist[plc_state->bestlag+i]; 338 float right = plc_state->hist[plc_state->bestlag+i]; 339 val = left*rcos[i-num_samples] + right*rcos[CVSD_OLAL-1-i+num_samples]; 340 plc_state->hist[CVSD_LHIST+i] = btstack_cvsd_plc_crop_sample(val); 341 } 342 343 for (;i<num_samples+CVSD_RT+CVSD_OLAL;i++){ 344 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 345 } 346 } else { 347 for (;i<num_samples+CVSD_RT+CVSD_OLAL;i++){ 348 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 349 } 350 } 351 352 for (i=0;i<num_samples;i++){ 353 out[i] = plc_state->hist[CVSD_LHIST+i]; 354 } 355 356 // shift the history buffer 357 for (i=0;i<CVSD_LHIST+CVSD_RT+CVSD_OLAL;i++){ 358 plc_state->hist[i] = plc_state->hist[i+num_samples]; 359 } 360 361 #ifdef OCTAVE_OUTPUT 362 if (oct_file){ 363 octave_fprintf_plot_output(plc_state, oct_file); 364 fclose(oct_file); 365 } 366 #endif 367 } 368 369 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){ 370 float val; 371 int i = 0; 372 #ifdef OCTAVE_OUTPUT 373 FILE * oct_file = NULL; 374 if (plc_state->nbf>0){ 375 oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD); 376 if (oct_file){ 377 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count); 378 } 379 } 380 #endif 381 if (plc_state->nbf>0){ 382 for (i=0;i<CVSD_RT;i++){ 383 out[i] = plc_state->hist[CVSD_LHIST+i]; 384 } 385 386 for (i=CVSD_RT;i<CVSD_RT+CVSD_OLAL;i++){ 387 float left = plc_state->hist[CVSD_LHIST+i]; 388 float right = in[i]; 389 val = left * rcos[i-CVSD_RT] + right *rcos[CVSD_OLAL+CVSD_RT-1-i]; 390 out[i] = (BTSTACK_CVSD_PLC_SAMPLE_FORMAT)val; 391 } 392 } 393 394 for (;i<num_samples;i++){ 395 out[i] = in[i]; 396 } 397 // Copy the output to the history buffer 398 for (i=0;i<num_samples;i++){ 399 plc_state->hist[CVSD_LHIST+i] = out[i]; 400 } 401 // shift the history buffer 402 for (i=0;i<CVSD_LHIST;i++){ 403 plc_state->hist[i] = plc_state->hist[i+num_samples]; 404 } 405 406 #ifdef OCTAVE_OUTPUT 407 if (oct_file){ 408 octave_fprintf_plot_output(plc_state, oct_file); 409 fclose(oct_file); 410 } 411 #endif 412 plc_state->nbf=0; 413 } 414 415 static int count_equal_samples(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * packet, uint16_t size){ 416 int count = 0; 417 int temp_count = 1; 418 int i; 419 for (i = 0; i < size-1; i++){ 420 if (packet[i] == packet[i+1]){ 421 temp_count++; 422 continue; 423 } 424 if (count < temp_count){ 425 count = temp_count; 426 } 427 temp_count = 1; 428 } 429 if (temp_count > count + 1){ 430 count = temp_count; 431 } 432 return count; 433 } 434 435 static int count_zeros(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){ 436 int nr_zeros = 0; 437 int i; 438 for (i = 0; i < size-1; i++){ 439 if (frame[i] == 0){ 440 nr_zeros++; 441 } 442 } 443 return nr_zeros; 444 } 445 446 // note: a zero_frame is currently also a 'bad_frame' 447 static int zero_frame(BTSTACK_CVSD_PLC_SAMPLE_FORMAT * frame, uint16_t size){ 448 return count_zeros(frame, size) > (size/2); 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 return count_equal_samples(frame, size) > size / 2; 454 } 455 456 457 void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * plc_state, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * in, uint16_t num_samples, BTSTACK_CVSD_PLC_SAMPLE_FORMAT * out){ 458 if (num_samples == 0) return; 459 460 plc_state->frame_count++; 461 462 int is_zero_frame = zero_frame(in, num_samples); 463 int is_bad_frame = bad_frame(plc_state, in, num_samples); 464 465 if (is_bad_frame){ 466 memcpy(out, in, num_samples * 2); 467 if (plc_state->good_samples > CVSD_LHIST){ 468 btstack_cvsd_plc_bad_frame(plc_state, num_samples, out); 469 if (is_zero_frame){ 470 plc_state->zero_frames_nr++; 471 } else { 472 plc_state->bad_frames_nr++; 473 } 474 } else { 475 memset(out, 0, num_samples * 2); 476 } 477 } else { 478 btstack_cvsd_plc_good_frame(plc_state, num_samples, in, out); 479 plc_state->good_frames_nr++; 480 if (plc_state->good_frames_nr == 1){ 481 log_info("First good frame at index %d\n", plc_state->frame_count-1); 482 } 483 plc_state->good_samples += num_samples; 484 } 485 } 486 487 void btstack_cvsd_dump_statistics(btstack_cvsd_plc_state_t * state){ 488 log_info("Good frames: %d\n", state->good_frames_nr); 489 log_info("Bad frames: %d\n", state->bad_frames_nr); 490 log_info("Zero frames: %d\n", state->zero_frames_nr); 491 log_info("Max Consecutive bad frames: %d\n", state->max_consecutive_bad_frames_nr); 492 } 493