tarina

git clone https://git.tarina.org/tarina
Log | Files | Refs | README | LICENSE

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 }