analyze.c (12478B)
1 /* 2 * Copyright (C) 2013-2015 Intel Corporation 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 * 9 * This program is distributed in the hope that it will be useful, 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 * GNU General Public License for more details. 13 * 14 */ 15 16 #include <stdio.h> 17 #include <stdlib.h> 18 #include <errno.h> 19 #include <stdbool.h> 20 #include <stdint.h> 21 22 #include <math.h> 23 #include <fftw3.h> 24 25 #include "aconfig.h" 26 #include "gettext.h" 27 28 #include "common.h" 29 #include "bat-signal.h" 30 31 static void check_amplitude(struct bat *bat, float *buf) 32 { 33 float sum, average, amplitude; 34 int i, percent; 35 36 /* calculate average value */ 37 for (i = 0, sum = 0.0; i < bat->frames; i++) 38 sum += buf[i]; 39 average = sum / bat->frames; 40 41 /* calculate peak-to-average amplitude */ 42 for (i = 0, sum = 0.0; i < bat->frames; i++) 43 sum += fabsf(buf[i] - average); 44 amplitude = sum / bat->frames * M_PI / 2.0; 45 46 /* calculate amplitude percentage against full range */ 47 percent = amplitude * 100 / ((1 << ((bat->sample_size << 3) - 1)) - 1); 48 49 fprintf(bat->log, _("Amplitude: %.1f; Percentage: [%d]\n"), 50 amplitude, percent); 51 if (percent < 0) 52 fprintf(bat->err, _("ERROR: Amplitude can't be negative!\n")); 53 else if (percent < 1) 54 fprintf(bat->err, _("WARNING: Signal too weak!\n")); 55 else if (percent > 100) 56 fprintf(bat->err, _("WARNING: Signal overflow!\n")); 57 } 58 59 /** 60 * 61 * @return 0 if peak detected at right frequency, 62 * 1 if peak detected somewhere else 63 * 2 if DC detected 64 */ 65 int check_peak(struct bat *bat, struct analyze *a, int end, int peak, float hz, 66 float mean, float p, int channel, int start) 67 { 68 int err; 69 float hz_peak = (float) (peak) * hz; 70 float delta_rate = DELTA_RATE * bat->target_freq[channel]; 71 float delta_HZ = DELTA_HZ; 72 float tolerance = (delta_rate > delta_HZ) ? delta_rate : delta_HZ; 73 74 fprintf(bat->log, _("Detected peak at %2.2f Hz of %2.2f dB\n"), hz_peak, 75 10.0 * log10f(a->mag[peak] / mean)); 76 fprintf(bat->log, _(" Total %3.1f dB from %2.2f to %2.2f Hz\n"), 77 10.0 * log10f(p / mean), start * hz, end * hz); 78 79 if (hz_peak < DC_THRESHOLD) { 80 fprintf(bat->err, _(" WARNING: Found low peak %2.2f Hz,"), 81 hz_peak); 82 fprintf(bat->err, _(" very close to DC\n")); 83 err = FOUND_DC; 84 } else if (hz_peak < bat->target_freq[channel] - tolerance) { 85 fprintf(bat->err, _(" FAIL: Peak freq too low %2.2f Hz\n"), 86 hz_peak); 87 err = FOUND_WRONG_PEAK; 88 } else if (hz_peak > bat->target_freq[channel] + tolerance) { 89 fprintf(bat->err, _(" FAIL: Peak freq too high %2.2f Hz\n"), 90 hz_peak); 91 err = FOUND_WRONG_PEAK; 92 } else { 93 fprintf(bat->log, _(" PASS: Peak detected")); 94 fprintf(bat->log, _(" at target frequency\n")); 95 err = 0; 96 } 97 98 return err; 99 } 100 101 /** 102 * Search for main frequencies in fft results and compare it to target 103 */ 104 static int check(struct bat *bat, struct analyze *a, int channel) 105 { 106 float hz = 1.0 / ((float) bat->frames / (float) bat->rate); 107 float mean = 0.0, t, sigma = 0.0, p = 0.0; 108 int i, start = -1, end = -1, peak = 0, signals = 0; 109 int err = 0, N = bat->frames / 2; 110 111 /* calculate mean */ 112 for (i = 0; i < N; i++) 113 mean += a->mag[i]; 114 mean /= (float) N; 115 116 /* calculate standard deviation */ 117 for (i = 0; i < N; i++) { 118 t = a->mag[i] - mean; 119 t *= t; 120 sigma += t; 121 } 122 sigma /= (float) N; 123 sigma = sqrtf(sigma); 124 125 /* clip any data less than k sigma + mean */ 126 for (i = 0; i < N; i++) { 127 if (a->mag[i] > mean + bat->sigma_k * sigma) { 128 129 /* find peak start points */ 130 if (start == -1) { 131 start = peak = end = i; 132 signals++; 133 } else { 134 if (a->mag[i] > a->mag[peak]) 135 peak = i; 136 end = i; 137 } 138 p += a->mag[i]; 139 } else if (start != -1) { 140 /* Check if peak is as expected */ 141 err |= check_peak(bat, a, end, peak, hz, mean, 142 p, channel, start); 143 end = start = -1; 144 if (signals == MAX_PEAKS) 145 break; 146 } 147 } 148 if (signals == 0) 149 err = -ENOPEAK; /* No peak detected */ 150 else if ((err == FOUND_DC) && (signals == 1)) 151 err = -EONLYDC; /* Only DC detected */ 152 else if ((err & FOUND_WRONG_PEAK) == FOUND_WRONG_PEAK) 153 err = -EBADPEAK; /* Bad peak detected */ 154 else 155 err = 0; /* Correct peak detected */ 156 157 fprintf(bat->log, _("Detected at least %d signal(s) in total\n"), 158 signals); 159 160 return err; 161 } 162 163 static void calc_magnitude(struct bat *bat, struct analyze *a, int N) 164 { 165 float r2, i2; 166 int i; 167 168 for (i = 1; i < N / 2; i++) { 169 r2 = a->out[i] * a->out[i]; 170 i2 = a->out[N - i] * a->out[N - i]; 171 172 a->mag[i] = sqrtf(r2 + i2); 173 } 174 a->mag[0] = 0.0; 175 } 176 177 static int find_and_check_harmonics(struct bat *bat, struct analyze *a, 178 int channel) 179 { 180 fftwf_plan p; 181 int err = -ENOMEM, N = bat->frames; 182 183 /* Allocate FFT buffers */ 184 a->in = (float *) fftwf_malloc(sizeof(float) * bat->frames); 185 if (a->in == NULL) 186 goto out1; 187 188 a->out = (float *) fftwf_malloc(sizeof(float) * bat->frames); 189 if (a->out == NULL) 190 goto out2; 191 192 a->mag = (float *) fftwf_malloc(sizeof(float) * bat->frames); 193 if (a->mag == NULL) 194 goto out3; 195 196 /* create FFT plan */ 197 p = fftwf_plan_r2r_1d(N, a->in, a->out, FFTW_R2HC, 198 FFTW_MEASURE | FFTW_PRESERVE_INPUT); 199 if (p == NULL) 200 goto out4; 201 202 /* convert source PCM to floats */ 203 bat->convert_sample_to_float(a->buf, a->in, bat->frames); 204 205 /* check amplitude */ 206 check_amplitude(bat, a->in); 207 208 /* run FFT */ 209 fftwf_execute(p); 210 211 /* FFT out is real and imaginary numbers - calc magnitude for each */ 212 calc_magnitude(bat, a, N); 213 214 /* check data */ 215 err = check(bat, a, channel); 216 217 fftwf_destroy_plan(p); 218 219 out4: 220 fftwf_free(a->mag); 221 out3: 222 fftwf_free(a->out); 223 out2: 224 fftwf_free(a->in); 225 out1: 226 return err; 227 } 228 229 static int calculate_noise_one_period(struct bat *bat, 230 struct noise_analyzer *na, float *src, 231 int length, int channel) 232 { 233 int i, shift = 0; 234 float tmp, rms, gain, residual; 235 float a = 0.0, b = 1.0; 236 237 /* step 1. phase compensation */ 238 239 if (length < 2 * na->nsamples) 240 return -EINVAL; 241 242 /* search for the beginning of a sine period */ 243 for (i = 0, tmp = 0.0, shift = -1; i < na->nsamples; i++) { 244 /* find i where src[i] >= 0 && src[i+1] < 0 */ 245 if (src[i] < 0.0) 246 continue; 247 if (src[i + 1] < 0.0) { 248 tmp = src[i] - src[i + 1]; 249 a = src[i] / tmp; 250 b = -src[i + 1] / tmp; 251 shift = i; 252 break; 253 } 254 } 255 256 /* didn't find the beginning of a sine period */ 257 if (shift == -1) 258 return -EINVAL; 259 260 /* shift sine waveform to source[0] = 0.0 */ 261 for (i = 0; i < na->nsamples; i++) 262 na->source[i] = a * src[i + shift + 1] + b * src[i + shift]; 263 264 /* step 2. gain compensation */ 265 266 /* calculate rms of signal amplitude */ 267 for (i = 0, tmp = 0.0; i < na->nsamples; i++) 268 tmp += na->source[i] * na->source[i]; 269 rms = sqrtf(tmp / na->nsamples); 270 271 gain = na->rms_tgt / rms; 272 273 for (i = 0; i < na->nsamples; i++) 274 na->source[i] *= gain; 275 276 /* step 3. calculate snr in dB */ 277 278 for (i = 0, tmp = 0.0, residual = 0.0; i < na->nsamples; i++) { 279 tmp = fabsf(na->target[i] - na->source[i]); 280 residual += tmp * tmp; 281 } 282 283 tmp = na->rms_tgt / sqrtf(residual / na->nsamples); 284 na->snr_db = 20.0 * log10f(tmp); 285 286 return 0; 287 } 288 289 static int calculate_noise(struct bat *bat, float *src, int channel) 290 { 291 int err = 0; 292 struct noise_analyzer na; 293 float freq = bat->target_freq[channel]; 294 float tmp, sum_snr_pc, avg_snr_pc, avg_snr_db; 295 int offset, i, cnt_noise, cnt_clean; 296 /* num of samples in each sine period */ 297 int nsamples = (int) ceilf(bat->rate / freq); 298 /* each section has 2 sine periods, the first one for locating 299 * and the second one for noise calculating */ 300 int nsamples_per_section = nsamples * 2; 301 /* all sine periods will be calculated except the first one */ 302 int nsection = bat->frames / nsamples - 1; 303 304 fprintf(bat->log, _("samples per period: %d\n"), nsamples); 305 fprintf(bat->log, _("total sections to detect: %d\n"), nsection); 306 na.source = (float *)malloc(sizeof(float) * nsamples); 307 if (!na.source) { 308 err = -ENOMEM; 309 goto out1; 310 } 311 312 na.target = (float *)malloc(sizeof(float) * nsamples); 313 if (!na.target) { 314 err = -ENOMEM; 315 goto out2; 316 } 317 318 /* generate standard single-tone signal */ 319 err = generate_sine_wave_raw_mono(bat, na.target, freq, nsamples); 320 if (err < 0) 321 goto out3; 322 323 na.nsamples = nsamples; 324 325 /* calculate rms of standard signal */ 326 for (i = 0, tmp = 0.0; i < nsamples; i++) 327 tmp += na.target[i] * na.target[i]; 328 na.rms_tgt = sqrtf(tmp / nsamples); 329 330 /* calculate average noise level */ 331 sum_snr_pc = 0.0; 332 cnt_clean = cnt_noise = 0; 333 for (i = 0, offset = 0; i < nsection; i++) { 334 na.snr_db = SNR_DB_INVALID; 335 336 err = calculate_noise_one_period(bat, &na, src + offset, 337 nsamples_per_section, channel); 338 if (err < 0) 339 goto out3; 340 341 if (na.snr_db > bat->snr_thd_db) { 342 cnt_clean++; 343 sum_snr_pc += 100.0 / powf(10.0, na.snr_db / 20.0); 344 } else { 345 cnt_noise++; 346 } 347 offset += nsamples; 348 } 349 350 if (cnt_noise > 0) { 351 fprintf(bat->err, _("Noise detected at %d points.\n"), 352 cnt_noise); 353 err = -cnt_noise; 354 if (cnt_clean == 0) 355 goto out3; 356 } else { 357 fprintf(bat->log, _("No noise detected.\n")); 358 } 359 360 avg_snr_pc = sum_snr_pc / cnt_clean; 361 avg_snr_db = 20.0 * log10f(100.0 / avg_snr_pc); 362 fprintf(bat->log, _("Average SNR is %.2f dB (%.2f %%) at %d points.\n"), 363 avg_snr_db, avg_snr_pc, cnt_clean); 364 365 out3: 366 free(na.target); 367 out2: 368 free(na.source); 369 out1: 370 return err; 371 } 372 373 static int find_and_check_noise(struct bat *bat, void *buf, int channel) 374 { 375 int err = 0; 376 float *source; 377 378 source = (float *)malloc(sizeof(float) * bat->frames); 379 if (!source) 380 return -ENOMEM; 381 382 /* convert source PCM to floats */ 383 bat->convert_sample_to_float(buf, source, bat->frames); 384 385 /* adjust waveform and calculate noise */ 386 err = calculate_noise(bat, source, channel); 387 388 free(source); 389 return err; 390 } 391 392 /** 393 * Convert interleaved samples from channels in samples from a single channel 394 */ 395 static int reorder_data(struct bat *bat) 396 { 397 char *p, *new_bat_buf; 398 int ch, i, j; 399 400 if (bat->channels == 1) 401 return 0; /* No need for reordering */ 402 403 p = malloc(bat->frames * bat->frame_size); 404 new_bat_buf = p; 405 if (p == NULL) 406 return -ENOMEM; 407 408 for (ch = 0; ch < bat->channels; ch++) { 409 for (j = 0; j < bat->frames; j++) { 410 for (i = 0; i < bat->sample_size; i++) { 411 *p++ = ((char *) (bat->buf))[j * bat->frame_size 412 + ch * bat->sample_size + i]; 413 } 414 } 415 } 416 417 free(bat->buf); 418 bat->buf = new_bat_buf; 419 420 return 0; 421 } 422 423 /* truncate sample frames for faster FFT analysis process */ 424 static int truncate_frames(struct bat *bat) 425 { 426 int shift = SHIFT_MAX; 427 428 for (; shift > SHIFT_MIN; shift--) 429 if (bat->frames & (1 << shift)) { 430 bat->frames = 1 << shift; 431 return 0; 432 } 433 434 return -EINVAL; 435 } 436 437 int analyze_capture(struct bat *bat) 438 { 439 int err = 0; 440 size_t items; 441 int c; 442 struct analyze a; 443 444 err = truncate_frames(bat); 445 if (err < 0) { 446 fprintf(bat->err, _("Invalid frame number for analysis: %d\n"), 447 bat->frames); 448 return err; 449 } 450 451 fprintf(bat->log, _("\nBAT analysis: signal has %d frames at %d Hz,"), 452 bat->frames, bat->rate); 453 fprintf(bat->log, _(" %d channels, %d bytes per sample.\n"), 454 bat->channels, bat->sample_size); 455 456 bat->buf = malloc(bat->frames * bat->frame_size); 457 if (bat->buf == NULL) 458 return -ENOMEM; 459 460 bat->fp = fopen(bat->capture.file, "rb"); 461 err = -errno; 462 if (bat->fp == NULL) { 463 fprintf(bat->err, _("Cannot open file: %s %d\n"), 464 bat->capture.file, err); 465 goto exit1; 466 } 467 468 /* Skip header */ 469 err = read_wav_header(bat, bat->capture.file, bat->fp, true); 470 if (err != 0) 471 goto exit2; 472 473 items = fread(bat->buf, bat->frame_size, bat->frames, bat->fp); 474 if (items != bat->frames) { 475 err = -EIO; 476 goto exit2; 477 } 478 479 err = reorder_data(bat); 480 if (err != 0) 481 goto exit2; 482 483 for (c = 0; c < bat->channels; c++) { 484 fprintf(bat->log, _("\nChannel %i - "), c + 1); 485 fprintf(bat->log, _("Checking for target frequency %2.2f Hz\n"), 486 bat->target_freq[c]); 487 a.buf = bat->buf + 488 c * bat->frames * bat->frame_size 489 / bat->channels; 490 if (!bat->standalone) { 491 err = find_and_check_harmonics(bat, &a, c); 492 if (err != 0) 493 goto exit2; 494 } 495 496 if (snr_is_valid(bat->snr_thd_db)) { 497 fprintf(bat->log, _("\nChecking for SNR: ")); 498 fprintf(bat->log, _("Threshold is %.2f dB (%.2f%%)\n"), 499 bat->snr_thd_db, 100.0 500 / powf(10.0, bat->snr_thd_db / 20.0)); 501 err = find_and_check_noise(bat, a.buf, c); 502 if (err != 0) 503 goto exit2; 504 } 505 } 506 507 exit2: 508 fclose(bat->fp); 509 exit1: 510 free(bat->buf); 511 512 return err; 513 }