00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef _GRFFTSINK_H_
00024 #define _GRFFTSINK_H_
00025
00026 #include <VrSink.h>
00027 #include <gr_fft.h>
00028 #include "VrGUI.h"
00029 #include <algorithm>
00030 #include <stdexcept>
00031
00032 extern "C" {
00033 #include <dlfcn.h>
00034 #include <float.h>
00035 #include <math.h>
00036 }
00037
00038 #define PRINT_PEAK 0
00039
00040
00041 #define FFT_XAXIS_NAME "Frequency (Hz)"
00042 #define FFT_YAXIS_NAME "Magnitude Squared (dB)"
00043
00044
00045 template<class iType>
00046 class GrFFTSink : public VrSink<iType> {
00047
00048 public:
00049 GrFFTSink (VrGUILayout *layout,
00050 double ymin, double ymax,
00051 int nPoints = DEFAULT_nPoints,
00052 const char *label = FFT_YAXIS_NAME);
00053
00054 ~GrFFTSink();
00055
00056 virtual const char *name() { return "GrFFTSink"; }
00057
00058 virtual void initialize();
00059
00060 virtual int work3 (VrSampleRange output,
00061 VrSampleRange inputs[], void *i[]);
00062
00063
00064 static const int DEFAULT_nPoints = 1024;
00065 static const int DIVISIONS = 10;
00066
00067 private:
00068 gr_fft_complex *d_fft;
00069 double *d_xValues;
00070 double *d_dbValues;
00071 float *d_window;
00072 int d_nPoints;
00073 float d_axis_offset;
00074 VrGUIPlot *d_display;
00075 VrGUILayout *d_layout;
00076 double d_ymin, d_ymax;
00077 int d_nextPoint;
00078 int d_one_or_two;
00079 int d_increment;
00080 int d_skip_count;
00081 int d_kludge_count;
00082 const char *d_label;
00083
00084 void collectData (iType *i, long count);
00085
00086 void set_skip_count (){
00087 d_skip_count = std::max (0, d_increment - d_nPoints);
00088 }
00089 };
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 template<class iType>
00100 GrFFTSink<iType>::GrFFTSink (VrGUILayout *layout,
00101 double ymin, double ymax, int nPoints,
00102 const char *label) : d_label (label)
00103 {
00104 d_layout = layout;
00105 d_nPoints = nPoints;
00106
00107 d_fft = new gr_fft_complex (d_nPoints);
00108
00109 d_dbValues = new double[d_nPoints];
00110 d_xValues = new double[d_nPoints];
00111 d_window = new float[d_nPoints];
00112
00113 for (int i = 0; i < d_nPoints; i++)
00114 d_window[i] = 0.54 - 0.46 * cos (2*M_PI/d_nPoints*i);
00115
00116 d_ymin = ymin;
00117 d_ymax = ymax;
00118 d_nextPoint = 0;
00119 d_axis_offset = 0.0;
00120 d_kludge_count = 0;
00121 }
00122
00123 template<class iType> void
00124 GrFFTSink<iType>::initialize()
00125 {
00126 iType test_for_complex=0;
00127 double samplingFrequency = getInputSamplingFrequencyN(0);
00128
00129 if (is_complex(test_for_complex)) {
00130
00131
00132
00133 d_display = new VrGUIPlot (d_layout, FFT_XAXIS_NAME, d_label, true,
00134 0, samplingFrequency,
00135 d_ymin, d_ymax, d_nPoints, DIVISIONS);
00136 d_one_or_two = 1;
00137 for (int i = 0; i < d_nPoints; i++)
00138 d_xValues[i] =
00139 ((double)i / (double) d_nPoints * samplingFrequency) - d_axis_offset;
00140 }
00141 else {
00142 d_display = new VrGUIPlot (d_layout, FFT_XAXIS_NAME, d_label, true,
00143 0, samplingFrequency/2,
00144 d_ymin, d_ymax, d_nPoints/2, DIVISIONS);
00145 d_one_or_two = 2;
00146 for (int i = 0; i <= d_nPoints/2 ; i++)
00147 d_xValues[i] =
00148 ((double)i / (double) d_nPoints * samplingFrequency) - d_axis_offset;
00149 }
00150
00151 double updateFrequency = 30;
00152 d_increment = (int) (samplingFrequency / updateFrequency);
00153 if (d_increment < 1)
00154 d_increment = 1;
00155
00156 set_skip_count ();
00157 }
00158
00159 template<class iType> int
00160 GrFFTSink<iType>::work3(VrSampleRange output,
00161 VrSampleRange inputs[], void *ai[])
00162 {
00163 sync (output.index);
00164
00165 collectData (((iType **) ai)[0], output.size);
00166
00167 return output.size;
00168 }
00169
00170
00171
00172
00173
00174 template<class iType> void
00175 GrFFTSink<iType>::collectData(iType *in, long samples_available)
00176 {
00177 VrComplex *fft_input = d_fft->get_inbuf ();
00178 VrComplex *fft_output = d_fft->get_outbuf ();
00179
00180 while (samples_available > 0){
00181
00182 if (d_skip_count > 0){
00183 int n = std::min ((long) d_skip_count, samples_available);
00184 d_skip_count -= n;
00185
00186 in += n;
00187 samples_available -= n;
00188 continue;
00189 }
00190
00191 if (d_nextPoint < d_nPoints) {
00192 iType sampleValue;
00193
00194 sampleValue = *in++;
00195 samples_available--;
00196
00197 fft_input[d_nextPoint] = sampleValue * d_window[d_nextPoint];
00198
00199 d_nextPoint++;
00200 }
00201
00202 if (d_nextPoint >= d_nPoints) {
00203 d_nextPoint = 0;
00204 set_skip_count ();
00205
00206 d_fft->execute ();
00207
00208
00209
00210
00211
00212
00213 for (int i = 0; i < d_nPoints ; i++){
00214 d_dbValues[i] =
00215 10 * log10 (real(fft_output[i]) * real(fft_output[i])
00216 + imag(fft_output[i]) * imag(fft_output[i]));
00217 }
00218
00219 d_display->data (d_xValues, d_dbValues, d_nPoints / d_one_or_two);
00220
00221 if (PRINT_PEAK && ++d_kludge_count == 250){
00222 double dbMax = d_dbValues[0];
00223 int dbMaxIndex = 0;
00224
00225 for (int i = 1; i < d_nPoints ; i++){
00226 if (d_dbValues[i] > dbMax){
00227 dbMax = d_dbValues[i];
00228 dbMaxIndex = i;
00229 }
00230 }
00231 printf ("dbMax = %f, dbMaxIndex = %d, freq = %g\n",
00232 dbMax, dbMaxIndex,
00233 dbMaxIndex * getSamplingFrequency() / d_nPoints);
00234 d_kludge_count = 0;
00235 }
00236
00237 }
00238 }
00239 }
00240
00241 template<class iType>
00242 GrFFTSink<iType>::~GrFFTSink()
00243 {
00244 delete d_fft;
00245 delete[] d_xValues;
00246 delete[] d_dbValues;
00247 delete[] d_window;
00248 }
00249 #endif