git.gag.com Git - debian/gnuradio/blob - ezdop/src/host/hunter/src/doppler.cc

   1 /*
   2  Copyright 2006 Johnathan Corgan.
   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 version 2
   6  as published by the Free Software Foundation.
   7
   8  This software is distributed in the hope that it will be useful,
   9  but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  GNU General Public License for more details.
  12
  13  You should have received a copy of the GNU General Public License
  14  along with GNU Radio; see the file COPYING.  If not, write to
  15  the Free Software Foundation, Inc., 51 Franklin Street,
  16  Boston, MA 02110-1301, USA.
  17 */
  18
  19 // Application level includes
  20 #include "doppler.h"
  21 #include "util.h"
  22 #include <dopctrl.h>
  23
  24 // wxWidgets includes
  25 #include <wx/log.h>
  26 #include <wx/frame.h>
  27
  28 // Boost includes
  29 #include <boost/scoped_array.hpp>
  30
  31 // System level includes
  32 #include <cmath>
  33
  34 // TODO: read from ezdop.h
  35 #define SAMPLERATE      8000
  36 #define MAXSAMPLE       0x3FF      // 12 bit ADC
  37 #define DEFAULT_SELECTED_ROTATION_RATE 2     // 500 Hz until told otherwise
  38
  39 #define QUANTUM         0.2        // Sample period in seconds
  40 #define DEFAULT_FILTER_LEVEL 20
  41
  42 #define NORMALIZEPHASE(x) \
  43         if ((x) > M_PI) \
  44             (x) -= 2*M_PI; \
  45         if ((x) < -M_PI) \
  46             (x) += 2*M_PI;
  47
  48 unsigned char rotation_rates[] = {
  49     8,      // 250 Hz
  50     5,      // 400 Hz
  51     4,      // 500 Hz
  52     3,      // 666 Hz
  53     2,      // 1000 Hz
  54     1       // 2000 Hz
  55 };
  56
  57 const wxEventType wxEVT_DOPPLER_UPDATE = wxNewEventType();
  58
  59 EZDopplerUpdate::EZDopplerUpdate(const wxEventType &event, float &in_phase,
  60                                  float &quadrature, float &volume) :
  61 wxNotifyEvent(event)
  62 {
  63     m_in_phase = in_phase;
  64     m_quadrature = quadrature;
  65     m_volume = volume;
  66 }
  67
  68 DopplerBackground::DopplerBackground(wxWindow *window, EZDoppler *doppler)
  69 {
  70     wxASSERT(window);
  71     wxASSERT(doppler);
  72
  73     m_running = false;
  74     m_dest = window;
  75     m_doppler = doppler;
  76         Create();
  77 }
  78
  79 // It's in thread.h but somehow gets undef'd
  80 typedef void *ExitCode;
  81 ExitCode DopplerBackground::Entry()
  82 {
  83     float in_phase, quadrature, phase, magnitude, volume, rflevel;
  84
  85     m_running = true;
  86         while (!TestDestroy()) {
  87             if (m_doppler->Sample(in_phase, quadrature, volume)) {
  88             EZDopplerUpdate update(wxEVT_DOPPLER_UPDATE, in_phase, quadrature, volume);
  89                 wxPostEvent(m_dest, update);
  90         }
  91         }
  92         m_running = false;
  93 }
  94
  95 EZDoppler::EZDoppler(wxWindow *gui)
  96 {
  97     wxASSERT(gui);
  98
  99     m_thread = NULL;
 100     m_gui = gui;
 101
 102     m_phase = complex<float>(0.0, 0.0);
 103     m_output = complex<float>(0.0, 0.0);
 104     m_alpha = complex<float>(0.0, 0.0);
 105     m_beta = complex<float>(0.0, 0.0);
 106
 107     m_offset = 0.0;
 108     m_angle = 0.0;
 109
 110     for(int i = 0; i < NUM_RATES; i++)
 111         m_calibration[i] = 0.0;
 112
 113     m_ezdop = ezdop_sptr(new ezdop());
 114     m_selected_rate = DEFAULT_SELECTED_ROTATION_RATE;
 115 }
 116
 117 EZDoppler::~EZDoppler()
 118 {
 119     if (m_ezdop->is_online()) {
 120         wxLogMessage(_T("EZDoppler::~EZDoppler(): doppler still online in destructor, finalizing"));
 121         Finalize();
 122     }
 123 }
 124
 125 bool EZDoppler::Initialize()
 126 {
 127     m_ezdop->init();
 128     if (m_ezdop->is_online())
 129         Reset();
 130
 131     return m_ezdop->is_online();
 132 }
 133
 134 bool EZDoppler::Finalize()
 135 {
 136     if (m_thread && m_thread->IsRunning()) {
 137         wxLogDebug(_T("EZDoppler::Finalize: finalizing a running doppler"));
 138         Stop();
 139     }
 140 }
 141
 142 bool EZDoppler::IsOnline()
 143 {
 144     return m_ezdop->is_online();
 145 }
 146
 147 bool EZDoppler::Reset()
 148 {
 149     if (m_thread && m_thread->IsRunning()) {
 150         wxLogDebug(_T("EZDoppler::Reset: resetting running doppler"));
 151         Stop();
 152     }
 153
 154     return m_ezdop->reset();
 155 }
 156
 157 bool EZDoppler::Start()
 158 {
 159     if (!(m_ezdop->rotate() && m_ezdop->stream()))
 160         return false;
 161
 162     m_thread = new DopplerBackground(m_gui, this);
 163     m_thread->Run();
 164 }
 165
 166 bool EZDoppler::Stop()
 167 {
 168     if (m_thread && m_thread->IsRunning()) {
 169         m_thread->Delete();
 170         while (m_thread->IsRunning()) {
 171             wxYield();
 172         }
 173     }
 174
 175     m_thread = NULL;
 176     return (m_ezdop->stop_streaming() && m_ezdop->stop_rotating());
 177 }
 178
 179 bool EZDoppler::SelectRotationRate(int n)
 180 {
 181     wxASSERT(n >= 0 && n < 6);
 182     wxLogDebug(_T("EZDoppler::SelectRotationRate: %i %i"), n, (int)(2000/rotation_rates[n]));
 183     m_selected_rate = n;
 184     return m_ezdop->set_rate(2000/rotation_rates[n]);
 185 }
 186
 187 int EZDoppler::GetRotationRate()
 188 {
 189     return m_selected_rate;
 190 }
 191
 192 bool EZDoppler::SetFilter(int n)
 193 {
 194     float beta = 30.0/(n*m_ezdop->rate()); // Empirically determined
 195
 196     m_alpha = complex<float>(1.0-beta, 0.0);
 197     m_beta = complex<float>(beta, 0.0);
 198
 199     return true;
 200 }
 201
 202 typedef boost::scoped_array<complex<float> > complexf_scoped_array;
 203
 204 // IQ is 2 complex floats, maximum rate is 2000, QUANTUM is period in seconds
 205 complex<float> buffer[(int)(2*QUANTUM*2000)];
 206
 207 bool EZDoppler::Sample(float &in_phase, float &quadrature, float &volume)
 208 {
 209     int nsamples = (int)(m_ezdop->rate()*QUANTUM);
 210
 211     if (!m_ezdop->read_iq(buffer, nsamples, volume))
 212         return false;
 213
 214     for (int i=0; i < nsamples; i++)
 215         m_phase = m_alpha*m_phase + m_beta*buffer[i];
 216
 217     // m_angle is the actual instrument reading regardless of calibration
 218     m_angle = atan2(m_phase.imag(), m_phase.real());
 219
 220     // Calibration angle is sum of equalized offset and global offset
 221     float cal_angle = m_calibration[m_selected_rate] + m_offset;
 222
 223     // Rotate I, Q by calibration angle
 224     complex<float> cal = complex<float>(cos(cal_angle), sin(cal_angle));
 225     m_output = m_phase*cal;
 226
 227     in_phase = m_output.real()*nsamples/512.0;
 228     quadrature = m_output.imag()*nsamples/512.0;
 229     // adjust volume
 230
 231 //  wxLogDebug(_T("%f %f %f"), in_phase, quadrature, volume);
 232     return true;
 233 }
 234
 235 bool EZDoppler::Calibrate(float phase)
 236 {
 237
 238     float offset = phase - m_angle;
 239     NORMALIZEPHASE(offset);
 240     m_calibration[m_selected_rate] = offset;
 241
 242     return true;
 243 }
 244
 245 bool EZDoppler::SetCalibration(int rate, float offset)
 246 {
 247
 248     wxASSERT(rate >= 0 && rate < 7);
 249     if (rate < 6)
 250         m_calibration[rate] = offset;
 251     else
 252         m_offset = offset;
 253
 254     return true;
 255 }
 256
 257 float EZDoppler::GetCalibration(int rate)
 258 {
 259     wxASSERT(rate >= 0 && rate < 7);
 260     if (rate < 6)
 261         return m_calibration[rate];
 262     else
 263         return m_offset;
 264
 265     return 0.0;
 266 }
 267
 268 bool EZDoppler::SetOffset(float offset)
 269 {
 270     m_offset = offset-m_angle-m_calibration[m_selected_rate];
 271     NORMALIZEPHASE(m_offset);
 272     NORMALIZEPHASE(m_offset);
 273     NORMALIZEPHASE(m_offset);
 274 }
 275
 276 bool EZDoppler::Nudge(float amount)
 277 {
 278     float cal = m_calibration[m_selected_rate];
 279     cal += amount;
 280     NORMALIZEPHASE(cal);
 281     m_calibration[m_selected_rate] = cal;
 282
 283     return true;
 284 }
 285
 286 bool EZDoppler::NudgeAll(float amount)
 287 {
 288     m_offset += amount;
 289     NORMALIZEPHASE(m_offset);
 290     return true;
 291 }