mirror of https://github.com/f4exb/sdrangel.git
New PLL: simple FLL code to be put in its own class later
This commit is contained in:
parent
a1a2078d7d
commit
d38d926a87
|
@ -41,6 +41,7 @@ PhaseLockComplex::PhaseLockComplex() :
|
||||||
m_dPhiHatAccum(0.0),
|
m_dPhiHatAccum(0.0),
|
||||||
m_phiHatCount(0),
|
m_phiHatCount(0),
|
||||||
m_y(1.0, 0.0),
|
m_y(1.0, 0.0),
|
||||||
|
m_p(1.0, 0.0),
|
||||||
m_yRe(1.0),
|
m_yRe(1.0),
|
||||||
m_yIm(0.0),
|
m_yIm(0.0),
|
||||||
m_freq(0.0),
|
m_freq(0.0),
|
||||||
|
@ -116,7 +117,9 @@ void PhaseLockComplex::reset()
|
||||||
m_dPhiHatAccum = 0.0f;
|
m_dPhiHatAccum = 0.0f;
|
||||||
m_phiHatCount = 0;
|
m_phiHatCount = 0;
|
||||||
m_y.real(1.0);
|
m_y.real(1.0);
|
||||||
m_y.real(0.0);
|
m_y.imag(0.0);
|
||||||
|
m_p.real(1.0);
|
||||||
|
m_p.imag(0.0);
|
||||||
m_yRe = 1.0f;
|
m_yRe = 1.0f;
|
||||||
m_yIm = 0.0f;
|
m_yIm = 0.0f;
|
||||||
m_freq = 0.0f;
|
m_freq = 0.0f;
|
||||||
|
@ -127,40 +130,18 @@ void PhaseLockComplex::reset()
|
||||||
|
|
||||||
void PhaseLockComplex::feedFLL(float re, float im)
|
void PhaseLockComplex::feedFLL(float re, float im)
|
||||||
{
|
{
|
||||||
|
m_yRe = cos(m_phiHat);
|
||||||
|
m_yIm = sin(m_phiHat);
|
||||||
|
std::complex<float> y(m_yRe, m_yIm);
|
||||||
std::complex<float> x(re, im);
|
std::complex<float> x(re, im);
|
||||||
m_phiHat1 = std::arg(x);
|
std::complex<float> p = x * m_y;
|
||||||
float dPhi = normalizeAngle(m_phiHat1 - m_phiHat2); // instantanoeus radian valued signal frequency in [-pi..pi] range
|
float cross = m_p.real()*p.imag() - p.real()*m_p.imag();
|
||||||
m_phiHat2 = m_phiHat1;
|
float dot = m_p.real()*p.real() + m_p.imag()*p.imag();
|
||||||
|
float eF = cross * (dot < 0 ? -1 : 1); // frequency error
|
||||||
|
|
||||||
// advance buffer
|
m_freq += eF; // correct instantaneous frequency
|
||||||
m_v2 = m_v1; // shift center register to upper register
|
m_phiHat += eF; // advance phase with instantaneous frequency
|
||||||
m_v1 = m_v0; // shift lower register to center register
|
m_p = p; // store previous product
|
||||||
|
|
||||||
// compute new lower register
|
|
||||||
m_v0 = dPhi - m_v1*m_a1 - m_v2*m_a2;
|
|
||||||
|
|
||||||
// compute new output
|
|
||||||
float freqHat = m_v0*m_b0 + m_v1*m_b1 + m_v2*m_b2;
|
|
||||||
|
|
||||||
// prevent saturation
|
|
||||||
if (freqHat > 2.0*M_PI)
|
|
||||||
{
|
|
||||||
m_v0 *= (freqHat - 2.0*M_PI) / freqHat;
|
|
||||||
m_v1 *= (freqHat - 2.0*M_PI) / freqHat;
|
|
||||||
m_v2 *= (freqHat - 2.0*M_PI) / freqHat;
|
|
||||||
freqHat -= 2.0*M_PI;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (freqHat < -2.0*M_PI)
|
|
||||||
{
|
|
||||||
m_v0 *= (freqHat + 2.0*M_PI) / freqHat;
|
|
||||||
m_v1 *= (freqHat + 2.0*M_PI) / freqHat;
|
|
||||||
m_v2 *= (freqHat + 2.0*M_PI) / freqHat;
|
|
||||||
freqHat += 2.0*M_PI;
|
|
||||||
}
|
|
||||||
|
|
||||||
m_phiHat += freqHat; // advance phase estimate with filtered signal frequency
|
|
||||||
m_freq = freqHat / 2.0*M_PI;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PhaseLockComplex::feed(float re, float im)
|
void PhaseLockComplex::feed(float re, float im)
|
||||||
|
@ -236,21 +217,6 @@ void PhaseLockComplex::feed(float re, float im)
|
||||||
m_phiHatCount = 0;
|
m_phiHatCount = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// m_avgPhi(m_phiHat);
|
|
||||||
// float vPhi = normalizeAngle(m_phiHat - m_avgPhi.asFloat());
|
|
||||||
//
|
|
||||||
// if ((vPhi > -0.2) && (vPhi < 0.2)) // locked condition
|
|
||||||
// {
|
|
||||||
// if (m_lockCount < 20) { // [0..20]
|
|
||||||
// m_lockCount++;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// else // unlocked condition
|
|
||||||
// {
|
|
||||||
// m_lockCount = 0;
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
// if (m_phiHatCount < (m_lockTime-1))
|
// if (m_phiHatCount < (m_lockTime-1))
|
||||||
// {
|
// {
|
||||||
// m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
|
// m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
|
||||||
|
|
|
@ -77,6 +77,7 @@ private:
|
||||||
float m_dPhiHatAccum;
|
float m_dPhiHatAccum;
|
||||||
int m_phiHatCount;
|
int m_phiHatCount;
|
||||||
std::complex<float> m_y;
|
std::complex<float> m_y;
|
||||||
|
std::complex<float> m_p;
|
||||||
float m_yRe;
|
float m_yRe;
|
||||||
float m_yIm;
|
float m_yIm;
|
||||||
float m_freq;
|
float m_freq;
|
||||||
|
|
Loading…
Reference in New Issue