bat/analyze.c

   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
  30 static void check_amplitude(struct bat *bat, double *buf)
  31 {
  32         double sum, average, amplitude;
  33         int i, percent;
  34
  35         /* calculate average value */
  36         for (i = 0, sum = 0.0; i < bat->frames; i++)
  37                 sum += buf[i];
  38         average = sum / bat->frames;
  39
  40         /* calculate peak-to-average amplitude */
  41         for (i = 0, sum = 0.0; i < bat->frames; i++)
  42                 sum += abs(buf[i] - average);
  43         amplitude = sum / bat->frames * M_PI / 2.0;
  44
  45         /* calculate amplitude percentage against full range */
  46         percent = amplitude * 100 / ((1 << ((bat->sample_size << 3) - 1)) - 1);
  47
  48         fprintf(bat->log, _("Amplitude: %.1f; Percentage: [%d]\n"),
  49                         amplitude, percent);
  50         if (percent < 0)
  51                 fprintf(bat->err, _("ERROR: Amplitude can't be negative!\n"));
  52         else if (percent < 1)
  53                 fprintf(bat->err, _("WARNING: Signal too weak!\n"));
  54         else if (percent > 100)
  55                 fprintf(bat->err, _("WARNING: Signal overflow!\n"));
  56 }
  57
  58 /**
  59  *
  60  * @return 0 if peak detected at right frequency,
  61  *         1 if peak detected somewhere else
  62  *         2 if DC detected
  63  */
  64 int check_peak(struct bat *bat, struct analyze *a, int end, int peak, float hz,
  65                 float mean, float p, int channel, int start)
  66 {
  67         int err;
  68         float hz_peak = (float) (peak) * hz;
  69         float delta_rate = DELTA_RATE * bat->target_freq[channel];
  70         float delta_HZ = DELTA_HZ;
  71         float tolerance = (delta_rate > delta_HZ) ? delta_rate : delta_HZ;
  72
  73         fprintf(bat->log, _("Detected peak at %2.2f Hz of %2.2f dB\n"), hz_peak,
  74                         10.0 * log10(a->mag[peak] / mean));
  75         fprintf(bat->log, _(" Total %3.1f dB from %2.2f to %2.2f Hz\n"),
  76                         10.0 * log10(p / mean), start * hz, end * hz);
  77
  78         if (hz_peak < DC_THRESHOLD) {
  79                 fprintf(bat->err, _(" WARNING: Found low peak %2.2f Hz,"),
  80                                 hz_peak);
  81                 fprintf(bat->err, _(" very close to DC\n"));
  82                 err = FOUND_DC;
  83         } else if (hz_peak < bat->target_freq[channel] - tolerance) {
  84                 fprintf(bat->err, _(" FAIL: Peak freq too low %2.2f Hz\n"),
  85                                 hz_peak);
  86                 err = FOUND_WRONG_PEAK;
  87         } else if (hz_peak > bat->target_freq[channel] + tolerance) {
  88                 fprintf(bat->err, _(" FAIL: Peak freq too high %2.2f Hz\n"),
  89                                 hz_peak);
  90                 err = FOUND_WRONG_PEAK;
  91         } else {
  92                 fprintf(bat->log, _(" PASS: Peak detected"));
  93                 fprintf(bat->log, _(" at target frequency\n"));
  94                 err = 0;
  95         }
  96
  97         return err;
  98 }
  99
 100 /**
 101  * Search for main frequencies in fft results and compare it to target
 102  */
 103 static int check(struct bat *bat, struct analyze *a, int channel)
 104 {
 105         float hz = 1.0 / ((float) bat->frames / (float) bat->rate);
 106         float mean = 0.0, t, sigma = 0.0, p = 0.0;
 107         int i, start = -1, end = -1, peak = 0, signals = 0;
 108         int err = 0, N = bat->frames / 2;
 109
 110         /* calculate mean */
 111         for (i = 0; i < N; i++)
 112                 mean += a->mag[i];
 113         mean /= (float) N;
 114
 115         /* calculate standard deviation */
 116         for (i = 0; i < N; i++) {
 117                 t = a->mag[i] - mean;
 118                 t *= t;
 119                 sigma += t;
 120         }
 121         sigma /= (float) N;
 122         sigma = sqrtf(sigma);
 123
 124         /* clip any data less than k sigma + mean */
 125         for (i = 0; i < N; i++) {
 126                 if (a->mag[i] > mean + bat->sigma_k * sigma) {
 127
 128                         /* find peak start points */
 129                         if (start == -1) {
 130                                 start = peak = end = i;
 131                                 signals++;
 132                         } else {
 133                                 if (a->mag[i] > a->mag[peak])
 134                                         peak = i;
 135                                 end = i;
 136                         }
 137                         p += a->mag[i];
 138                 } else if (start != -1) {
 139                         /* Check if peak is as expected */
 140                         err |= check_peak(bat, a, end, peak, hz, mean,
 141                                         p, channel, start);
 142                         end = start = -1;
 143                         if (signals == MAX_PEAKS)
 144                                 break;
 145                 }
 146         }
 147         if (signals == 0)
 148                 err = -ENOPEAK; /* No peak detected */
 149         else if ((err == FOUND_DC) && (signals == 1))
 150                 err = -EONLYDC; /* Only DC detected */
 151         else if ((err & FOUND_WRONG_PEAK) == FOUND_WRONG_PEAK)
 152                 err = -EBADPEAK; /* Bad peak detected */
 153         else
 154                 err = 0; /* Correct peak detected */
 155
 156         fprintf(bat->log, _("Detected at least %d signal(s) in total\n"),
 157                         signals);
 158
 159         return err;
 160 }
 161
 162 static void calc_magnitude(struct bat *bat, struct analyze *a, int N)
 163 {
 164         double r2, i2;
 165         int i;
 166
 167         for (i = 1; i < N / 2; i++) {
 168                 r2 = a->out[i] * a->out[i];
 169                 i2 = a->out[N - i] * a->out[N - i];
 170
 171                 a->mag[i] = sqrtf(r2 + i2);
 172         }
 173         a->mag[0] = 0.0;
 174 }
 175
 176 static int find_and_check_harmonics(struct bat *bat, struct analyze *a,
 177                 int channel)
 178 {
 179         fftw_plan p;
 180         int err = -ENOMEM, N = bat->frames;
 181
 182         /* Allocate FFT buffers */
 183         a->in = (double *) fftw_malloc(sizeof(double) * bat->frames);
 184         if (a->in == NULL)
 185                 goto out1;
 186
 187         a->out = (double *) fftw_malloc(sizeof(double) * bat->frames);
 188         if (a->out == NULL)
 189                 goto out2;
 190
 191         a->mag = (double *) fftw_malloc(sizeof(double) * bat->frames);
 192         if (a->mag == NULL)
 193                 goto out3;
 194
 195         /* create FFT plan */
 196         p = fftw_plan_r2r_1d(N, a->in, a->out, FFTW_R2HC,
 197                         FFTW_MEASURE | FFTW_PRESERVE_INPUT);
 198         if (p == NULL)
 199                 goto out4;
 200
 201         /* convert source PCM to doubles */
 202         bat->convert_sample_to_double(a->buf, a->in, bat->frames);
 203
 204         /* check amplitude */
 205         check_amplitude(bat, a->in);
 206
 207         /* run FFT */
 208         fftw_execute(p);
 209
 210         /* FFT out is real and imaginary numbers - calc magnitude for each */
 211         calc_magnitude(bat, a, N);
 212
 213         /* check data */
 214         err = check(bat, a, channel);
 215
 216         fftw_destroy_plan(p);
 217
 218 out4:
 219         fftw_free(a->mag);
 220 out3:
 221         fftw_free(a->out);
 222 out2:
 223         fftw_free(a->in);
 224 out1:
 225         return err;
 226 }
 227
 228 /**
 229  * Convert interleaved samples from channels in samples from a single channel
 230  */
 231 static int reorder_data(struct bat *bat)
 232 {
 233         char *p, *new_bat_buf;
 234         int ch, i, j;
 235
 236         if (bat->channels == 1)
 237                 return 0; /* No need for reordering */
 238
 239         p = malloc(bat->frames * bat->frame_size);
 240         new_bat_buf = p;
 241         if (p == NULL)
 242                 return -ENOMEM;
 243
 244         for (ch = 0; ch < bat->channels; ch++) {
 245                 for (j = 0; j < bat->frames; j++) {
 246                         for (i = 0; i < bat->sample_size; i++) {
 247                                 *p++ = ((char *) (bat->buf))[j * bat->frame_size
 248                                                 + ch * bat->sample_size + i];
 249                         }
 250                 }
 251         }
 252
 253         free(bat->buf);
 254         bat->buf = new_bat_buf;
 255
 256         return 0;
 257 }
 258
 259 /* truncate sample frames for faster FFT analysis process */
 260 static int truncate_frames(struct bat *bat)
 261 {
 262         int shift = SHIFT_MAX;
 263
 264         for (; shift > SHIFT_MIN; shift--)
 265                 if (bat->frames & (1 << shift)) {
 266                         bat->frames = 1 << shift;
 267                         return 0;
 268                 }
 269
 270         return -EINVAL;
 271 }
 272
 273 int analyze_capture(struct bat *bat)
 274 {
 275         int err = 0;
 276         size_t items;
 277         int c;
 278         struct analyze a;
 279
 280         err = truncate_frames(bat);
 281         if (err < 0) {
 282                 fprintf(bat->err, _("Invalid frame number for analysis: %d\n"),
 283                                 bat->frames);
 284                 return err;
 285         }
 286
 287         fprintf(bat->log, _("\nBAT analysis: signal has %d frames at %d Hz,"),
 288                         bat->frames, bat->rate);
 289         fprintf(bat->log, _(" %d channels, %d bytes per sample.\n"),
 290                         bat->channels, bat->sample_size);
 291
 292         bat->buf = malloc(bat->frames * bat->frame_size);
 293         if (bat->buf == NULL)
 294                 return -ENOMEM;
 295
 296         bat->fp = fopen(bat->capture.file, "rb");
 297         err = -errno;
 298         if (bat->fp == NULL) {
 299                 fprintf(bat->err, _("Cannot open file: %s %d\n"),
 300                                 bat->capture.file, err);
 301                 goto exit1;
 302         }
 303
 304         /* Skip header */
 305         err = read_wav_header(bat, bat->capture.file, bat->fp, true);
 306         if (err != 0)
 307                 goto exit2;
 308
 309         items = fread(bat->buf, bat->frame_size, bat->frames, bat->fp);
 310         if (items != bat->frames) {
 311                 err = -EIO;
 312                 goto exit2;
 313         }
 314
 315         err = reorder_data(bat);
 316         if (err != 0)
 317                 goto exit2;
 318
 319         for (c = 0; c < bat->channels; c++) {
 320                 fprintf(bat->log, _("\nChannel %i - "), c + 1);
 321                 fprintf(bat->log, _("Checking for target frequency %2.2f Hz\n"),
 322                                 bat->target_freq[c]);
 323                 a.buf = bat->buf +
 324                                 c * bat->frames * bat->frame_size
 325                                 / bat->channels;
 326                 err = find_and_check_harmonics(bat, &a, c);
 327         }
 328
 329 exit2:
 330         fclose(bat->fp);
 331 exit1:
 332         free(bat->buf);
 333
 334         return err;
 335 }