//-----------------------------------------------------------------------------
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;
}
