Emeric Vigier | eebea67 | 2012-08-06 17:36:30 -0400 | [diff] [blame] | 1 | /* |
| 2 | ** Copyright (C) 2002-2011 Erik de Castro Lopo <erikd@mega-nerd.com> |
| 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 | ** You should have received a copy of the GNU General Public License |
| 15 | ** along with this program; if not, write to the Free Software |
| 16 | ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA. |
| 17 | */ |
| 18 | |
| 19 | #include "config.h" |
| 20 | |
| 21 | #include "util.h" |
| 22 | |
| 23 | #if (HAVE_FFTW3 == 1) |
| 24 | |
| 25 | #include <stdio.h> |
| 26 | #include <stdlib.h> |
| 27 | #include <string.h> |
| 28 | #include <math.h> |
| 29 | |
| 30 | #include <fftw3.h> |
| 31 | |
| 32 | #define MAX_SPEC_LEN (1<<18) |
| 33 | #define MAX_PEAKS 10 |
| 34 | |
| 35 | static void log_mag_spectrum (double *input, int len, double *magnitude) ; |
| 36 | static void smooth_mag_spectrum (double *magnitude, int len) ; |
| 37 | static double find_snr (const double *magnitude, int len, int expected_peaks) ; |
| 38 | |
| 39 | typedef struct |
| 40 | { double peak ; |
| 41 | int index ; |
| 42 | } PEAK_DATA ; |
| 43 | |
| 44 | double |
| 45 | calculate_snr (float *data, int len, int expected_peaks) |
| 46 | { static double magnitude [MAX_SPEC_LEN] ; |
| 47 | static double datacopy [MAX_SPEC_LEN] ; |
| 48 | |
| 49 | double snr = 200.0 ; |
| 50 | int k ; |
| 51 | |
| 52 | if (len > MAX_SPEC_LEN) |
| 53 | { printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ; |
| 54 | exit (1) ; |
| 55 | } ; |
| 56 | |
| 57 | for (k = 0 ; k < len ; k++) |
| 58 | datacopy [k] = data [k] ; |
| 59 | |
| 60 | /* Pad the data just a little to speed up the FFT. */ |
| 61 | while ((len & 0x1F) && len < MAX_SPEC_LEN) |
| 62 | { datacopy [len] = 0.0 ; |
| 63 | len ++ ; |
| 64 | } ; |
| 65 | |
| 66 | log_mag_spectrum (datacopy, len, magnitude) ; |
| 67 | smooth_mag_spectrum (magnitude, len / 2) ; |
| 68 | |
| 69 | snr = find_snr (magnitude, len, expected_peaks) ; |
| 70 | |
| 71 | return snr ; |
| 72 | } /* calculate_snr */ |
| 73 | |
| 74 | /*============================================================================== |
| 75 | ** There is a slight problem with trying to measure SNR with the method used |
| 76 | ** here; the side lobes of the windowed FFT can look like a noise/aliasing peak. |
| 77 | ** The solution is to smooth the magnitude spectrum by wiping out troughs |
| 78 | ** between adjacent peaks as done here. |
| 79 | ** This removes side lobe peaks without affecting noise/aliasing peaks. |
| 80 | */ |
| 81 | |
| 82 | static void linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) ; |
| 83 | |
| 84 | static void |
| 85 | smooth_mag_spectrum (double *mag, int len) |
| 86 | { PEAK_DATA peaks [2] ; |
| 87 | |
| 88 | int k ; |
| 89 | |
| 90 | memset (peaks, 0, sizeof (peaks)) ; |
| 91 | |
| 92 | /* Find first peak. */ |
| 93 | for (k = 1 ; k < len - 1 ; k++) |
| 94 | { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) |
| 95 | { peaks [0].peak = mag [k] ; |
| 96 | peaks [0].index = k ; |
| 97 | break ; |
| 98 | } ; |
| 99 | } ; |
| 100 | |
| 101 | /* Find subsequent peaks ans smooth between peaks. */ |
| 102 | for (k = peaks [0].index + 1 ; k < len - 1 ; k++) |
| 103 | { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) |
| 104 | { peaks [1].peak = mag [k] ; |
| 105 | peaks [1].index = k ; |
| 106 | |
| 107 | if (peaks [1].peak > peaks [0].peak) |
| 108 | linear_smooth (mag, &peaks [1], &peaks [0]) ; |
| 109 | else |
| 110 | linear_smooth (mag, &peaks [0], &peaks [1]) ; |
| 111 | peaks [0] = peaks [1] ; |
| 112 | } ; |
| 113 | } ; |
| 114 | |
| 115 | } /* smooth_mag_spectrum */ |
| 116 | |
| 117 | static void |
| 118 | linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) |
| 119 | { int k ; |
| 120 | |
| 121 | if (smaller->index < larger->index) |
| 122 | { for (k = smaller->index + 1 ; k < larger->index ; k++) |
| 123 | mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ; |
| 124 | } |
| 125 | else |
| 126 | { for (k = smaller->index - 1 ; k >= larger->index ; k--) |
| 127 | mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ; |
| 128 | } ; |
| 129 | |
| 130 | } /* linear_smooth */ |
| 131 | |
| 132 | /*============================================================================== |
| 133 | */ |
| 134 | |
| 135 | static int |
| 136 | peak_compare (const void *vp1, const void *vp2) |
| 137 | { const PEAK_DATA *peak1, *peak2 ; |
| 138 | |
| 139 | peak1 = (const PEAK_DATA*) vp1 ; |
| 140 | peak2 = (const PEAK_DATA*) vp2 ; |
| 141 | |
| 142 | return (peak1->peak < peak2->peak) ? 1 : -1 ; |
| 143 | } /* peak_compare */ |
| 144 | |
| 145 | static double |
| 146 | find_snr (const double *magnitude, int len, int expected_peaks) |
| 147 | { PEAK_DATA peaks [MAX_PEAKS] ; |
| 148 | |
| 149 | int k, peak_count = 0 ; |
| 150 | double snr ; |
| 151 | |
| 152 | memset (peaks, 0, sizeof (peaks)) ; |
| 153 | |
| 154 | /* Find the MAX_PEAKS largest peaks. */ |
| 155 | for (k = 1 ; k < len - 1 ; k++) |
| 156 | { if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1]) |
| 157 | { if (peak_count < MAX_PEAKS) |
| 158 | { peaks [peak_count].peak = magnitude [k] ; |
| 159 | peaks [peak_count].index = k ; |
| 160 | peak_count ++ ; |
| 161 | qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; |
| 162 | } |
| 163 | else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak) |
| 164 | { peaks [MAX_PEAKS - 1].peak = magnitude [k] ; |
| 165 | peaks [MAX_PEAKS - 1].index = k ; |
| 166 | qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ; |
| 167 | } ; |
| 168 | } ; |
| 169 | } ; |
| 170 | |
| 171 | if (peak_count < expected_peaks) |
| 172 | { printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ; |
| 173 | return -1.0 ; |
| 174 | } ; |
| 175 | |
| 176 | /* Sort the peaks. */ |
| 177 | qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; |
| 178 | |
| 179 | snr = peaks [0].peak ; |
| 180 | for (k = 1 ; k < peak_count ; k++) |
| 181 | if (fabs (snr - peaks [k].peak) > 10.0) |
| 182 | return fabs (peaks [k].peak) ; |
| 183 | |
| 184 | return snr ; |
| 185 | } /* find_snr */ |
| 186 | |
| 187 | static void |
| 188 | log_mag_spectrum (double *input, int len, double *magnitude) |
| 189 | { fftw_plan plan = NULL ; |
| 190 | |
| 191 | double maxval ; |
| 192 | int k ; |
| 193 | |
| 194 | if (input == NULL || magnitude == NULL) |
| 195 | return ; |
| 196 | |
| 197 | plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE | FFTW_PRESERVE_INPUT) ; |
| 198 | if (plan == NULL) |
| 199 | { printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ; |
| 200 | exit (1) ; |
| 201 | } ; |
| 202 | |
| 203 | fftw_execute (plan) ; |
| 204 | |
| 205 | fftw_destroy_plan (plan) ; |
| 206 | |
| 207 | /* (k < N/2 rounded up) */ |
| 208 | maxval = 0.0 ; |
| 209 | for (k = 1 ; k < len / 2 ; k++) |
| 210 | { magnitude [k] = sqrt (magnitude [k] * magnitude [k] + magnitude [len - k - 1] * magnitude [len - k - 1]) ; |
| 211 | maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ; |
| 212 | } ; |
| 213 | |
| 214 | memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ; |
| 215 | |
| 216 | /* Don't care about DC component. Make it zero. */ |
| 217 | magnitude [0] = 0.0 ; |
| 218 | |
| 219 | /* log magnitude. */ |
| 220 | for (k = 0 ; k < len ; k++) |
| 221 | { magnitude [k] = magnitude [k] / maxval ; |
| 222 | magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ; |
| 223 | } ; |
| 224 | |
| 225 | return ; |
| 226 | } /* log_mag_spectrum */ |
| 227 | |
| 228 | #else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */ |
| 229 | |
| 230 | double |
| 231 | calculate_snr (float *data, int len, int expected_peaks) |
| 232 | { double snr = 200.0 ; |
| 233 | |
| 234 | data = data ; |
| 235 | len = len ; |
| 236 | expected_peaks = expected_peaks ; |
| 237 | |
| 238 | return snr ; |
| 239 | } /* calculate_snr */ |
| 240 | |
| 241 | #endif |
| 242 | |