diff --git a/gpssim_hackrf.c b/gpssim_hackrf.c deleted file mode 100644 index 9294ed6..0000000 --- a/gpssim_hackrf.c +++ /dev/null @@ -1,1388 +0,0 @@ -#define _CRT_SECURE_NO_DEPRECATE - -#define _HACKRF -// $ hackrf_transfer -t gpssim.bin -f 1575420000 -s 2600000 -a 1 -x 0 -// with external 60dB attenuation - -#include -#include -#include -#include -#include -#include - -#define MAX_CHAR (100) - -#define MAX_SAT (32) -#define MAX_CHAN (16) - -#define N_SBF (51) // 6 seconds per subframe, 6 sec * 51 = 306 sec (max) -#define N_DWRD (N_SBF*10) // 10 word per subframe - -#define SECONDS_IN_WEEK 604800.0 -#define SECONDS_IN_HALF_WEEK 302400.0 -#define SECONDS_IN_DAY 86400.0 -#define SECONDS_IN_HOUR 3600.0 -#define SECONDS_IN_MINUTE 60.0 - -#define POW2_M5 0.03125 -#define POW2_M19 1.907348632812500e-6 -#define POW2_M29 1.862645149230957e-9 -#define POW2_M31 4.656612873077393e-10 -#define POW2_M33 1.164153218269348e-10 -#define POW2_M43 1.136868377216160e-13 -#define POW2_M55 2.775557561562891e-17 - -// Conventional values employed in GPS ephemeris model (ICD-GPS-200) -#define GM_EARTH 3.986005e14 -#define OMEGA_EARTH 7.2921151467e-5 -#define PI 3.1415926535898 - -#define R2D 57.2957795131 - -#define SPEED_OF_LIGHT 2.99792458e8 -#define LAMBDA_L1 0.190293672798365 - -#define CARR_FREQ (1575.42e6) -#define CODE_FREQ (1.023e6) -#define CARR_TO_CODE (1.0/1540.0) -#ifdef _HACKRF -#define SAMP_FREQ (2.6e6) -#else -#define SAMP_FREQ (4.0e6) -#endif -#define ADC_GAIN (250.0) // for bladeRF txvga1 = -25dB with 50dB external attenuation - -#define USER_MOTION_SIZE (3000) // max 300 sec at 10Hz -#ifdef _HACKRF -#define IQ_BUFF_SIZE (260000) -#else -#define IQ_BUFF_SIZE (400000) // 0.4Msps per 0.1sec -#endif - -typedef struct -{ - int week; - double sec; -} gpstime_t; - -typedef struct -{ - int y; - int m; - int d; - int hh; - int mm; - double sec; -} datetime_t; - -typedef struct -{ - int vflg; - gpstime_t toc; - gpstime_t toe; - int iodc; - int iode; - double deltan; - double cuc; - double cus; - double cic; - double cis; - double crc; - double crs; - double ecc; - double sqrta; - double m0; - double omg0; - double inc0; - double aop; - double omgdot; - double idot; - double af0; - double af1; - double af2; - double tgd; -} ephem_t; - -typedef struct -{ - gpstime_t g; - double range; - double rate; -} range_t; - -typedef struct -{ - int prn; - int ca[1023]; - double f_carr,f_code; - double carr_phase,code_phase; - gpstime_t g0; - unsigned long dwrd[N_DWRD]; - int iword; - int ibit; - int icode; - int dataBit; - int codeCA; - short *iq_buff; -} channel_t; - -void codegen(int *ca, int prn) -{ - int delay[] = { - 5, 6, 7, 8, 17, 18, 139, 140, 141, 251, - 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, - 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, - 861, 862}; - - int g1[1023],g2[1023],r1[10],r2[10],c1,c2; - int i,j; - int sv = prn-1; - - if (prn<1 || prn>32) - return; - - for (i=0;i<10;i++) - r1[i] = r2[i] = -1; - - for (i=0; i<1023; i++) - { - g1[i] = r1[9]; - g2[i] = r2[9]; - c1 = r1[2]*r1[9]; - c2 = r2[1]*r2[2]*r2[5]*r2[7]*r2[8]*r2[9]; - - for (j=9; j>0; j--) - { - r1[j] = r1[j-1]; - r2[j] = r2[j-1]; - } - r1[0] = c1; - r2[0] = c2; - } - - for (i=0,j=1023-delay[prn-1]; i<1023; i++,j++) - ca[i] = (1-g1[i]*g2[j%1023])/2; - - return; -} - -void date2gps(datetime_t *t, gpstime_t *g) -{ - int doy[12] = {0,31,59,90,120,151,181,212,243,273,304,334}; - int ye; - int de; - int lpdays; - - ye = t->y - 1980; - - // Compute the number of leap days since Jan 5/Jan 6, 1980. - lpdays = ye/4 + 1; - if ((ye%4)==0 && t->m<=2) - lpdays--; - - // Compute the number of days elapsed since Jan 5/Jan 6, 1980. - de = ye*365 + doy[t->m-1] + t->d + lpdays - 6; - - // Convert time to GPS weeks and seconds. - g->week = de / 7; - g->sec = (double)(de%7)*SECONDS_IN_DAY + t->hh*SECONDS_IN_HOUR - + t->mm*SECONDS_IN_MINUTE + t->sec; - - return; -} - -void xyz2llh(double *xyz, double *llh) -{ - double a,eps,e,e2; - double x,y,z; - double rho2,dz,zdz,nh,slat,n,dz_new; - - a = 6378137.0; - e = 0.0818191908426; - - eps = 1.0e-3; - e2 = e*e; - - x = xyz[0]; - y = xyz[1]; - z = xyz[2]; - - rho2 = x*x + y*y; - dz = e2*z; - - while (1) - { - zdz = z + dz; - nh = sqrt(rho2 + zdz*zdz); - slat = zdz / nh; - n = a / sqrt(1.0-e2*slat*slat); - dz_new = n*e2*slat; - - if (fabs(dz-dz_new) < eps) - break; - - dz = dz_new; - } - - llh[0] = atan2(zdz, sqrt(rho2)); - llh[1] = atan2(y, x); - llh[2] = nh - n; - - return; -} -/* -void llh2xyz(double *llh, double *xyz) -{ - double n; - double a; - double e; - double e2; - double clat; - double slat; - double clon; - double slon; - double d,nph; - double tmp; - - a = 6378137.0; - e = 0.0818191908426; - e2 = e*e; - - clat = cos(llh[0]); - slat = sin(llh[0]); - clon = cos(llh[1]); - slon = sin(llh[1]); - d = e*slat; - - n = a/sqrt(1.0-d*d); - nph = n + llh[2]; - - tmp = nph*clat; - xyz[0] = tmp*clon; - xyz[1] = tmp*slon; - xyz[2] = ((1.0-e2)*n + llh[2])*slat; - - return; -} -*/ -void ltcmat(double *llh, double t[3][3]) -{ - double slat, clat; - double slon, clon; - - slat = sin(llh[0]); - clat = cos(llh[0]); - slon = sin(llh[1]); - clon = cos(llh[1]); - - t[0][0] = -slat*clon; - t[0][1] = -slat*slon; - t[0][2] = clat; - t[1][0] = -slon; - t[1][1] = clon; - t[1][2] = 0.0; - t[2][0] = clat*clon; - t[2][1] = clat*slon; - t[2][2] = slat; - - return; -} - -void ecef2neu(double *xyz, double t[3][3], double *neu) -{ - neu[0] = t[0][0]*xyz[0] + t[0][1]*xyz[1] + t[0][2]*xyz[2]; - neu[1] = t[1][0]*xyz[0] + t[1][1]*xyz[1] + t[1][2]*xyz[2]; - neu[2] = t[2][0]*xyz[0] + t[2][1]*xyz[1] + t[2][2]*xyz[2]; - - return; -} - -void neu2azel(double *azel, double *neu) -{ - double ne; - - azel[0] = atan2(neu[1],neu[0]); - if (azel[0]<0.0) - azel[0] += (2.0*PI); - - ne = sqrt(neu[0]*neu[0] + neu[1]*neu[1]); - azel[1] = atan2(neu[2], ne); - - return; -} - -void satpos(ephem_t eph, gpstime_t g, double *pos, double *vel, double *clk) -{ - // Computing Satellite Velocity using the Broadcast Ephemeris - // http://www.ngs.noaa.gov/gps-toolbox/bc_velo.htm - - double tk; - double a; - double mk; - double mkdot; - double ek; - double ekold; - double ekdot; - double cek,sek; - double pk; - double pkdot; - double c2pk,s2pk; - double uk; - double ukdot; - double cuk,suk; - double ok; - double okdot; - double sok,cok; - double ik; - double ikdot; - double sik,cik; - double rk; - double rkdot; - double xpk,ypk; - double xpkdot,ypkdot; - - double relativistic, OneMinusecosE, sqrtOneMinuse2, tmp; - - tk = g.sec - eph.toe.sec; - - if(tk>SECONDS_IN_HALF_WEEK) - tk -= SECONDS_IN_WEEK; - else if(tk<-SECONDS_IN_HALF_WEEK) - tk += SECONDS_IN_WEEK; - - a = eph.sqrta*eph.sqrta; - - mkdot = sqrt(GM_EARTH/(a*a*a)) + eph.deltan; - mk = eph.m0 + mkdot*tk; - - ek = mk; - ekold = ek + 1.0; - - while(fabs(ek-ekold)>1.0E-14) - { - ekold = ek; - OneMinusecosE = 1.0-eph.ecc*cos(ekold); - ek = ek + (mk-ekold+eph.ecc*sin(ekold))/OneMinusecosE; - } - - sek = sin(ek); - cek = cos(ek); - - ekdot = mkdot/OneMinusecosE; - - relativistic = -4.442807633E-10*eph.ecc*eph.sqrta*sek; - - sqrtOneMinuse2 = sqrt(1.0 - eph.ecc*eph.ecc); - - pk = atan2(sqrtOneMinuse2*sek,cek-eph.ecc) + eph.aop; - pkdot = sqrtOneMinuse2*ekdot/OneMinusecosE; - - s2pk = sin(2.0*pk); - c2pk = cos(2.0*pk); - - uk = pk + eph.cus*s2pk + eph.cuc*c2pk; - suk = sin(uk); - cuk = cos(uk); - ukdot = pkdot*(1.0 + 2.0*(eph.cus*c2pk - eph.cuc*s2pk)); - - rk = a*OneMinusecosE + eph.crc*c2pk + eph.crs*s2pk; - rkdot = a*eph.ecc*sek*ekdot + 2.0*pkdot*(eph.crs*c2pk - eph.crc*s2pk); - - ik = eph.inc0 + eph.idot*tk + eph.cic*c2pk + eph.cis*s2pk; - sik = sin(ik); - cik = cos(ik); - ikdot = eph.idot + 2.0*pkdot*(eph.cis*c2pk - eph.cic*s2pk); - - xpk = rk*cuk; - ypk = rk*suk; - xpkdot = rkdot*cuk - ypk*ukdot; - ypkdot = rkdot*suk + xpk*ukdot; - - okdot = eph.omgdot - OMEGA_EARTH; - ok = eph.omg0 + tk*okdot - OMEGA_EARTH*eph.toe.sec; - sok = sin(ok); - cok = cos(ok); - - pos[0] = xpk*cok - ypk*cik*sok; - pos[1] = xpk*sok + ypk*cik*cok; - pos[2] = ypk*sik; - - tmp = ypkdot*cik - ypk*sik*ikdot; - - vel[0] = -okdot*pos[1] + xpkdot*cok - tmp*sok; - vel[1] = okdot*pos[0] + xpkdot*sok + tmp*cok; - vel[2] = ypk*cik*ikdot + ypkdot*sik; - - clk[0] = eph.af0 + tk*(eph.af1 + tk*eph.af2) + relativistic - eph.tgd; - clk[1] = eph.af1 + 2.0*tk*eph.af2; - - return; -} - -void eph2sbf(ephem_t eph, unsigned long sbf[5][10]) -{ - unsigned long wn; - unsigned long toe; - unsigned long toc; - unsigned long iode; - unsigned long iodc; - long deltan; - long cuc; - long cus; - long cic; - long cis; - long crc; - long crs; - unsigned long ecc; - unsigned long sqrta; - long m0; - long omg0; - long inc0; - long aop; - long omgdot; - long idot; - long af0; - long af1; - long af2; - long tgd; - - unsigned long ura = 2UL; - unsigned long dataId = 1UL; - unsigned long sbf4_page25_svId = 63UL; - unsigned long sbf5_page25_svId = 51UL; - - unsigned long wna; - unsigned long toa; - - wn = (unsigned long)(eph.toe.week%1024); - toe = (unsigned long)(eph.toe.sec/16.0); - toc = (unsigned long)(eph.toc.sec/16.0); - iode = (unsigned long)(eph.iode); - iodc = (unsigned long)(eph.iodc); - deltan = (long)(eph.deltan/POW2_M43/PI); - cuc = (long)(eph.cuc/POW2_M29); - cus = (long)(eph.cus/POW2_M29); - cic = (long)(eph.cic/POW2_M29); - cis = (long)(eph.cis/POW2_M29); - crc = (long)(eph.crc/POW2_M5); - crs = (long)(eph.crs/POW2_M5); - ecc = (unsigned long)(eph.ecc/POW2_M33); - sqrta = (unsigned long)(eph.sqrta/POW2_M19); - m0 = (long)(eph.m0/POW2_M31/PI); - omg0 = (long)(eph.omg0/POW2_M31/PI); - inc0 = (long)(eph.inc0/POW2_M31/PI); - aop = (long)(eph.aop/POW2_M31/PI); - omgdot = (long)(eph.omgdot/POW2_M43/PI); - idot = (long)(eph.idot/POW2_M43/PI); - af0 = (long)(eph.af0/POW2_M31); - af1 = (long)(eph.af1/POW2_M43); - af2 = (long)(eph.af2/POW2_M55); - tgd = (long)(eph.tgd/POW2_M31); - - wna = (unsigned long)(eph.toe.week%256); - toa = (unsigned long)(eph.toe.sec/4096.0); - - // Subframe 1 - sbf[0][0] = 0x8B0000UL<<6; - sbf[0][1] = 0x1UL<<8; - sbf[0][2] = ((wn&0x3FFUL)<<20) | (ura<<14) | (((iodc>>8)&0x3UL)<<6); - sbf[0][3] = 0UL; - sbf[0][4] = 0UL; - sbf[0][5] = 0UL; - sbf[0][6] = (tgd&0xFFUL)<<6; - sbf[0][7] = ((iodc&0xFFUL)<<22) | ((toc&0xFFFFUL)<<6); - sbf[0][8] = ((af2&0xFFUL)<<22) | ((af1&0xFFFFUL)<<6); - sbf[0][9] = (af0&0x3FFFFFUL)<<8; - - // Subframe 2 - sbf[1][0] = 0x8B0000UL<<6; - sbf[1][1] = 0x2UL<<8; - sbf[1][2] = ((iode&0xFFUL)<<22) | ((crs&0xFFFFUL)<<6); - sbf[1][3] = ((deltan&0xFFFFUL)<<14) | (((m0>>24)&0xFFUL)<<6); - sbf[1][4] = (m0&0xFFFFFFUL)<<6; - sbf[1][5] = ((cuc&0xFFFFUL)<<14) | (((ecc>>24)&0xFFUL)<<6); - sbf[1][6] = (ecc&0xFFFFFFUL)<<6; - sbf[1][7] = ((cus&0xFFFFUL)<<14) | (((sqrta>>24)&0xFFUL)<<6); - sbf[1][8] = (sqrta&0xFFFFFFUL)<<6; - sbf[1][9] = (toe&0xFFFFUL)<<14; - - // Subframe 3 - sbf[2][0] = 0x8B0000UL<<6; - sbf[2][1] = 0x3UL<<8; - sbf[2][2] = ((cic&0xFFFFUL)<<14) | (((omg0>>24)&0xFFUL)<<6); - sbf[2][3] = (omg0&0xFFFFFFUL)<<6; - sbf[2][4] = ((cis&0xFFFFUL)<<14) | (((inc0>>24)&0xFFUL)<<6); - sbf[2][5] = (inc0&0xFFFFFFUL)<<6; - sbf[2][6] = ((crc&0xFFFFUL)<<14) | (((aop>>24)&0xFFUL)<<6); - sbf[2][7] = (aop&0xFFFFFFUL)<<6; - sbf[2][8] = (omgdot&0xFFFFFFUL)<<6; - sbf[2][9] = ((iode&0xFFUL)<<22) | ((idot&0x3FFFUL)<<8); - - // Subframe 4, page 25 - sbf[3][0] = 0x8B0000UL<<6; - sbf[3][1] = 0x4UL<<8; - sbf[3][2] = (dataId<<28) | (sbf4_page25_svId<<22); - sbf[3][3] = 0UL; - sbf[3][4] = 0UL; - sbf[3][5] = 0UL; - sbf[3][6] = 0UL; - sbf[3][7] = 0UL; - sbf[3][8] = 0UL; - sbf[3][9] = 0UL; - - // Subframe 5, page 25 - sbf[4][0] = 0x8B0000UL<<6; - sbf[4][1] = 0x5UL<<8; - sbf[4][2] = (dataId<<28) | (sbf5_page25_svId<<22) | ((toa&0xFFUL)<<14) | ((wna&0xFFUL)<<6); - sbf[4][3] = 0UL; - sbf[4][4] = 0UL; - sbf[4][5] = 0UL; - sbf[4][6] = 0UL; - sbf[4][7] = 0UL; - sbf[4][8] = 0UL; - sbf[4][9] = 0UL; - - return; -} - -unsigned long countBits(unsigned long v) -{ - unsigned long c; - const int S[] = {1, 2, 4, 8, 16}; - const unsigned long B[] = { - 0x55555555, 0x33333333, 0x0F0F0F0F, 0x00FF00FF, 0x0000FFFF}; - - c = v; - c = ((c >> S[0]) & B[0]) + (c & B[0]); - c = ((c >> S[1]) & B[1]) + (c & B[1]); - c = ((c >> S[2]) & B[2]) + (c & B[2]); - c = ((c >> S[3]) & B[3]) + (c & B[3]); - c = ((c >> S[4]) & B[4]) + (c & B[4]); - - return(c); -} - -unsigned long computeChecksum(unsigned long source, int nib) -{ - /* - Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30* - Bits 29 to 6 = Source data bits, d1, d2, ..., d24 - Bits 5 to 0 = Empty parity bits - */ - - /* - Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30* - Bits 29 to 6 = Data bits transmitted by the SV, D1, D2, ..., D24 - Bits 5 to 0 = Computed parity bits, D25, D26, ..., D30 - */ - - /* - 1 2 3 - bit 12 3456 7890 1234 5678 9012 3456 7890 - --- ------------------------------------- - D25 11 1011 0001 1111 0011 0100 1000 0000 - D26 01 1101 1000 1111 1001 1010 0100 0000 - D27 10 1110 1100 0111 1100 1101 0000 0000 - D28 01 0111 0110 0011 1110 0110 1000 0000 - D29 10 1011 1011 0001 1111 0011 0100 0000 - D30 00 1011 0111 1010 1000 1001 1100 0000 - */ - - unsigned long bmask[6] = { - 0x3B1F3480UL, 0x1D8F9A40UL, 0x2EC7CD00UL, - 0x1763E680UL, 0x2BB1F340UL, 0x0B7A89C0UL }; - - unsigned long D; - unsigned long d = source & 0x3FFFFFC0UL; - unsigned long D29 = (source>>31)&0x1UL; - unsigned long D30 = (source>>30)&0x1UL; - - if (nib) // Non-information bearing bits for word 2 and 10 - { - /* - Solve bits 23 and 24 to presearve parity check - with zeros in bits 29 and 30. - */ - - if ((D30 + countBits(bmask[4] & d)) % 2) - d ^= (0x1UL<<6); - if ((D29 + countBits(bmask[5] & d)) % 2) - d ^= (0x1UL<<7); - } - - D = d; - if (D30) - D ^= 0x3FFFFFC0UL; - - D |= ((D29 + countBits(bmask[0] & d)) % 2) << 5; - D |= ((D30 + countBits(bmask[1] & d)) % 2) << 4; - D |= ((D29 + countBits(bmask[2] & d)) % 2) << 3; - D |= ((D30 + countBits(bmask[3] & d)) % 2) << 2; - D |= ((D30 + countBits(bmask[4] & d)) % 2) << 1; - D |= ((D29 + countBits(bmask[5] & d)) % 2); - - D &= 0x3FFFFFFFUL; - //D |= (source & 0xC0000000UL); // Add D29* and D30* from source data bits - - return(D); -} - -int checkExpDesignator(char *str, int len) -{ - int i,n=0; - - for (i=0; i-SECONDS_IN_HOUR) && (dt<=SECONDS_IN_HOUR)) - { - strncpy(tmp, str, 2); - tmp[2] = 0; - sv = atoi(tmp)-1; - - if (eph[sv].vflg==0) - { - eph[sv].toc = g; - - strncpy(tmp, str+22, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); // tmp[15]='E'; - eph[sv].af0 = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].af1 = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].af2 = atof(tmp); - - // BROADCAST ORBIT - 1 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+3, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].iode = (int)atof(tmp); - - strncpy(tmp, str+22, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].crs = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].deltan = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].m0 = atof(tmp); - - // BROADCAST ORBIT - 2 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+3, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].cuc = atof(tmp); - - strncpy(tmp, str+22, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].ecc = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].cus = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].sqrta = atof(tmp); - - // BROADCAST ORBIT - 3 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+3, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].toe.sec = atof(tmp); - - strncpy(tmp, str+22, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].cic = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].omg0 = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].cis = atof(tmp); - - // BROADCAST ORBIT - 4 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+3, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].inc0 = atof(tmp); - - strncpy(tmp, str+22, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].crc = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].aop = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].omgdot = atof(tmp); - - // BROADCAST ORBIT - 5 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+3, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].idot = atof(tmp); - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].toe.week = (int)atof(tmp); - - // BROADCAST ORBIT - 6 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - strncpy(tmp, str+41, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].tgd = atof(tmp); - - strncpy(tmp, str+60, 19); - tmp[19] = 0; - checkExpDesignator(tmp, 19); - eph[sv].iodc = (int)atof(tmp); - - // BROADCAST ORBIT - 7 - if (NULL==fgets(str, MAX_CHAR, fp)) - break; - - eph[sv].vflg = 1; - - nsat++; - } - } - else - break; - } - - fclose(fp); - - return(nsat); -} - -void subVect(double *y, double *x1, double *x2) -{ - y[0] = x1[0]-x2[0]; - y[1] = x1[1]-x2[1]; - y[2] = x1[2]-x2[2]; - - return; -} - -double normVect(double *x) -{ - return(sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2])); -} - -double dotProd(double *x1, double *x2) -{ - return(x1[0]*x2[0]+x1[1]*x2[1]+x1[2]*x2[2]); -} - -void computeRange(range_t *rho, ephem_t eph, gpstime_t g, double xyz[]) -{ - double pos[3],vel[3],clk[2]; - double los[3]; - double tau; - double range,rate; - - // SV position at time of the pseudorange observation. - satpos(eph, g, pos, vel, clk); - - // Receiver to satellite vector and light-time. - subVect(los, pos, xyz); - tau = normVect(los)/SPEED_OF_LIGHT; - - // Extrapolate the satellite position backwards to the transmission time. - pos[0] -= vel[0]*tau; - pos[1] -= vel[1]*tau; - pos[2] -= vel[2]*tau; - - // Earth rotation correction. The change in velocity can be neglected. - pos[0] += pos[1]*OMEGA_EARTH*tau; - pos[1] -= pos[0]*OMEGA_EARTH*tau; - - // New observer to satellite vector and satellite range. - subVect(los, pos, xyz); - range = normVect(los); - - // Pseudorange. - rho->range = range - SPEED_OF_LIGHT*clk[0]; - - // Relative velocity of SV and receiver. - rate = dotProd(vel, los)/range; - - // Pseudorange rate. - rho->rate = rate; // - SPEED_OF_LIGHT*clk[1]; - - // Time of application - rho->g = g; - - return; -} - -void computeCodePhase(channel_t *chan, range_t rho0, range_t rho1, double dt) -{ - double ms; - int ims; - double rhorate; - - // Pseudorange rate. - rhorate = (rho1.range - rho0.range)/dt; - - // Carrier and code frequency. - chan->f_carr = -rhorate/LAMBDA_L1; - chan->f_code = CODE_FREQ + chan->f_carr*CARR_TO_CODE; - - // Initial code phase and data bit counters. - ms = (((rho0.g.sec-chan->g0.sec)+6.0) - rho0.range/SPEED_OF_LIGHT)*1000.0; - - ims = (int)ms; - chan->code_phase = (ms-(double)ims)*1023.0; // in chip - - chan->iword = ims/600; // 1 word = 30 bits = 600 ms - ims -= chan->iword*600; - - chan->ibit = ims/20; // 1 bit = 20 code = 20 ms - ims -= chan->ibit*20; - - chan->icode = ims; // 1 code = 1 ms - - chan->codeCA = chan->ca[(int)chan->code_phase]*2-1; - chan->dataBit = (int)((chan->dwrd[chan->iword]>>(29-chan->ibit)) & 0x1UL)*2-1; - - return; -} - -int readUserMotion(double xyz[USER_MOTION_SIZE][3], char *filename) -{ - FILE *fp; - int numd; - double t,x,y,z; - - if (NULL==(fp=fopen(filename,"rt"))) - return(-1); - - for (numd=0; numd [output (default: gpssim.bin)]\n"); - exit(1); - } - - strcpy(navfile, argv[1]); - strcpy(umfile, argv[2]); - - if (argc==3) - strcpy(outfile, "gpssim.bin"); - else - strcpy(outfile, argv[3]); - - //////////////////////////////////////////////////////////// - // Receiver position - //////////////////////////////////////////////////////////// - - // Read user motion file - numd = readUserMotion(umd, umfile); - - if (numd==-1) - { - printf("Failed to open user motion file.\n"); - exit(1); - } - else if (numd==0) - { - printf("Failed to read user motion data.\n"); - exit(1); - } - - printf("User motion data = %d\n", numd); - - // Initial location - xyz[0] = umd[0][0]; - xyz[1] = umd[0][1]; - xyz[2] = umd[0][2]; - - // Geodetic coordinate system - xyz2llh(xyz, llh); - - printf("xyz = %11.1f, %11.1f, %11.1f\n", xyz[0], xyz[1], xyz[2]); - printf("llh = %11.6f, %11.6f, %11.1f\n", llh[0]*R2D, llh[1]*R2D, llh[2]); - - //////////////////////////////////////////////////////////// - // Read ephemeris - //////////////////////////////////////////////////////////// - - neph = readRinexNav(eph, navfile); - - if (neph==-1) - { - printf("Failed to open ephemeris file.\n"); - exit(1); - } - - g0.week = -1; - - if (neph>0) - { - for (sv=0; svelvmask) - { - chan[nsat].prn = sv+1; - nsat++; - - printf("%02d %6.1f %5.1f\n", sv+1, azel[0]*R2D, azel[1]*R2D); - } - - //printf("%1c%02d %6.1f %5.1f\n", (azel[1]>elvmask)?'*':' ', sv+1, azel[0]*R2D, azel[1]*R2D); - } - } - - printf("Number of channels = %d\n", nsat); - - //return(0); - - //////////////////////////////////////////////////////////// - // Baseband signal buffer and output file - //////////////////////////////////////////////////////////// - - // Allocate I/Q buffer -#ifdef _HACKRF - iq_buff = (signed char *)calloc(2*IQ_BUFF_SIZE, 1); -#else - iq_buff = (short *)calloc(2*IQ_BUFF_SIZE, 2); -#endif - if (iq_buff==NULL) - { - printf("Faild to allocate IQ buffer.\n"); - exit(1); - } - - // Open output file - if (NULL==(fp=fopen(outfile,"wb"))) - { - printf("Failed to open output file.\n"); - exit(1); - } - - //////////////////////////////////////////////////////////// - // Initialize channels - //////////////////////////////////////////////////////////// - - // Initial reception time - grx0 = g0; - - for (i=0; i Configuration Properties -> C/C++ -> Language -> Open MP Support -> Yes (/openmp) - for (i=0; i=1023.0) - { - chan[i].code_phase -= 1023.0; - - chan[i].icode++; - - if (chan[i].icode>=20) // 20 C/A codes = 1 navigation data bit - { - chan[i].icode = 0; - chan[i].ibit++; - - if (chan[i].ibit>=30) // 30 navigation data bits = 1 word - { - chan[i].ibit = 0; - chan[i].iword++; - } - - // Set new navigation data bit - chan[i].dataBit = (int)((chan[i].dwrd[chan[i].iword]>>(29-chan[i].ibit)) & 0x1UL)*2-1; - } - } - - // Set currnt code chip - chan[i].codeCA = chan[i].ca[(int)chan[i].code_phase]*2-1; - - // Update carrier phase - chan[i].carr_phase += chan[i].f_carr * delt; - - if (chan[i].carr_phase>=1.0) - chan[i].carr_phase -= 1.0; - else if (chan[i].carr_phase<0.0) - chan[i].carr_phase += 1.0; - } - } // End of omp parallel for - - for (isamp=0; isamp<2*IQ_BUFF_SIZE; isamp++) - { - iq_buff[isamp] = 0; - - for (i=0; i>4); // 12-bit bladeRF -> 8-bit HackRF -#else - iq_buff[isamp] += chan[i].iq_buff[isamp]; -#endif - } -#ifdef _HACKRF - fwrite(iq_buff, 1, 2*IQ_BUFF_SIZE, fp); -#else - fwrite(iq_buff, 2, 2*IQ_BUFF_SIZE, fp); // 2 (I/Q) * 2 (short) * 4 Msps = 16,000,000 byte/s -#endif - // Next second - grx0.sec += 0.1; - - // Update time counter - printf("\rTime = %4.1f", grx0.sec-g0.sec); - fflush(stdout); - } - - tend = clock(); - - printf("\nDone!\n"); - - // Free I/Q buffer - free(iq_buff); - for (i=0; i