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