VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-22 16:08:39 -05:00
Code Issues Projects Releases Wiki Activity

New PLL: experimental lock condition algorithm based on phi hat averaging (2) + FLL input and locking mechanixm

Browse Source
This commit is contained in:
f4exb 2018-05-16 14:20:26 +02:00
parent 10c56fc47a
commit a1a2078d7d
2 changed files with 102 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
sdrbase/dsp/phaselockcomplex.cpp
View File

@ -52,7 +52,7 @@ PhaseLockComplex::PhaseLockComplex() :
m_lockTime(2400),
m_lockTimef(2400.0f),
m_lockThreshold(4.8f),
m_avgPhi(240)
m_avgF(2400)
{
}
@ -97,8 +97,8 @@ void PhaseLockComplex::setSampleRate(unsigned int sampleRate)
m_lockTime1 = sampleRate / 100; // 10ms for order 1
m_lockTime = sampleRate / 20; // 50ms for order > 1
m_lockTimef = (float) m_lockTime;
m_lockThreshold = m_lockTime * 0.002f; // threshold of 0.002 taking division by lock time into account
m_avgPhi.resize(sampleRate / 200);
m_lockThreshold = m_lockTime * 0.00015f; // threshold of 0.002 taking division by lock time into account
m_avgF.resize(m_lockTime);
reset();
}
@ -125,6 +125,44 @@ void PhaseLockComplex::reset()
m_lockCount = 0;
}
void PhaseLockComplex::feedFLL(float re, float im)
{
std::complex<float> x(re, im);
m_phiHat1 = std::arg(x);
float dPhi = normalizeAngle(m_phiHat1 - m_phiHat2); // instantanoeus radian valued signal frequency in [-pi..pi] range
m_phiHat2 = m_phiHat1;
// advance buffer
m_v2 = m_v1; // shift center register to upper register
m_v1 = m_v0; // shift lower register to center register
// 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)
{
m_yRe = cos(m_phiHat);
@ -169,6 +207,35 @@ void PhaseLockComplex::feed(float re, float im)
// lock estimation
if (m_pskOrder > 1)
{
float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
m_avgF(dPhi);
if (m_phiHatCount < (m_lockTime-1))
{
m_phiHatCount++;
}
else
{
m_freq = m_avgF.asFloat();
float dFreq = m_freq - m_freqPrev;
if ((dFreq > -m_lockThreshold) && (dFreq < m_lockThreshold))
{
if (m_lockCount < 20) {
m_lockCount++;
}
}
else{
if (m_lockCount > 0) {
m_lockCount--;
}
}
m_freqPrev = m_freq;
m_phiHatCount = 0;
}
// m_avgPhi(m_phiHat);
// float vPhi = normalizeAngle(m_phiHat - m_avgPhi.asFloat());
//
@ -183,39 +250,38 @@ void PhaseLockComplex::feed(float re, float im)
// m_lockCount = 0;
// }
float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
if (m_phiHatCount < (m_lockTime-1))
{
m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
m_phiHatCount++;
}
else
{
float dPhi11 = (m_dPhiHatAccum - m_phiHat1); // optimized out division by lock time
float dPhi12 = (m_phiHat1 - m_phiHat2);
m_lock = dPhi11 - dPhi12; // second derivative of phase to get lock status
// if (m_phiHatCount < (m_lockTime-1))
// {
// m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
// m_phiHatCount++;
// }
// else
// {
// float dPhi11 = (m_dPhiHatAccum - m_phiHat1); // optimized out division by lock time
// float dPhi12 = (m_phiHat1 - m_phiHat2);
// m_lock = dPhi11 - dPhi12; // second derivative of phase to get lock status
if ((m_lock > -m_lockThreshold) && (m_lock < m_lockThreshold)) // includes re-multiplication by lock time
{
if (m_lockCount < 20) { // [0..20]
m_lockCount++;
}
}
else
{
if (m_lockCount > 0) {
m_lockCount -= 2;
}
}
// if ((m_lock > -m_lockThreshold) && (m_lock < m_lockThreshold)) // includes re-multiplication by lock time
// {
// if (m_lockCount < 20) { // [0..20]
// m_lockCount++;
// }
// }
// else
// {
// if (m_lockCount > 0) {
// m_lockCount -= 2;
// }
// }
m_phiHat2 = m_phiHat1;
m_phiHat1 = m_dPhiHatAccum;
m_dPhiHatAccum = 0.0f;
m_phiHatCount = 0;
}
// m_phiHat2 = m_phiHat1;
// m_phiHat1 = m_dPhiHatAccum;
// m_dPhiHatAccum = 0.0f;
// m_phiHatCount = 0;
// }
m_phiHatPrev = m_phiHat;
// m_phiHatPrev = m_phiHat;
}
else
{

5
sdrbase/dsp/phaselockcomplex.h
View File

@ -45,7 +45,10 @@ public:
/** Set sample rate information only for frequency and lock condition calculation */
void setSampleRate(unsigned int sampleRate);
void reset();
/** Feed PLL with a new signa sample */
void feed(float re, float im);
/** Same but turns into a FLL using the same filtering structure and NCO output. No lock condition. */
void feedFLL(float re, float im);
const std::complex<float>& getComplex() const { return m_y; }
float getReal() const { return m_yRe; }
float getImag() const { return m_yIm; }
@ -85,7 +88,7 @@ private:
int m_lockTime;
float m_lockTimef;
float m_lockThreshold;
MovingAverageUtilVar<float, float> m_avgPhi;
MovingAverageUtilVar<float, float> m_avgF;
};