SDRAngel  4.11.5
Developer docs for <a href="https://github.com/f4exb/sdrangel">SDRangel<\a>, an Open Source Qt5 / OpenGL 3.0+ SDR and signal analyzer frontend to various hardware.
phasediscri.h
Go to the documentation of this file.
1 // Copyright (C) 2015 F4EXB //
3 // written by Edouard Griffiths //
4 // //
5 // This program is free software; you can redistribute it and/or modify //
6 // it under the terms of the GNU General Public License as published by //
7 // the Free Software Foundation as version 3 of the License, or //
8 // (at your option) any later version. //
9 // //
10 // This program is distributed in the hope that it will be useful, //
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of //
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
13 // GNU General Public License V3 for more details. //
14 // //
15 // You should have received a copy of the GNU General Public License //
16 // along with this program. If not, see <http://www.gnu.org/licenses/>. //
18 
19 #ifndef INCLUDE_DSP_PHASEDISCRI_H_
20 #define INCLUDE_DSP_PHASEDISCRI_H_
21 
22 #include "dsp/dsptypes.h"
23 
24 #undef M_PI
25 #define M_PI 3.14159265358979323846
26 
28 {
29 public:
33  void reset()
34  {
35  m_m1Sample = 0;
36  m_m2Sample = 0;
37  }
38 
42  void setFMScaling(Real fmScaling)
43  {
44  m_fmScaling = fmScaling;
45  }
46 
52  {
53  Complex d(std::conj(m_m1Sample) * sample);
54  m_m1Sample = sample;
55  return (std::atan2(d.imag(), d.real()) / M_PI) * m_fmScaling;
56  }
57 
62  Real phaseDiscriminatorDelta(const Complex& sample, double& magsq, Real& fmDev)
63  {
64  Real fltI = sample.real();
65  Real fltQ = sample.imag();
66  magsq = fltI*fltI + fltQ*fltQ;
67 
68  Real curArg = atan2_approximation2((float) fltQ, (float) fltI);
69  fmDev = (curArg - m_prevArg) / M_PI;
70  m_prevArg = curArg;
71 
72  if (fmDev < -1.0f) {
73  fmDev += 2.0f;
74  } else if (fmDev > 1.0f) {
75  fmDev -= 2.0f;
76  }
77 
78  return fmDev * m_fmScaling;
79  }
80 
87  {
88  Real ip = sample.real() - m_m2Sample.real();
89  Real qp = sample.imag() - m_m2Sample.imag();
90  Real h1 = m_m1Sample.real() * qp;
91  Real h2 = m_m1Sample.imag() * ip;
92 
94  m_m1Sample = sample;
95 
96  //return ((h1 - h2) / M_PI_2) * m_fmScaling;
97  return (h1 - h2) * m_fmScaling;
98  }
99 
103  Real phaseDiscriminator3(const Complex& sample, long double& magsq, Real& fltVal)
104  {
105  Real fltI = sample.real();
106  Real fltQ = sample.imag();
107  double fltNorm;
108  Real fltNormI;
109  Real fltNormQ;
110  //Real fltVal;
111 
112  magsq = fltI*fltI + fltQ*fltQ;
113  fltNorm = std::sqrt(magsq);
114 
115  fltNormI= fltI/fltNorm;
116  fltNormQ= fltQ/fltNorm;
117 
118  fltVal = m_fltPreviousI*(fltNormQ - m_fltPreviousQ2);
119  fltVal -= m_fltPreviousQ*(fltNormI - m_fltPreviousI2);
120  fltVal += 2.0f;
121  fltVal /= 4.0f; // normally it is /4
122 
125 
126  m_fltPreviousQ = fltNormQ;
127  m_fltPreviousI = fltNormI;
128 
129  return fltVal * m_fmScaling;
130  }
131 
132 private:
141 
142  float atan2_approximation1(float y, float x)
143  {
144  //http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html
145  //Volkan SALMA
146 
147  const float ONEQTR_PI = M_PI / 4.0;
148  const float THRQTR_PI = 3.0 * M_PI / 4.0;
149  float r, angle;
150  float abs_y = std::fabs(y) + 1e-10f; // kludge to prevent 0/0 condition
151  if ( x < 0.0f )
152  {
153  r = (x + abs_y) / (abs_y - x);
154  angle = THRQTR_PI;
155  }
156  else
157  {
158  r = (x - abs_y) / (x + abs_y);
159  angle = ONEQTR_PI;
160  }
161  angle += (0.1963f * r * r - 0.9817f) * r;
162  if ( y < 0.0f )
163  return( -angle ); // negate if in quad III or IV
164  else
165  return( angle );
166 
167 
168  }
169 
170  #define PI_FLOAT 3.14159265f
171  #define PIBY2_FLOAT 1.5707963f
172  // |error| < 0.005
173  float atan2_approximation2( float y, float x )
174  {
175  if ( x == 0.0f )
176  {
177  if ( y > 0.0f ) return PIBY2_FLOAT;
178  if ( y == 0.0f ) return 0.0f;
179  return -PIBY2_FLOAT;
180  }
181  float atan;
182  float z = y/x;
183  if ( std::fabs( z ) < 1.0f )
184  {
185  atan = z/(1.0f + 0.28f*z*z);
186  if ( x < 0.0f )
187  {
188  if ( y < 0.0f ) return atan - PI_FLOAT;
189  return atan + PI_FLOAT;
190  }
191  }
192  else
193  {
194  atan = PIBY2_FLOAT - z/(z*z + 0.28f);
195  if ( y < 0.0f ) return atan - PI_FLOAT;
196  }
197  return atan;
198  }
199 };
200 
201 #endif /* INCLUDE_DSP_PHASEDISCRI_H_ */
#define PI_FLOAT
Definition: phasediscri.h:170
void setFMScaling(Real fmScaling)
Definition: phasediscri.h:42
#define M_PI
Definition: phasediscri.h:25
float atan2_approximation2(float y, float x)
Definition: phasediscri.h:173
float atan2_approximation1(float y, float x)
Definition: phasediscri.h:142
#define PIBY2_FLOAT
Definition: phasediscri.h:171
Real phaseDiscriminator(const Complex &sample)
Definition: phasediscri.h:51
Fixed< IntType, IntBits > sqrt(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2283
Real phaseDiscriminator3(const Complex &sample, long double &magsq, Real &fltVal)
Definition: phasediscri.h:103
std::complex< Real > Complex
Definition: dsptypes.h:43
Real phaseDiscriminator2(const Complex &sample)
Definition: phasediscri.h:86
Real phaseDiscriminatorDelta(const Complex &sample, double &magsq, Real &fmDev)
Definition: phasediscri.h:62
float Real
Definition: dsptypes.h:42