/// param localReplica the code/carrier that this object is to track
/// param codeSpacing the correlator spacing (in sec) that will be used for
/// the code. This class will quantize this value to the closest number
/// of ticks.
EMLTracker::EMLTracker(CCReplica& localReplica, double codeSpacing) :
GenericTracker(localReplica),
ticksPerChip(static_cast<unsigned>(1.0/localReplica.chipsPerTick)),
eplSpacing(static_cast<unsigned>((codeSpacing / localReplica.tickSize))),pllError(0), pllAlpha(/*0.2*/0.1), pllBeta(/*0.05*/0.025),
dllError(0), dllAlpha(/*6*/3), dllBeta(/*0.01*/0.005),
iadCount(0), nav(false), baseGain(1.0/(0.1767*1.404)),
inSumSq(0), lrSumSq(0),iadThreshold(0.02),
dllMode(dmFar), pllMode(pmUnlocked), navChange(true), prevNav(true),periodCount(10),prn(0)
{
early.setDelay(2*eplSpacing);
prompt.setDelay(eplSpacing);
late.setDelay(0);
// Since our 'prompt' code is really a late code we should really advance
// our local replica by this amount but not have it count as part of our
// code phase offset.
correlatorBias = eplSpacing * localReplica.chipsPerTick;
localReplica.moveCodePhase(correlatorBias);
localReplica.codePhaseOffset -= correlatorBias;
// Walk the code by the correlator spacing
searchSize = eplSpacing * localReplica.tickSize/localReplica.codeChipLen;
iadCountMax = static_cast<unsigned long>(
localReplica.codeGenPtr->getSyncIndex() / localReplica.chipsPerTick);
iadCountDefault = iadCountMax;
};
bool RxSim::initialize(int argc, char *argv[]) throw()
{
using namespace gpstk::StringUtils;
CommandOptionWithAnyArg
codeOpt('c', "code",
"The code/carrier to track. ARG takes the form of "
"code:carrier:prn:offset:doppler. Code is either c or p. "
"Carrier is either 1 or 2. Prn is an integer between 1 and 32. "
"Offset is a number in us, Doppler is a number in Hz. Currently, "
"only one signal can be specified. For example, to track P code "
"on L2 for PRN 3, with no initial time or doppler offset, "
"specify -c p:2:3:0:0"),
dllAlphaOpt('\0', "dllAlpha",
"The gain on the phase update for the code tracker. The "
"default is 1e-5 chips/tick"),
dllBetaOpt('\0', "dllBeta",
"The gain on the frequency update for the code tracker. The "
"default is 1e-12 chips/tick"),
pllAlphaOpt('\0', "pllAlpha",
"The gain on the phase update for the carrier tracker. The "
"default is 0.4 cycles/tick"),
pllBetaOpt('\0', "pllBeta",
"The gain on the frequency update for the carrier tracker. "
"The default is 0.1 cycles / iad_period"),
sampleRateOpt('r',"sample-rate",
"Specifies the nominal sample rate, in MHz. The "
"default is 20 MHz."),
interFreqOpt('x',"inter-freq",
"Specifies the intermediate frequency of the receiver,"
" in MHz. Default is 0.42 MHz. If there is no down-"
"conversion, the IF should be the L1 or L2 carrier"
" frequency" ),
quantizationOpt('q', "quantization",
"They quantization applied to the data. 1, 2 or f. "
"The default is f."),
gainOpt('g', "gain",
"Gain to apply to the if prior to digitization, in dB. Default is 0."),
timeLimitOpt('t', "time-limit",
"Limit the amount of data to process. Specify time in ms. Defaults to all data."),
inputOpt('i', "input",
"Where to get the IQ samples from. The default is to use stdin.");
CommandOptionWithNumberArg
bandsOpt('b', "bands",
"The number of complex samples per epoch. The default is 2.");
if (!BasicFramework::initialize(argc,argv))
return false;
if (timeLimitOpt.getCount())
timeLimit = asDouble(timeLimitOpt.getValue()[0]) * 1e-3;
if (!codeOpt.getCount())
{
cout << "Must specify a code/carrier to track. Bye." << endl;
return false;
}
string val=codeOpt.getValue()[0];
const char delim(':');
if (numWords(val, delim) != 5)
{
cout << "Error in code parameter:" << val << endl;
return false;
}
string code = lowerCase(word(val, 0, delim));
band = asInt(word(val, 1, delim));
int prn = asInt(word(val, 2, delim));
double offset = asDouble(word(val, 3, delim)) * 1e-6;
double doppler = asDouble(word(val, 4, delim));
CodeGenerator* codeGenPtr;
double chipFreq;
switch (code[0])
{
case 'c':
codeGenPtr = new CACodeGenerator(prn);
chipFreq = CA_CHIP_FREQ_GPS;
break;
case 'p':
codeGenPtr = new PCodeGenerator(prn);
chipFreq = PY_CHIP_FREQ_GPS;
break;
default:
cout << "Unsupported code: " << code << endl;
return false;
}
if (sampleRateOpt.getCount())
timeStep = 1/(asDouble(sampleRateOpt.getValue().front()) * 1e6 );
if (interFreqOpt.getCount())
interFreq = asDouble(interFreqOpt.getValue().front()) * 1e6;
// Note that this object is responsible for destroying
// the codeGenPtr object
cc = new CCReplica(timeStep, chipFreq, interFreq, codeGenPtr);
double chips = offset / cc->codeChipLen;
cc->moveCodePhase(chips);
cc->setCodeFreqOffsetHz(doppler);
cc->setCarrierFreqOffsetHz(doppler);
double spacing = 0.5 * cc->codeChipLen;
if (spacing < timeStep)
spacing = timeStep;
tr = new EMLTracker(*cc, spacing);
if (dllAlphaOpt.getCount())
tr->dllAlpha = asDouble(dllAlphaOpt.getValue()[0]);
if (dllBetaOpt.getCount())
tr->dllBeta = asDouble(dllBetaOpt.getValue()[0]);
if (pllAlphaOpt.getCount())
tr->pllAlpha = asDouble(pllAlphaOpt.getValue()[0]);
if (pllBetaOpt.getCount())
tr->pllBeta = asDouble(pllBetaOpt.getValue()[0]);
tr->debugLevel = debugLevel;
char quantization='f';
if (quantizationOpt.getCount())
quantization = quantizationOpt.getValue()[0][0];
switch (quantization)
{
case '1': input = new IQ1Stream(); break;
case '2': input = new IQ2Stream(); break;
case 'f':
default: input = new IQFloatStream(); break;
}
if (inputOpt.getCount())
{
input->open(inputOpt.getValue()[0].c_str());
}
else
{
using std::basic_ios;
input->copyfmt(std::cin);
input->clear(std::cin.rdstate());
input->basic_ios<char>::rdbuf(std::cin.rdbuf());
input->filename = "<stdin>";
}
if (bandsOpt.getCount())
input->bands = asInt(bandsOpt.getValue()[0]);
if (gainOpt.getCount())
{
double gainDb = StringUtils::asDouble(gainOpt.getValue()[0]);
gain = exp10(gainDb/10.);
}
if (verboseLevel)
{
cout << "# Taking input from " << input->filename
<< " (" << input->bands << " samples/epoch)" << endl
<< "# Rx gain level: " << gain << endl;
tr->dump(cout, 1);
}
return true;
}
//-----------------------------------------------------------------------------
void Acquire::process()
{
// Set up all arrays:
// fftw_complex data type is a double[2] where the 0 element is the real
// part and the 1 element is the imaginary part.
// Local code replicas in time domain
vector< fftw_complex* > l(bins);
for(int i=0;i<bins;i++)
{
fftw_complex* v;
v = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
l[i] = v;
}
// Input data in time domain
fftw_complex* in;
in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
// Local code replicas in frequency domain
vector< fftw_complex* > L(bins);
for(int i=0;i<bins;i++)
{
fftw_complex* v2;
v2 = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
L[i] = v2;
}
// Input data in frequency domain
fftw_complex* IN;
IN = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
fftw_complex* I;
fftw_complex* O;
// Input * local replica in frequency domain.
vector< fftw_complex* > MULT(bins);
for(int i=0;i<bins;i++)
{
fftw_complex* v3;
v3 = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
MULT[i] = v3;
}
// Final results in time domain
vector< fftw_complex* > fin(bins);
for(int i=0;i<bins;i++)
{
fftw_complex* v4;
v4 = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * numSamples);
fin[i] = v4;
}
// -------------------------------------------------------------------
// Get input code
int sample = 0;
complex<float> s;
while (*input >> s && sample < numSamples)
{
in[sample][0] = real(s);
in[sample][1] = imag(s);
sample ++;
for(int i = 1; i < bands; i++)
{*input >> s;} // gpsSim outputs 2 bands (L1 and L2),
//one after the other.
// This program currently supports L1 only, this loop throws away
// the input from L2, or any other bands.
}
int count;
if(prn == 0) // Check if we are tracking all prns or just one.
{
count = 32;
prn = 1;
}
else
count = 1;
while(count > 0)
{
count--;
CACodeGenerator* codeGenPtr = new CACodeGenerator(prn);
float chipFreq = gpstk::CA_CHIP_FREQ;
// Convert input code to frequency domain.
fftw_plan p;
p = fftw_plan_dft_1d(numSamples, in, IN, FFTW_FORWARD, FFTW_ESTIMATE);
fftw_execute(p);
// Create local code replicas for each frequency bin.
float f = -(freqSearchWidth/2); // initial doppler offset
for(int i = 0; i < bins; i++)
{
cc = new CCReplica(1/sampleRate, chipFreq, interFreq+f, codeGenPtr);
cc->reset();
for(int k = 0; k < numSamples; k++)
{
complex<double> carrier = cc->getCarrier();
complex<double> code = cc->getCode() ? 1 : -1;
l[i][k][0] = real((complex<double>)((code) * (carrier)));
l[i][k][1] = imag((complex<double>)((code) * (carrier)));
cc->tick();
}
f += freqBinWidth;
}
// Convert local code replicas to frequency domain.
fftw_plan plans[bins];
pthread_t thread_id[bins];
vector<Par> par(bins);
int rc;
for(int i = 0; i < bins; i++)
{
plans[i] = fftw_plan_dft_1d
(numSamples, l[i], L[i], FFTW_FORWARD, FFTW_ESTIMATE);
par[i].plan = plans[i];
}
// Execute FFTs
for(int i = 0; i < bins; i++)
{
rc = pthread_create( &thread_id[i], NULL, compute, &par[i] ) ;
if (rc)
{
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
}
for(int i = 0; i < bins; i++)
{
rc = pthread_join( thread_id[i], NULL) ;
if (rc)
{
printf("ERROR; return code from pthread_join() is %d\n", rc);
exit(-1);
}
}
// above code replaces following block which = no multithreading.
/*for(int i = 0; i < bins ; i++)
{
fftw_execute(plans[i]);
} */
// NOTE: may want to put following multiplication into the pthread
// function.
// Will need to pass #bins, #samples and pointers to vectors in a Par.
// Right now we're only using pthreads to compute FFT's of local codes
// in parallel, and there is hardly any performance increase.
// Multiply conjugate of input frequency samples by
// local frequency samples (point by point).
complex<double> lo;
complex<double> input;
complex<double> temp;
for(int i = 0; i < bins; i++)
{
for(int k = 0; k < numSamples; k++)
{
lo = (complex<double>(L[i][k][0],L[i][k][1]))
/(double)sqrt(numSamples);
input = conj(complex<double>(IN[k][0],IN[k][1]))
/(double)sqrt(numSamples);
temp = lo * input;
MULT[i][k][0] = real(temp);
MULT[i][k][1] = imag(temp);
}
}
// Convert back to time domain and find peak.
for(int i = 0; i < bins; i++)
{
p = fftw_plan_dft_1d(numSamples, MULT[i], fin[i], 1, FFTW_ESTIMATE);
fftw_execute(p);
}
double max = 0.0;
int bin, shift;
for(int i = 0; i < bins; i++)
{
for(int k = 0; k < numSamples; k++)
{
fin[i][k][0] = abs(complex<double>
(fin[i][k][0],fin[i][k][1])
/ (double)sqrt(numSamples));
if(real(complex<double>(fin[i][k][0],fin[i][k][1])) > max)
{
max = real(complex<double>(fin[i][k][0],fin[i][k][1]));
shift = k;
bin = i;
}
}
}
// Adjust code phase value for multiple periods in time domain.
while(shift > (sampleRate*1e-3))
{
shift = shift - (sampleRate*1e-3);
}
// Dump Information.
if(max < height)
cout << "PRN: " << prn << " - Unable to acquire." << endl;
if(max >= height)
{
cout << "PRN: " << prn << " - Doppler: "
<< (bin*1e3)/(1000/freqBinWidth) - (freqSearchWidth/2)
<< " Offset: " << shift*1000/(sampleRate*1e-3)
<< " Height: " << max << endl;
cout << " - Tracker Input: -c c:1:" << prn << ":"
<< shift*1000/(sampleRate*1e-3)-5 << ":"
// Subtracting 5 right now to make sure the tracker starts
// on the "left side" of the peak.
<< (bin*1e3)/(1000/freqBinWidth) - (freqSearchWidth/2) << endl;
}
// At some point need to add a more sophisticated check for successful
// acquisition like a snr measure, although a simple cutoff works well.
// Output correlation curve for graphing purposes.
/*for(int k = 0; k < numSamples; k++)
{
cout << float((k/16.368)*1.023) << " " << (fin[bin][k][0]) << endl;
}*/
prn++;
fftw_destroy_plan(p);
} //while
fftw_free(IN); fftw_free(in);
}
//-----------------------------------------------------------------------------
void Corltr::process()
{
const unsigned windowTicks = static_cast<unsigned>(window / timeStep);
const unsigned maxSamp=windowTicks+1;
const double stepSize = cc->codeChipLen/4;
vector< complex<double> > in(maxSamp);
unsigned numSamp = 0;
complex<float> s;
double sumSq = 0.0;
while (*input >> s && numSamp < maxSamp)
{
in[numSamp] = s;
sumSq += s.real()*s.real() + s.imag()*s.imag();
for (int i=1; i<bands; i++)
*input >> s;
numSamp++;
}
if (numSamp != maxSamp)
{
cout << "Insufficient samples for specified window size. Exiting." << endl;
exit(-1);
}
if (verboseLevel)
cout << "# numSamp:" << numSamp << endl
<< "# timeStep:" << timeStep * 1e9 << " nsec" << endl
<< "# window:" << windowTicks << " samples" << endl
<< "# doppler:" << doppler << " Hz" << endl
<< "# freqErr:" << freqErr * 1e6 << " ppm" << endl
<< "# offset:" << offset*1e6 << " usec" << endl
<< "# Input sumSq: " << sumSq << endl;
cc->setCodeFreqOffsetHz(doppler);
cc->setCarrierFreqOffsetHz(doppler);
if (verboseLevel)
cc->dump(cout);
if (verboseLevel)
cout << "#h delay sum r snr " << endl
<< "#u us cnt cnt dBc-Hz" << endl;
double maxSnr=0, maxR=0, maxDelay=0;
for (int i=0; i<steps; i++)
{
double delay = i * stepSize + offset;
cc->reset();
cc->moveCodePhase(delay / cc->codeChipLen);
cc->setCodeFreqOffsetHz(doppler);
cc->setCarrierFreqOffsetHz(doppler);
SimpleCorrelator<double> sum;
double mySumSq=0;
for (int j=0; j<windowTicks; j++)
{
cc->tick();
complex<double> carrier = cc->getCarrier();
complex<double> m0 = in[j] * conj(carrier);
complex<double> code = cc->getCode() ? plusOne : minusOne;
complex<double> cc = conj(carrier) * conj(code);
mySumSq += cc.real()*cc.real() + cc.imag()*cc.imag();
sum.process(m0, conj(code));
}
double r = abs(sum()) / sqrt(sumSq)/sqrt(mySumSq);
double snr= 10*log10(r*r/timeStep);
if (snr>maxSnr)
{
maxSnr = snr;
maxR=r;
maxDelay=delay;
}
if (!peakOnly)
cout << setprecision(9) << delay*1e6
<< " " << setprecision(4) << abs(sum())
<< " " << r << " " << snr << endl;
}
if (peakOnly)
cout << setprecision(9) << maxDelay*1e6
<< setprecision(4) << " " << maxR << " " << maxSnr << endl;
}
