From ced2785a3a43590174ad909655bc708d0ddaa0b6 Mon Sep 17 00:00:00 2001 From: 168492376 <168492376@qq.com> Date: Tue, 31 Dec 2024 00:44:43 +0800 Subject: [PATCH] add config save and load --- gnss-sim.pro | 22 +- gpssim.c | 2488 ++++++++++++++++++++++++++++++++++++++++++++++++ gpssim.h | 187 ++++ mainwindow.cpp | 138 +++ mainwindow.h | 12 + uhd_device.cpp | 82 ++ uhd_device.h | 8 + workThread.cpp | 115 +++ workThread.h | 45 + 9 files changed, 3092 insertions(+), 5 deletions(-) create mode 100644 gpssim.c create mode 100644 gpssim.h create mode 100644 uhd_device.cpp create mode 100644 uhd_device.h create mode 100644 workThread.cpp create mode 100644 workThread.h diff --git a/gnss-sim.pro b/gnss-sim.pro index cd693a97..230dc0f0 100644 --- a/gnss-sim.pro +++ b/gnss-sim.pro @@ -1,28 +1,40 @@ QT += core gui -greaterThan(QT_MAJOR_VERSION, 4): QT += widgets +greaterThan(QT_MAJOR_VERSION, 4): QT += widgets concurrent CONFIG += c++17 +msvc{ + QMAKE_CFLAGS += /utf-8 + QMAKE_CXXFLAGS += /utf-8 +} + # You can make your code fail to compile if it uses deprecated APIs. # In order to do so, uncomment the following line. #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 INCLUDEPATH += \ - $$PWD/3rdparty/uhd/include + $$PWD/3rdparty/uhd/include \ + $$PWD/3rdparty/ SOURCES += \ + gpssim.c \ main.cpp \ - mainwindow.cpp + mainwindow.cpp \ + uhd_device.cpp \ + workThread.cpp HEADERS += \ - mainwindow.h + gpssim.h \ + mainwindow.h \ + uhd_device.h \ + workThread.h FORMS += \ mainwindow.ui win32{ -LIBS += -L$$PWD/3rdparty/uhd/lib -luhd +LIBS += -L$$PWD/3rdparty/uhd/lib -luhd -L$$PWD/3rdparty/lib64-msvc-14.3 -llibboost_thread-vc143-mt-gd-x64-1_86 } # Default rules for deployment. diff --git a/gpssim.c b/gpssim.c new file mode 100644 index 00000000..02233c8e --- /dev/null +++ b/gpssim.c @@ -0,0 +1,2488 @@ +#define _CRT_SECURE_NO_DEPRECATE + +#include +#include +#include +#include +#include +#ifdef _WIN32 +//#include "getopt.h" +#else +#include +#endif +#include "gpssim.h" + +int sinTable512[] = { + 2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, + 50, 53, 56, 59, 62, 65, 68, 71, 74, 77, 80, 83, 86, 89, 91, 94, + 97, 100, 103, 105, 108, 111, 114, 116, 119, 122, 125, 127, 130, 132, 135, 138, + 140, 143, 145, 148, 150, 153, 155, 157, 160, 162, 164, 167, 169, 171, 173, 176, + 178, 180, 182, 184, 186, 188, 190, 192, 194, 196, 198, 200, 202, 204, 205, 207, + 209, 210, 212, 214, 215, 217, 218, 220, 221, 223, 224, 225, 227, 228, 229, 230, + 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 241, 242, 243, 244, 244, 245, + 245, 246, 247, 247, 248, 248, 248, 249, 249, 249, 249, 250, 250, 250, 250, 250, + 250, 250, 250, 250, 250, 249, 249, 249, 249, 248, 248, 248, 247, 247, 246, 245, + 245, 244, 244, 243, 242, 241, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232, + 230, 229, 228, 227, 225, 224, 223, 221, 220, 218, 217, 215, 214, 212, 210, 209, + 207, 205, 204, 202, 200, 198, 196, 194, 192, 190, 188, 186, 184, 182, 180, 178, + 176, 173, 171, 169, 167, 164, 162, 160, 157, 155, 153, 150, 148, 145, 143, 140, + 138, 135, 132, 130, 127, 125, 122, 119, 116, 114, 111, 108, 105, 103, 100, 97, + 94, 91, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59, 56, 53, 50, + 47, 44, 41, 38, 35, 32, 29, 26, 23, 20, 17, 14, 11, 8, 5, 2, + -2, -5, -8, -11, -14, -17, -20, -23, -26, -29, -32, -35, -38, -41, -44, -47, + -50, -53, -56, -59, -62, -65, -68, -71, -74, -77, -80, -83, -86, -89, -91, -94, + -97,-100,-103,-105,-108,-111,-114,-116,-119,-122,-125,-127,-130,-132,-135,-138, + -140,-143,-145,-148,-150,-153,-155,-157,-160,-162,-164,-167,-169,-171,-173,-176, + -178,-180,-182,-184,-186,-188,-190,-192,-194,-196,-198,-200,-202,-204,-205,-207, + -209,-210,-212,-214,-215,-217,-218,-220,-221,-223,-224,-225,-227,-228,-229,-230, + -232,-233,-234,-235,-236,-237,-238,-239,-240,-241,-241,-242,-243,-244,-244,-245, + -245,-246,-247,-247,-248,-248,-248,-249,-249,-249,-249,-250,-250,-250,-250,-250, + -250,-250,-250,-250,-250,-249,-249,-249,-249,-248,-248,-248,-247,-247,-246,-245, + -245,-244,-244,-243,-242,-241,-241,-240,-239,-238,-237,-236,-235,-234,-233,-232, + -230,-229,-228,-227,-225,-224,-223,-221,-220,-218,-217,-215,-214,-212,-210,-209, + -207,-205,-204,-202,-200,-198,-196,-194,-192,-190,-188,-186,-184,-182,-180,-178, + -176,-173,-171,-169,-167,-164,-162,-160,-157,-155,-153,-150,-148,-145,-143,-140, + -138,-135,-132,-130,-127,-125,-122,-119,-116,-114,-111,-108,-105,-103,-100, -97, + -94, -91, -89, -86, -83, -80, -77, -74, -71, -68, -65, -62, -59, -56, -53, -50, + -47, -44, -41, -38, -35, -32, -29, -26, -23, -20, -17, -14, -11, -8, -5, -2 +}; + +int cosTable512[] = { + 250, 250, 250, 250, 250, 249, 249, 249, 249, 248, 248, 248, 247, 247, 246, 245, + 245, 244, 244, 243, 242, 241, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232, + 230, 229, 228, 227, 225, 224, 223, 221, 220, 218, 217, 215, 214, 212, 210, 209, + 207, 205, 204, 202, 200, 198, 196, 194, 192, 190, 188, 186, 184, 182, 180, 178, + 176, 173, 171, 169, 167, 164, 162, 160, 157, 155, 153, 150, 148, 145, 143, 140, + 138, 135, 132, 130, 127, 125, 122, 119, 116, 114, 111, 108, 105, 103, 100, 97, + 94, 91, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59, 56, 53, 50, + 47, 44, 41, 38, 35, 32, 29, 26, 23, 20, 17, 14, 11, 8, 5, 2, + -2, -5, -8, -11, -14, -17, -20, -23, -26, -29, -32, -35, -38, -41, -44, -47, + -50, -53, -56, -59, -62, -65, -68, -71, -74, -77, -80, -83, -86, -89, -91, -94, + -97,-100,-103,-105,-108,-111,-114,-116,-119,-122,-125,-127,-130,-132,-135,-138, + -140,-143,-145,-148,-150,-153,-155,-157,-160,-162,-164,-167,-169,-171,-173,-176, + -178,-180,-182,-184,-186,-188,-190,-192,-194,-196,-198,-200,-202,-204,-205,-207, + -209,-210,-212,-214,-215,-217,-218,-220,-221,-223,-224,-225,-227,-228,-229,-230, + -232,-233,-234,-235,-236,-237,-238,-239,-240,-241,-241,-242,-243,-244,-244,-245, + -245,-246,-247,-247,-248,-248,-248,-249,-249,-249,-249,-250,-250,-250,-250,-250, + -250,-250,-250,-250,-250,-249,-249,-249,-249,-248,-248,-248,-247,-247,-246,-245, + -245,-244,-244,-243,-242,-241,-241,-240,-239,-238,-237,-236,-235,-234,-233,-232, + -230,-229,-228,-227,-225,-224,-223,-221,-220,-218,-217,-215,-214,-212,-210,-209, + -207,-205,-204,-202,-200,-198,-196,-194,-192,-190,-188,-186,-184,-182,-180,-178, + -176,-173,-171,-169,-167,-164,-162,-160,-157,-155,-153,-150,-148,-145,-143,-140, + -138,-135,-132,-130,-127,-125,-122,-119,-116,-114,-111,-108,-105,-103,-100, -97, + -94, -91, -89, -86, -83, -80, -77, -74, -71, -68, -65, -62, -59, -56, -53, -50, + -47, -44, -41, -38, -35, -32, -29, -26, -23, -20, -17, -14, -11, -8, -5, -2, + 2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, + 50, 53, 56, 59, 62, 65, 68, 71, 74, 77, 80, 83, 86, 89, 91, 94, + 97, 100, 103, 105, 108, 111, 114, 116, 119, 122, 125, 127, 130, 132, 135, 138, + 140, 143, 145, 148, 150, 153, 155, 157, 160, 162, 164, 167, 169, 171, 173, 176, + 178, 180, 182, 184, 186, 188, 190, 192, 194, 196, 198, 200, 202, 204, 205, 207, + 209, 210, 212, 214, 215, 217, 218, 220, 221, 223, 224, 225, 227, 228, 229, 230, + 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 241, 242, 243, 244, 244, 245, + 245, 246, 247, 247, 248, 248, 248, 249, 249, 249, 249, 250, 250, 250, 250, 250 +}; + +// Receiver antenna attenuation in dB for boresight angle = 0:5:180 [deg] +double ant_pat_db[37] = { + 0.00, 0.00, 0.22, 0.44, 0.67, 1.11, 1.56, 2.00, 2.44, 2.89, 3.56, 4.22, + 4.89, 5.56, 6.22, 6.89, 7.56, 8.22, 8.89, 9.78, 10.67, 11.56, 12.44, 13.33, + 14.44, 15.56, 16.67, 17.78, 18.89, 20.00, 21.33, 22.67, 24.00, 25.56, 27.33, 29.33, + 31.56 +}; + +int allocatedSat[MAX_SAT]; + +double xyz[USER_MOTION_SIZE][3]; + +/*! \brief Subtract two vectors of double + * \param[out] y Result of subtraction + * \param[in] x1 Minuend of subtraction + * \param[in] x2 Subtrahend of subtraction + */ +void subVect(double *y, const double *x1, const double *x2) +{ + y[0] = x1[0]-x2[0]; + y[1] = x1[1]-x2[1]; + y[2] = x1[2]-x2[2]; + + return; +} + +/*! \brief Compute Norm of Vector + * \param[in] x Input vector + * \returns Length (Norm) of the input vector + */ +double normVect(const double *x) +{ + return(sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2])); +} + +/*! \brief Compute dot-product of two vectors + * \param[in] x1 First multiplicand + * \param[in] x2 Second multiplicand + * \returns Dot-product of both multiplicands + */ +double dotProd(const double *x1, const double *x2) +{ + return(x1[0]*x2[0]+x1[1]*x2[1]+x1[2]*x2[2]); +} + +/* !\brief generate the C/A code sequence for a given Satellite Vehicle PRN + * \param[in] prn PRN number of the Satellite Vehicle + * \param[out] ca Caller-allocated integer array of 1023 bytes + */ +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[CA_SEQ_LEN], g2[CA_SEQ_LEN]; + int r1[N_DWRD_SBF], r2[N_DWRD_SBF]; + int c1, c2; + int i,j; + + if (prn<1 || prn>32) + return; + + for (i=0; i0; j--) + { + r1[j] = r1[j-1]; + r2[j] = r2[j-1]; + } + r1[0] = c1; + r2[0] = c2; + } + + for (i=0,j=CA_SEQ_LEN-delay[prn-1]; iy - 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 gps2date(const gpstime_t *g, datetime_t *t) +{ + // Convert Julian day number to calendar date + int c = (int)(7*g->week + floor(g->sec/86400.0)+2444245.0) + 1537; + int d = (int)((c-122.1)/365.25); + int e = 365*d + d/4; + int f = (int)((c-e)/30.6001); + + t->d = c - e - (int)(30.6001*f); + t->m = f - 1 - 12*(f/14); + t->y = d - 4715 - ((7 + t->m)/10); + + t->hh = ((int)(g->sec/3600.0))%24; + t->mm = ((int)(g->sec/60.0))%60; + t->sec = g->sec - 60.0*floor(g->sec/60.0); + + return; +} + +/*! \brief Convert Earth-centered Earth-fixed (ECEF) into Lat/Long/Height + * \param[in] xyz Input Array of X, Y and Z ECEF coordinates + * \param[out] llh Output Array of Latitude, Longitude and Height + */ +void xyz2llh(const double *xyz, double *llh) +{ + double a,eps,e,e2; + double x,y,z; + double rho2,dz,zdz,nh,slat,n,dz_new; + + a = WGS84_RADIUS; + e = WGS84_ECCENTRICITY; + + eps = 1.0e-3; + e2 = e*e; + + if (normVect(xyz)SECONDS_IN_HALF_WEEK) + tk -= SECONDS_IN_WEEK; + else if(tk<-SECONDS_IN_HALF_WEEK) + tk += SECONDS_IN_WEEK; + + mk = eph.m0 + eph.n*tk; + ek = mk; + ekold = ek + 1.0; + + OneMinusecosE = 0; // Suppress the uninitialized warning. + 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 = eph.n/OneMinusecosE; + + relativistic = -4.442807633E-10*eph.ecc*eph.sqrta*sek; + + pk = atan2(eph.sq1e2*sek,cek-eph.ecc) + eph.aop; + pkdot = eph.sq1e2*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 = eph.A*OneMinusecosE + eph.crc*c2pk + eph.crs*s2pk; + rkdot = eph.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; + + ok = eph.omg0 + tk*eph.omgkdot - 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] = -eph.omgkdot*pos[1] + xpkdot*cok - tmp*sok; + vel[1] = eph.omgkdot*pos[0] + xpkdot*sok + tmp*cok; + vel[2] = ypk*cik*ikdot + ypkdot*sik; + + // Satellite clock correction + tk = g.sec - eph.toc.sec; + + if(tk>SECONDS_IN_HALF_WEEK) + tk -= SECONDS_IN_WEEK; + else if(tk<-SECONDS_IN_HALF_WEEK) + tk += SECONDS_IN_WEEK; + + clk[0] = eph.af0 + tk*(eph.af1 + tk*eph.af2) + relativistic - eph.tgd; + clk[1] = eph.af1 + 2.0*tk*eph.af2; + + return; +} + +/*! \brief Compute Subframe from Ephemeris + * \param[in] eph Ephemeris of given SV + * \param[out] sbf Array of five sub-frames, 10 long words each + */ +void eph2sbf(const ephem_t eph, const ionoutc_t ionoutc, unsigned long sbf[5][N_DWRD_SBF]) +{ + 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; + int svhlth; + int codeL2; + + unsigned long ura = 0UL; + unsigned long dataId = 1UL; + unsigned long sbf4_page25_svId = 63UL; + unsigned long sbf5_page25_svId = 51UL; + + unsigned long wna; + unsigned long toa; + + signed long alpha0,alpha1,alpha2,alpha3; + signed long beta0,beta1,beta2,beta3; + signed long A0,A1; + signed long dtls; + unsigned long tot,wnt,wnlsf,dtlsf,dn; + unsigned long sbf4_page18_svId = 56UL; + + // FIXED: This has to be the "transmission" week number, not for the ephemeris reference time + //wn = (unsigned long)(eph.toe.week%1024); + wn = 0UL; + 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); + svhlth = (unsigned long)(eph.svhlth); + codeL2 = (unsigned long)(eph.codeL2); + + wna = (unsigned long)(eph.toe.week%256); + toa = (unsigned long)(eph.toe.sec/4096.0); + + alpha0 = (signed long)round(ionoutc.alpha0/POW2_M30); + alpha1 = (signed long)round(ionoutc.alpha1/POW2_M27); + alpha2 = (signed long)round(ionoutc.alpha2/POW2_M24); + alpha3 = (signed long)round(ionoutc.alpha3/POW2_M24); + beta0 = (signed long)round(ionoutc.beta0/2048.0); + beta1 = (signed long)round(ionoutc.beta1/16384.0); + beta2 = (signed long)round(ionoutc.beta2/65536.0); + beta3 = (signed long)round(ionoutc.beta3/65536.0); + A0 = (signed long)round(ionoutc.A0/POW2_M30); + A1 = (signed long)round(ionoutc.A1/POW2_M50); + dtls = (signed long)(ionoutc.dtls); + tot = (unsigned long)(ionoutc.tot/4096); + wnt = (unsigned long)(ionoutc.wnt%256); + + // 2016/12/31 (Sat) -> WNlsf = 1929, DN = 7 (http://navigationservices.agi.com/GNSSWeb/) + // Days are counted from 1 to 7 (Sunday is 1). + if (ionoutc.leapen==TRUE) + { + wnlsf = (unsigned long)(ionoutc.wnlsf%256); + dn = (unsigned long)(ionoutc.dn); + dtlsf = (unsigned long)(ionoutc.dtlsf); + } + else + { + wnlsf = 1929%256; + dn = 7; + dtlsf = 18; + } + // Subframe 1 + sbf[0][0] = 0x8B0000UL<<6; + sbf[0][1] = 0x1UL<<8; + sbf[0][2] = ((wn&0x3FFUL)<<20) | ((codeL2&0x3UL)<<18) | ((ura&0xFUL)<<14) | ((svhlth&0x3FUL)<<8) | (((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); + + if (ionoutc.vflg==TRUE) + { + // Subframe 4, page 18 + sbf[3][0] = 0x8B0000UL<<6; + sbf[3][1] = 0x4UL<<8; + sbf[3][2] = (dataId<<28) | (sbf4_page18_svId<<22) | ((alpha0&0xFFUL)<<14) | ((alpha1&0xFFUL)<<6); + sbf[3][3] = ((alpha2&0xFFUL)<<22) | ((alpha3&0xFFUL)<<14) | ((beta0&0xFFUL)<<6); + sbf[3][4] = ((beta1&0xFFUL)<<22) | ((beta2&0xFFUL)<<14) | ((beta3&0xFFUL)<<6); + sbf[3][5] = (A1&0xFFFFFFUL)<<6; + sbf[3][6] = ((A0>>8)&0xFFFFFFUL)<<6; + sbf[3][7] = ((A0&0xFFUL)<<22) | ((tot&0xFFUL)<<14) | ((wnt&0xFFUL)<<6); + sbf[3][8] = ((dtls&0xFFUL)<<22) | ((wnlsf&0xFFUL)<<14) | ((dn&0xFFUL)<<6); + sbf[3][9] = (dtlsf&0xFFUL)<<22; + + } + else + { + // 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; +} + +/*! \brief Count number of bits set to 1 + * \param[in] v long word in which bits are counted + * \returns Count of bits set to 1 + */ +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); +} + +/*! \brief Compute the Checksum for one given word of a subframe + * \param[in] source The input data + * \param[in] nib Does this word contain non-information-bearing bits? + * \returns Computed Checksum + */ +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 preserve 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); +} + +/*! \brief Replace all 'E' exponential designators to 'D' + * \param str String in which all occurrences of 'E' are replaced with * 'D' + * \param len Length of input string in bytes + * \returns Number of characters replaced + */ +int replaceExpDesignator(char *str, int len) +{ + int i,n=0; + + for (i=0; i=SECONDS_IN_WEEK) + { + g1.sec -= SECONDS_IN_WEEK; + g1.week++; + } + + while (g1.sec<0.0) + { + g1.sec += SECONDS_IN_WEEK; + g1.week--; + } + + return(g1); +} + +/*! \brief Read Ephemeris data from the RINEX Navigation file */ +/* \param[out] eph Array of Output SV ephemeris data + * \param[in] fname File name of the RINEX file + * \returns Number of sets of ephemerides in the file + */ +int readRinexNavAll(ephem_t eph[][MAX_SAT], ionoutc_t *ionoutc, const char *fname) +{ + FILE *fp; + int ieph; + + int sv; + char str[MAX_CHAR]; + char tmp[20]; + + datetime_t t; + gpstime_t g; + gpstime_t g0; + double dt; + + int flags = 0x0; + + if (NULL==(fp=fopen(fname, "rt"))) + return(-1); + + // Clear valid flag + for (ieph=0; iephalpha0 = atof(tmp); + + strncpy(tmp, str+14, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->alpha1 = atof(tmp); + + strncpy(tmp, str+26, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->alpha2 = atof(tmp); + + strncpy(tmp, str+38, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->alpha3 = atof(tmp); + + //read wntlsf, dn, and dtlsf from fil + + flags |= 0x1; + } + else if (strncmp(str+60, "ION BETA", 8)==0) + { + strncpy(tmp, str+2, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->beta0 = atof(tmp); + + strncpy(tmp, str+14, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->beta1 = atof(tmp); + + strncpy(tmp, str+26, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->beta2 = atof(tmp); + + strncpy(tmp, str+38, 12); + tmp[12] = 0; + replaceExpDesignator(tmp, 12); + ionoutc->beta3 = atof(tmp); + + flags |= 0x1<<1; + } + else if (strncmp(str+60, "DELTA-UTC", 9)==0) + { + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + ionoutc->A0 = atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + ionoutc->A1 = atof(tmp); + + strncpy(tmp, str+41, 9); + tmp[9] = 0; + ionoutc->tot = atoi(tmp); + + strncpy(tmp, str+50, 9); + tmp[9] = 0; + ionoutc->wnt = atoi(tmp); + + if (ionoutc->tot%4096==0) + flags |= 0x1<<2; + } + else if (strncmp(str+60, "LEAP SECONDS", 12)==0) + { + strncpy(tmp, str, 6); + tmp[6] = 0; + ionoutc->dtls = atoi(tmp); + + flags |= 0x1<<3; + } + } + + ionoutc->vflg = FALSE; + if (flags==0xF) // Read all Iono/UTC lines + ionoutc->vflg = TRUE; + + // Read ephemeris blocks + g0.week = -1; + ieph = 0; + + while (1) + { + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + // PRN + strncpy(tmp, str, 2); + tmp[2] = 0; + sv = atoi(tmp)-1; + + // EPOCH + strncpy(tmp, str+3, 2); + tmp[2] = 0; + t.y = atoi(tmp) + 2000; + + strncpy(tmp, str+6, 2); + tmp[2] = 0; + t.m = atoi(tmp); + + strncpy(tmp, str+9, 2); + tmp[2] = 0; + t.d = atoi(tmp); + + strncpy(tmp, str+12, 2); + tmp[2] = 0; + t.hh = atoi(tmp); + + strncpy(tmp, str+15, 2); + tmp[2] = 0; + t.mm = atoi(tmp); + + strncpy(tmp, str+18, 4); + tmp[2] = 0; + t.sec = atof(tmp); + + date2gps(&t, &g); + + if (g0.week==-1) + g0 = g; + + // Check current time of clock + dt = subGpsTime(g, g0); + + if (dt>SECONDS_IN_HOUR) + { + g0 = g; + ieph++; // a new set of ephemerides + + if (ieph>=EPHEM_ARRAY_SIZE) + break; + } + + // Date and time + eph[ieph][sv].t = t; + + // SV CLK + eph[ieph][sv].toc = g; + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); // tmp[15]='E'; + eph[ieph][sv].af0 = atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].af1 = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].af2 = atof(tmp); + + // BROADCAST ORBIT - 1 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].iode = (int)atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].crs = atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].deltan = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].m0 = atof(tmp); + + // BROADCAST ORBIT - 2 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].cuc = atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].ecc = atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].cus = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].sqrta = atof(tmp); + + // BROADCAST ORBIT - 3 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].toe.sec = atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].cic = atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].omg0 = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].cis = atof(tmp); + + // BROADCAST ORBIT - 4 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].inc0 = atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].crc = atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].aop = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].omgdot = atof(tmp); + + // BROADCAST ORBIT - 5 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+3, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].idot = atof(tmp); + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].codeL2 = (int)atof(tmp); + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].toe.week = (int)atof(tmp); + + // BROADCAST ORBIT - 6 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + strncpy(tmp, str+22, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].svhlth = (int)atof(tmp); + if ((eph[ieph][sv].svhlth>0) && (eph[ieph][sv].svhlth<32)) + eph[ieph][sv].svhlth += 32; // Set MSB to 1 + + strncpy(tmp, str+41, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].tgd = atof(tmp); + + strncpy(tmp, str+60, 19); + tmp[19] = 0; + replaceExpDesignator(tmp, 19); + eph[ieph][sv].iodc = (int)atof(tmp); + + // BROADCAST ORBIT - 7 + if (NULL==fgets(str, MAX_CHAR, fp)) + break; + + // Set valid flag + eph[ieph][sv].vflg = 1; + + // Update the working variables + eph[ieph][sv].A = eph[ieph][sv].sqrta * eph[ieph][sv].sqrta; + eph[ieph][sv].n = sqrt(GM_EARTH/(eph[ieph][sv].A*eph[ieph][sv].A*eph[ieph][sv].A)) + eph[ieph][sv].deltan; + eph[ieph][sv].sq1e2 = sqrt(1.0 - eph[ieph][sv].ecc*eph[ieph][sv].ecc); + eph[ieph][sv].omgkdot = eph[ieph][sv].omgdot - OMEGA_EARTH; + } + + fclose(fp); + + if (g0.week>=0) + ieph += 1; // Number of sets of ephemerides + + return(ieph); +} + +double ionosphericDelay(const ionoutc_t *ionoutc, gpstime_t g, double *llh, double *azel) +{ + double iono_delay = 0.0; + double E,phi_u,lam_u,F; + + if (ionoutc->enable==FALSE) + return (0.0); // No ionospheric delay + + E = azel[1]/PI; + phi_u = llh[0]/PI; + lam_u = llh[1]/PI; + + // Obliquity factor + F = 1.0 + 16.0*pow((0.53 - E),3.0); + + if (ionoutc->vflg==FALSE) + iono_delay = F*5.0e-9*SPEED_OF_LIGHT; + else + { + double t,psi,phi_i,lam_i,phi_m,phi_m2,phi_m3; + double AMP,PER,X,X2,X4; + + // Earth's central angle between the user position and the earth projection of + // ionospheric intersection point (semi-circles) + psi = 0.0137/(E + 0.11) - 0.022; + + // Geodetic latitude of the earth projection of the ionospheric intersection point + // (semi-circles) + phi_i = phi_u + psi*cos(azel[0]); + if(phi_i>0.416) + phi_i = 0.416; + else if(phi_i<-0.416) + phi_i = -0.416; + + // Geodetic longitude of the earth projection of the ionospheric intersection point + // (semi-circles) + lam_i = lam_u + psi*sin(azel[0])/cos(phi_i*PI); + + // Geomagnetic latitude of the earth projection of the ionospheric intersection + // point (mean ionospheric height assumed 350 km) (semi-circles) + phi_m = phi_i + 0.064*cos((lam_i - 1.617)*PI); + phi_m2 = phi_m*phi_m; + phi_m3 = phi_m2*phi_m; + + AMP = ionoutc->alpha0 + ionoutc->alpha1*phi_m + + ionoutc->alpha2*phi_m2 + ionoutc->alpha3*phi_m3; + if (AMP<0.0) + AMP = 0.0; + + PER = ionoutc->beta0 + ionoutc->beta1*phi_m + + ionoutc->beta2*phi_m2 + ionoutc->beta3*phi_m3; + if (PER<72000.0) + PER = 72000.0; + + // Local time (sec) + t = SECONDS_IN_DAY/2.0*lam_i + g.sec; + while(t>=SECONDS_IN_DAY) + t -= SECONDS_IN_DAY; + while(t<0) + t += SECONDS_IN_DAY; + + // Phase (radians) + X = 2.0*PI*(t - 50400.0)/PER; + + if(fabs(X)<1.57) + { + X2 = X*X; + X4 = X2*X2; + iono_delay = F*(5.0e-9 + AMP*(1.0 - X2/2.0 + X4/24.0))*SPEED_OF_LIGHT; + } + else + iono_delay = F*5.0e-9*SPEED_OF_LIGHT; + } + + return (iono_delay); +} + +/*! \brief Compute range between a satellite and the receiver + * \param[out] rho The computed range + * \param[in] eph Ephemeris data of the satellite + * \param[in] g GPS time at time of receiving the signal + * \param[in] xyz position of the receiver + */ +void computeRange(range_t *rho, ephem_t eph, ionoutc_t *ionoutc, gpstime_t g, double xyz[]) +{ + double pos[3],vel[3],clk[2]; + double los[3]; + double tau; + double range,rate; + double xrot,yrot; + + double llh[3],neu[3]; + double tmat[3][3]; + + // 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. + xrot = pos[0] + pos[1]*OMEGA_EARTH*tau; + yrot = pos[1] - pos[0]*OMEGA_EARTH*tau; + pos[0] = xrot; + pos[1] = yrot; + + // New observer to satellite vector and satellite range. + subVect(los, pos, xyz); + range = normVect(los); + rho->d = range; + + // 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; + + // Azimuth and elevation angles. + xyz2llh(xyz, llh); + ltcmat(llh, tmat); + ecef2neu(los, tmat, neu); + neu2azel(rho->azel, neu); + + // Add ionospheric delay + rho->iono_delay = ionosphericDelay(ionoutc, g, llh, rho->azel); + rho->range += rho->iono_delay; + + return; +} + +/*! \brief Compute the code phase for a given channel (satellite) + * \param chan Channel on which we operate (is updated) + * \param[in] rho1 Current range, after \a dt has expired + * \param[in dt delta-t (time difference) in seconds + */ +void computeCodePhase(channel_t *chan, range_t rho1, double dt) +{ + double ms; + int ims; + double rhorate; + + // Pseudorange rate. + rhorate = (rho1.range - chan->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 = ((subGpsTime(chan->rho0.g,chan->g0)+6.0) - chan->rho0.range/SPEED_OF_LIGHT)*1000.0; + + ims = (int)ms; + chan->code_phase = (ms-(double)ims)*CA_SEQ_LEN; // 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; + + // Save current pseudorange + chan->rho0 = rho1; + + return; +} + +/*! \brief Read the list of user motions from the input file + * \param[out] xyz Output array of ECEF vectors for user motion + * \param[[in] filename File name of the text input file + * \returns Number of user data motion records read, -1 on error + */ +int readUserMotion(double xyz[USER_MOTION_SIZE][3], const char *filename) +{ + FILE *fp; + int numd; + char str[MAX_CHAR]; + double t,x,y,z; + + if (NULL==(fp=fopen(filename,"rt"))) + return(-1); + + for (numd=0; numd 90.0 || llh[0] < -90.0 || llh[1]>180.0 || llh[1] < -180.0) + { + fprintf(stderr, "ERROR: Invalid file format (time[s], latitude[deg], longitude[deg], height [m].\n"); + numd = 0; // Empty user motion + break; + } + + llh[0] /= R2D; // convert to RAD + llh[1] /= R2D; // convert to RAD + + llh2xyz(llh, xyz[numd]); + } + + fclose(fp); + + return (numd); +} + +int readNmeaGGA(double xyz[USER_MOTION_SIZE][3], const char *filename) +{ + FILE *fp; + int numd = 0; + char str[MAX_CHAR]; + char *token; + double llh[3],pos[3]; + char tmp[8]; + + if (NULL==(fp=fopen(filename,"rt"))) + return(-1); + + while (1) + { + if (fgets(str, MAX_CHAR, fp)==NULL) + break; + + token = strtok(str, ","); + + if (strncmp(token+3, "GGA", 3)==0) + { + token = strtok(NULL, ","); // Date and time + + token = strtok(NULL, ","); // Latitude + strncpy(tmp, token, 2); + tmp[2] = 0; + + llh[0] = atof(tmp) + atof(token+2)/60.0; + + token = strtok(NULL, ","); // North or south + if (token[0]=='S') + llh[0] *= -1.0; + + llh[0] /= R2D; // in radian + + token = strtok(NULL, ","); // Longitude + strncpy(tmp, token, 3); + tmp[3] = 0; + + llh[1] = atof(tmp) + atof(token+3)/60.0; + + token = strtok(NULL, ","); // East or west + if (token[0]=='W') + llh[1] *= -1.0; + + llh[1] /= R2D; // in radian + + token = strtok(NULL, ","); // GPS fix + token = strtok(NULL, ","); // Number of satellites + token = strtok(NULL, ","); // HDOP + + token = strtok(NULL, ","); // Altitude above meas sea level + + llh[2] = atof(token); + + token = strtok(NULL, ","); // in meter + + token = strtok(NULL, ","); // Geoid height above WGS84 ellipsoid + + llh[2] += atof(token); + + // Convert geodetic position into ECEF coordinates + llh2xyz(llh, pos); + + xyz[numd][0] = pos[0]; + xyz[numd][1] = pos[1]; + xyz[numd][2] = pos[2]; + + // Update the number of track points + numd++; + + if (numd>=USER_MOTION_SIZE) + break; + } + } + + fclose(fp); + + return (numd); +} + +int generateNavMsg(gpstime_t g, channel_t *chan, int init) +{ + int iwrd,isbf; + gpstime_t g0; + unsigned long wn,tow; + unsigned sbfwrd; + unsigned long prevwrd; + int nib; + + g0.week = g.week; + g0.sec = (double)(((unsigned long)(g.sec+0.5))/30UL) * 30.0; // Align with the full frame length = 30 sec + chan->g0 = g0; // Data bit reference time + + wn = (unsigned long)(g0.week%1024); + tow = ((unsigned long)g0.sec)/6UL; + + if (init==1) // Initialize subframe 5 + { + prevwrd = 0UL; + + for (iwrd=0; iwrdsbf[4][iwrd]; + + // Add TOW-count message into HOW + if (iwrd==1) + sbfwrd |= ((tow&0x1FFFFUL)<<13); + + // Compute checksum + sbfwrd |= (prevwrd<<30) & 0xC0000000UL; // 2 LSBs of the previous transmitted word + nib = ((iwrd==1)||(iwrd==9))?1:0; // Non-information bearing bits for word 2 and 10 + chan->dwrd[iwrd] = computeChecksum(sbfwrd, nib); + + prevwrd = chan->dwrd[iwrd]; + } + } + else // Save subframe 5 + { + for (iwrd=0; iwrddwrd[iwrd] = chan->dwrd[N_DWRD_SBF*N_SBF+iwrd]; + + prevwrd = chan->dwrd[iwrd]; + } + /* + // Sanity check + if (((chan->dwrd[1])&(0x1FFFFUL<<13)) != ((tow&0x1FFFFUL)<<13)) + { + fprintf(stderr, "\nWARNING: Invalid TOW in subframe 5.\n"); + return(0); + } + */ + } + + for (isbf=0; isbfsbf[isbf][iwrd]; + + // Add transmission week number to Subframe 1 + if ((isbf==0)&&(iwrd==2)) + sbfwrd |= (wn&0x3FFUL)<<20; + + // Add TOW-count message into HOW + if (iwrd==1) + sbfwrd |= ((tow&0x1FFFFUL)<<13); + + // Compute checksum + sbfwrd |= (prevwrd<<30) & 0xC0000000UL; // 2 LSBs of the previous transmitted word + nib = ((iwrd==1)||(iwrd==9))?1:0; // Non-information bearing bits for word 2 and 10 + chan->dwrd[(isbf+1)*N_DWRD_SBF+iwrd] = computeChecksum(sbfwrd, nib); + + prevwrd = chan->dwrd[(isbf+1)*N_DWRD_SBF+iwrd]; + } + } + + return(1); +} + +int checkSatVisibility(ephem_t eph, gpstime_t g, double *xyz, double elvMask, double *azel) +{ + double llh[3],neu[3]; + double pos[3],vel[3],clk[3],los[3]; + double tmat[3][3]; + + if (eph.vflg != 1) + return (-1); // Invalid + + xyz2llh(xyz,llh); + ltcmat(llh, tmat); + + satpos(eph, g, pos, vel, clk); + subVect(los, pos, xyz); + ecef2neu(los, tmat, neu); + neu2azel(azel, neu); + + if (azel[1]*R2D > elvMask) + return (1); // Visible + // else + return (0); // Invisible +} + +int allocateChannel(channel_t *chan, ephem_t *eph, ionoutc_t ionoutc, gpstime_t grx, double *xyz, double elvMask) +{ + int nsat=0; + int i,sv; + double azel[2]; + + range_t rho; + double ref[3]={0.0}; + double r_ref,r_xyz; + double phase_ini; + + for (sv=0; sv=0) // Not visible but allocated + { + // Clear channel + chan[allocatedSat[sv]].prn = 0; + + // Clear satellite allocation flag + allocatedSat[sv] = -1; + } + } + + return(nsat); +} + +void usage(void) +{ + fprintf(stderr, "Usage: gps-sdr-sim [options]\n" + "Options:\n" + " -e RINEX navigation file for GPS ephemerides (required)\n" + " -u User motion file in ECEF x, y, z format (dynamic mode)\n" + " -x User motion file in lat, lon, height format (dynamic mode)\n" + " -g NMEA GGA stream (dynamic mode)\n" + " -c ECEF X,Y,Z in meters (static mode) e.g. 3967283.154,1022538.181,4872414.484\n" + " -l Lat, lon, height (static mode) e.g. 35.681298,139.766247,10.0\n" + " -L User leap future event in GPS week number, day number, next leap second e.g. 2347,3,19\n" + " -t Scenario start time YYYY/MM/DD,hh:mm:ss\n" + " -T Overwrite TOC and TOE to scenario start time\n" + " -d Duration [sec] (dynamic mode max: %.0f, static mode max: %d)\n" + " -o I/Q sampling data file (default: gpssim.bin)\n" + " -s Sampling frequency [Hz] (default: 2600000)\n" + " -b I/Q data format [1/8/16] (default: 16)\n" + " -i Disable ionospheric delay for spacecraft scenario\n" + " -p [fixed_gain] Disable path loss and hold power level constant\n" + " -v Show details about simulated channels\n", + ((double)USER_MOTION_SIZE) / 10.0, STATIC_MAX_DURATION); + + return; +} + +#if 0 +int main(int argc, char *argv[]) +{ + clock_t tstart,tend; + + FILE *fp; + + int sv; + int neph,ieph; + ephem_t eph[EPHEM_ARRAY_SIZE][MAX_SAT]; + gpstime_t g0; + + double llh[3]; + + int i; + channel_t chan[MAX_CHAN]; + double elvmask = 0.0; // in degree + + int ip,qp; + int iTable; + short *iq_buff = NULL; + signed char *iq8_buff = NULL; + + gpstime_t grx; + double delt; + int isamp; + + int iumd; + int numd; + char umfile[MAX_CHAR]; + + + int staticLocationMode = FALSE; + int nmeaGGA = FALSE; + int umLLH = FALSE; + + char navfile[MAX_CHAR]; + char outfile[MAX_CHAR]; + + double samp_freq; + int iq_buff_size; + int data_format; + + int result; + + int gain[MAX_CHAN]; + double path_loss; + double ant_gain; + int fixed_gain = 128; + double ant_pat[37]; + int ibs; // boresight angle index + + datetime_t t0,tmin,tmax; + gpstime_t gmin,gmax; + double dt; + int igrx; + + double duration; + int iduration; + int verb; + + int timeoverwrite = FALSE; // Overwrite the TOC and TOE in the RINEX file + + ionoutc_t ionoutc; + int path_loss_enable = TRUE; + + //////////////////////////////////////////////////////////// + // Read options + //////////////////////////////////////////////////////////// + + // Default options + navfile[0] = 0; + umfile[0] = 0; + strcpy(outfile, "gpssim.bin"); + samp_freq = 2.6e6; + data_format = SC16; + g0.week = -1; // Invalid start time + iduration = USER_MOTION_SIZE; + duration = (double)iduration/10.0; // Default duration + verb = FALSE; + ionoutc.enable = TRUE; + ionoutc.leapen = FALSE; + + if (argc<3) + { + usage(); + exit(1); + } + + while ((result=getopt(argc,argv,"e:u:x:g:c:l:o:s:b:L:T:t:d:ipv"))!=-1) + { + switch (result) + { + case 'e': + strcpy(navfile, optarg); + break; + case 'u': + strcpy(umfile, optarg); + nmeaGGA = FALSE; + umLLH = FALSE; + break; + case 'x': + // Added by romalvarezllorens@gmail.com + strcpy(umfile, optarg); + umLLH = TRUE; + break; + case 'g': + strcpy(umfile, optarg); + nmeaGGA = TRUE; + break; + case 'c': + // Static ECEF coordinates input mode + staticLocationMode = TRUE; + sscanf(optarg,"%lf,%lf,%lf",&xyz[0][0],&xyz[0][1],&xyz[0][2]); + break; + case 'l': + // Static geodetic coordinates input mode + // Added by scateu@gmail.com + staticLocationMode = TRUE; + sscanf(optarg,"%lf,%lf,%lf",&llh[0],&llh[1],&llh[2]); + llh[0] = llh[0] / R2D; // convert to RAD + llh[1] = llh[1] / R2D; // convert to RAD + llh2xyz(llh,xyz[0]); // Convert llh to xyz + break; + case 'o': + strcpy(outfile, optarg); + break; + case 's': + samp_freq = atof(optarg); + if (samp_freq<1.0e6) + { + fprintf(stderr, "ERROR: Invalid sampling frequency.\n"); + exit(1); + } + break; + case 'b': + data_format = atoi(optarg); + if (data_format!=SC01 && data_format!=SC08 && data_format!=SC16) + { + fprintf(stderr, "ERROR: Invalid I/Q data format.\n"); + exit(1); + } + break; + case 'L': + // enable custom Leap Event + ionoutc.leapen = TRUE; + sscanf(optarg,"%d,%d,%d", &ionoutc.wnlsf, &ionoutc.dn, &ionoutc.dtlsf); + if (ionoutc.dn<1 && ionoutc.dn>7) + { + fprintf(stderr, "ERROR: Invalid GPS day number"); + exit(1); + } + if (ionoutc.wnlsf<0) + { + fprintf(stderr, "ERROR: Invalid GPS week number"); + exit(1); + } + if (ionoutc.dtlsf<-128 && ionoutc.dtlsf>127) + { + fprintf(stderr, "ERROR: Invalid delta leap second"); + exit(1); + } + break; + case 'T': + timeoverwrite = TRUE; + if (strncmp(optarg, "now", 3)==0) + { + time_t timer; + struct tm *gmt; + + time(&timer); + gmt = gmtime(&timer); + + t0.y = gmt->tm_year+1900; + t0.m = gmt->tm_mon+1; + t0.d = gmt->tm_mday; + t0.hh = gmt->tm_hour; + t0.mm = gmt->tm_min; + t0.sec = (double)gmt->tm_sec; + + date2gps(&t0, &g0); + + break; + } + case 't': + sscanf(optarg, "%d/%d/%d,%d:%d:%lf", &t0.y, &t0.m, &t0.d, &t0.hh, &t0.mm, &t0.sec); + if (t0.y<=1980 || t0.m<1 || t0.m>12 || t0.d<1 || t0.d>31 || + t0.hh<0 || t0.hh>23 || t0.mm<0 || t0.mm>59 || t0.sec<0.0 || t0.sec>=60.0) + { + fprintf(stderr, "ERROR: Invalid date and time.\n"); + exit(1); + } + t0.sec = floor(t0.sec); + date2gps(&t0, &g0); + break; + case 'd': + duration = atof(optarg); + break; + case 'i': + ionoutc.enable = FALSE; // Disable ionospheric correction + break; + case 'p': + if (optind < argc && argv[optind][0] != '-') // Check if next item is an argument + { + fixed_gain = atoi(argv[optind]); + if (fixed_gain < 1 || fixed_gain > 128) + { + fprintf(stderr, "ERROR: Fixed gain must be between 1 and 128.\n"); + exit(1); + } + optind++; // Move past this argument for next iteration + } + path_loss_enable = FALSE; // Disable path loss + break; + case 'v': + verb = TRUE; + break; + case ':': + case '?': + usage(); + exit(1); + default: + break; + } + } + + if (navfile[0]==0) + { + fprintf(stderr, "ERROR: GPS ephemeris file is not specified.\n"); + exit(1); + } + + if (umfile[0]==0 && !staticLocationMode) + { + // Default static location; Tokyo + staticLocationMode = TRUE; + llh[0] = 35.681298 / R2D; + llh[1] = 139.766247 / R2D; + llh[2] = 10.0; + } + + if (duration<0.0 || (duration>((double)USER_MOTION_SIZE)/10.0 && !staticLocationMode) || (duration>STATIC_MAX_DURATION && staticLocationMode)) + { + fprintf(stderr, "ERROR: Invalid duration.\n"); + exit(1); + } + iduration = (int)(duration*10.0 + 0.5); + + // Buffer size + samp_freq = floor(samp_freq/10.0); + iq_buff_size = (int)samp_freq; // samples per 0.1sec + samp_freq *= 10.0; + + delt = 1.0/samp_freq; + + //////////////////////////////////////////////////////////// + // Receiver position + //////////////////////////////////////////////////////////// + + if (!staticLocationMode) + { + // Read user motion file + if (nmeaGGA==TRUE) + numd = readNmeaGGA(xyz, umfile); + else if (umLLH == TRUE) + numd = readUserMotionLLH(xyz, umfile); + else + numd = readUserMotion(xyz, umfile); + + if (numd==-1) + { + fprintf(stderr, "ERROR: Failed to open user motion / NMEA GGA file.\n"); + exit(1); + } + else if (numd==0) + { + fprintf(stderr, "ERROR: Failed to read user motion / NMEA GGA data.\n"); + exit(1); + } + + // Set simulation duration + if (numd>iduration) + numd = iduration; + + // Set user initial position + xyz2llh(xyz[0], llh); + } + else + { + // Static geodetic coordinates input mode: "-l" + // Added by scateu@gmail.com + fprintf(stderr, "Using static location mode.\n"); + + // Set simulation duration + numd = iduration; + + // Set user initial position + llh2xyz(llh, xyz[0]); + } + + fprintf(stderr, "xyz = %11.1f, %11.1f, %11.1f\n", xyz[0][0], xyz[0][1], xyz[0][2]); + fprintf(stderr, "llh = %11.6f, %11.6f, %11.1f\n", llh[0]*R2D, llh[1]*R2D, llh[2]); + + //////////////////////////////////////////////////////////// + // Read ephemeris + //////////////////////////////////////////////////////////// + + neph = readRinexNavAll(eph, &ionoutc, navfile); + + if (neph==0) + { + fprintf(stderr, "ERROR: No ephemeris available.\n"); + exit(1); + } + else if (neph==-1) + { + fprintf(stderr, "ERROR: ephemeris file not found.\n"); + exit(1); + } + + if ((verb==TRUE)&&(ionoutc.vflg==TRUE)) + { + fprintf(stderr, " %12.3e %12.3e %12.3e %12.3e\n", + ionoutc.alpha0, ionoutc.alpha1, ionoutc.alpha2, ionoutc.alpha3); + fprintf(stderr, " %12.3e %12.3e %12.3e %12.3e\n", + ionoutc.beta0, ionoutc.beta1, ionoutc.beta2, ionoutc.beta3); + fprintf(stderr, " %19.11e %19.11e %9d %9d\n", + ionoutc.A0, ionoutc.A1, ionoutc.tot, ionoutc.wnt); + fprintf(stderr, "%6d\n", ionoutc.dtls); + } + + for (sv=0; sv=0) // Scenario start time has been set. + { + if (timeoverwrite==TRUE) + { + gpstime_t gtmp; + datetime_t ttmp; + double dsec; + + gtmp.week = g0.week; + gtmp.sec = (double)(((int)(g0.sec))/7200)*7200.0; + + dsec = subGpsTime(gtmp,gmin); + + // Overwrite the UTC reference week number + ionoutc.wnt = gtmp.week; + ionoutc.tot = (int)gtmp.sec; + + // Iono/UTC parameters may no longer valid + //ionoutc.vflg = FALSE; + + // Overwrite the TOC and TOE to the scenario start time + for (sv=0; sv=-SECONDS_IN_HOUR && dt=0) // ieph has been set + break; + } + + if (ieph == -1) + { + fprintf(stderr, "ERROR: No current set of ephemerides has been found.\n"); + exit(1); + } + + //////////////////////////////////////////////////////////// + // Baseband signal buffer and output file + //////////////////////////////////////////////////////////// + + // Allocate I/Q buffer + iq_buff = calloc(2*iq_buff_size, 2); + + if (iq_buff==NULL) + { + fprintf(stderr, "ERROR: Failed to allocate 16-bit I/Q buffer.\n"); + exit(1); + } + + if (data_format==SC08) + { + iq8_buff = calloc(2*iq_buff_size, 1); + if (iq8_buff==NULL) + { + fprintf(stderr, "ERROR: Failed to allocate 8-bit I/Q buffer.\n"); + exit(1); + } + } + else if (data_format==SC01) + { + iq8_buff = calloc(iq_buff_size/4, 1); // byte = {I0, Q0, I1, Q1, I2, Q2, I3, Q3} + if (iq8_buff==NULL) + { + fprintf(stderr, "ERROR: Failed to allocate compressed 1-bit I/Q buffer.\n"); + exit(1); + } + } + + // Open output file + // "-" can be used as name for stdout + if(strcmp("-", outfile)){ + if (NULL==(fp=fopen(outfile,"wb"))) + { + fprintf(stderr, "ERROR: Failed to open output file.\n"); + exit(1); + } + }else{ + fp = stdout; + } + + //////////////////////////////////////////////////////////// + // Initialize channels + //////////////////////////////////////////////////////////// + + // Clear all channels + for (i=0; i0) + fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn, + chan[i].azel[0]*R2D, chan[i].azel[1]*R2D, chan[i].rho0.d, chan[i].rho0.iono_delay); + } + + //////////////////////////////////////////////////////////// + // Receiver antenna gain pattern + //////////////////////////////////////////////////////////// + + for (i=0; i<37; i++) + ant_pat[i] = pow(10.0, -ant_pat_db[i]/20.0); + + //////////////////////////////////////////////////////////// + // Generate baseband signals + //////////////////////////////////////////////////////////// + + tstart = clock(); + + // Update receiver time + grx = incGpsTime(grx, 0.1); + + for (iumd=1; iumd0) + { + // Refresh code phase and data bit counters + range_t rho; + sv = chan[i].prn-1; + + // Current pseudorange + if (!staticLocationMode) + computeRange(&rho, eph[ieph][sv], &ionoutc, grx, xyz[iumd]); + else + computeRange(&rho, eph[ieph][sv], &ionoutc, grx, xyz[0]); + + chan[i].azel[0] = rho.azel[0]; + chan[i].azel[1] = rho.azel[1]; + + // Update code phase and data bit counters + computeCodePhase(&chan[i], rho, 0.1); +#ifndef FLOAT_CARR_PHASE + chan[i].carr_phasestep = (int)round(512.0 * 65536.0 * chan[i].f_carr * delt); +#endif + // Path loss + path_loss = 20200000.0/rho.d; + + // Receiver antenna gain + ibs = (int)((90.0-rho.azel[1]*R2D)/5.0); // covert elevation to boresight + ant_gain = ant_pat[ibs]; + + // Signal gain + if (path_loss_enable == TRUE) + gain[i] = (int)(path_loss * ant_gain * 128.0); // scaled by 2^7 + else + gain[i] = fixed_gain; // hold the power level constant + } + } + + for (isamp=0; isamp0) + { +#ifdef FLOAT_CARR_PHASE + iTable = (int)floor(chan[i].carr_phase*512.0); +#else + iTable = (chan[i].carr_phase >> 16) & 0x1ff; // 9-bit index +#endif + ip = chan[i].dataBit * chan[i].codeCA * cosTable512[iTable] * gain[i]; + qp = chan[i].dataBit * chan[i].codeCA * sinTable512[iTable] * gain[i]; + + // Accumulate for all visible satellites + i_acc += ip; + q_acc += qp; + + // Update code phase + chan[i].code_phase += chan[i].f_code * delt; + + if (chan[i].code_phase>=CA_SEQ_LEN) + { + chan[i].code_phase -= CA_SEQ_LEN; + + 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++; + /* + if (chan[i].iword>=N_DWRD) + fprintf(stderr, "\nWARNING: Subframe word buffer overflow.\n"); + */ + } + + // Set new navigation data bit + chan[i].dataBit = (int)((chan[i].dwrd[chan[i].iword]>>(29-chan[i].ibit)) & 0x1UL)*2-1; + } + } + + // Set current code chip + chan[i].codeCA = chan[i].ca[(int)chan[i].code_phase]*2-1; + + // Update carrier phase +#ifdef FLOAT_CARR_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; +#else + chan[i].carr_phase += chan[i].carr_phasestep; +#endif + } + } + + // Scaled by 2^7 + i_acc = (i_acc+64)>>7; + q_acc = (q_acc+64)>>7; + + // Store I/Q samples into buffer + iq_buff[isamp*2] = (short)i_acc; + iq_buff[isamp*2+1] = (short)q_acc; + } + + if (data_format==SC01) + { + for (isamp=0; isamp<2*iq_buff_size; isamp++) + { + if (isamp%8==0) + iq8_buff[isamp/8] = 0x00; + + iq8_buff[isamp/8] |= (iq_buff[isamp]>0?0x01:0x00)<<(7-isamp%8); + } + + fwrite(iq8_buff, 1, iq_buff_size/4, fp); + } + else if (data_format==SC08) + { + for (isamp=0; isamp<2*iq_buff_size; isamp++) + { + iq8_buff[isamp] = iq_buff[isamp]>>4; // 12-bit bladeRF -> 8-bit HackRF + //iq8_buff[isamp] = iq_buff[isamp] >> 8; // for PocketSDR + } + + fwrite(iq8_buff, 1, 2*iq_buff_size, fp); + } + else // data_format==SC16 + { + fwrite(iq_buff, 2, 2*iq_buff_size, fp); + } + + // + // Update navigation message and channel allocation every 30 seconds + // + + igrx = (int)(grx.sec*10.0+0.5); + + if (igrx%300==0) // Every 30 seconds + { + // Update navigation message + for (i=0; i0) + generateNavMsg(grx, &chan[i], 0); + } + + // Refresh ephemeris and subframes + // Quick and dirty fix. Need more elegant way. + for (sv=0; sv0) + fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn, + chan[i].azel[0]*R2D, chan[i].azel[1]*R2D, chan[i].rho0.d, chan[i].rho0.iono_delay); + } + } + } + + // Update receiver time + grx = incGpsTime(grx, 0.1); + + // Update time counter + fprintf(stderr, "\rTime into run = %4.1f", subGpsTime(grx, g0)); + fflush(stdout); + } + + tend = clock(); + + fprintf(stderr, "\nDone!\n"); + + // Free I/Q buffer + free(iq_buff); + + // Close file + fclose(fp); + + // Process time + fprintf(stderr, "Process time = %.1f [sec]\n", (double)(tend-tstart)/CLOCKS_PER_SEC); + + return(0); +} +#endif diff --git a/gpssim.h b/gpssim.h new file mode 100644 index 00000000..4175bb98 --- /dev/null +++ b/gpssim.h @@ -0,0 +1,187 @@ +#ifndef GPSSIM_H +#define GPSSIM_H + +//#define FLOAT_CARR_PHASE // For RKT simulation. Higher computational load, but smoother carrier phase. + +#define TRUE (1) +#define FALSE (0) + +/*! \brief Maximum length of a line in a text file (RINEX, motion) */ +#define MAX_CHAR (100) + +/*! \brief Maximum number of satellites in RINEX file */ +#define MAX_SAT (32) + +/*! \brief Maximum number of channels we simulate */ +#define MAX_CHAN (16) + +/*! \brief Maximum number of user motion points */ +#ifndef USER_MOTION_SIZE +#define USER_MOTION_SIZE (3000) // max duration at 10Hz +#endif + +/*! \brief Maximum duration for static mode*/ +#define STATIC_MAX_DURATION (86400) // second + +/*! \brief Number of subframes */ +#define N_SBF (5) // 5 subframes per frame + +/*! \brief Number of words per subframe */ +#define N_DWRD_SBF (10) // 10 word per subframe + +/*! \brief Number of words */ +#define N_DWRD ((N_SBF+1)*N_DWRD_SBF) // Subframe word buffer size + +/*! \brief C/A code sequence length */ +#define CA_SEQ_LEN (1023) + +#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 + +#define POW2_M50 8.881784197001252e-016 +#define POW2_M30 9.313225746154785e-010 +#define POW2_M27 7.450580596923828e-009 +#define POW2_M24 5.960464477539063e-008 + +// 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 WGS84_RADIUS 6378137.0 +#define WGS84_ECCENTRICITY 0.0818191908426 + +#define R2D 57.2957795131 + +#define SPEED_OF_LIGHT 2.99792458e8 +#define LAMBDA_L1 0.190293672798365 + +/*! \brief GPS L1 Carrier frequency */ +#define CARR_FREQ (1575.42e6) +/*! \brief C/A code frequency */ +#define CODE_FREQ (1.023e6) +#define CARR_TO_CODE (1.0/1540.0) + +// Sampling data format +#define SC01 (1) +#define SC08 (8) +#define SC16 (16) + +#define EPHEM_ARRAY_SIZE (15) // for daily GPS broadcast ephemers file (brdc) + +/*! \brief Structure representing GPS time */ +typedef struct +{ + int week; /*!< GPS week number (since January 1980) */ + double sec; /*!< second inside the GPS \a week */ +} gpstime_t; + +/*! \brief Structure repreenting UTC time */ +typedef struct +{ + int y; /*!< Calendar year */ + int m; /*!< Calendar month */ + int d; /*!< Calendar day */ + int hh; /*!< Calendar hour */ + int mm; /*!< Calendar minutes */ + double sec; /*!< Calendar seconds */ +} datetime_t; + +/*! \brief Structure representing ephemeris of a single satellite */ +typedef struct +{ + int vflg; /*!< Valid Flag */ + datetime_t t; + gpstime_t toc; /*!< Time of Clock */ + gpstime_t toe; /*!< Time of Ephemeris */ + int iodc; /*!< Issue of Data, Clock */ + int iode; /*!< Isuse of Data, Ephemeris */ + double deltan; /*!< Delta-N (radians/sec) */ + double cuc; /*!< Cuc (radians) */ + double cus; /*!< Cus (radians) */ + double cic; /*!< Correction to inclination cos (radians) */ + double cis; /*!< Correction to inclination sin (radians) */ + double crc; /*!< Correction to radius cos (meters) */ + double crs; /*!< Correction to radius sin (meters) */ + double ecc; /*!< e Eccentricity */ + double sqrta; /*!< sqrt(A) (sqrt(m)) */ + double m0; /*!< Mean anamoly (radians) */ + double omg0; /*!< Longitude of the ascending node (radians) */ + double inc0; /*!< Inclination (radians) */ + double aop; + double omgdot; /*!< Omega dot (radians/s) */ + double idot; /*!< IDOT (radians/s) */ + double af0; /*!< Clock offset (seconds) */ + double af1; /*!< rate (sec/sec) */ + double af2; /*!< acceleration (sec/sec^2) */ + double tgd; /*!< Group delay L2 bias */ + int svhlth; + int codeL2; + // Working variables follow + double n; /*!< Mean motion (Average angular velocity) */ + double sq1e2; /*!< sqrt(1-e^2) */ + double A; /*!< Semi-major axis */ + double omgkdot; /*!< OmegaDot-OmegaEdot */ +} ephem_t; + +typedef struct +{ + int enable; + int vflg; + double alpha0,alpha1,alpha2,alpha3; + double beta0,beta1,beta2,beta3; + double A0,A1; + int dtls,tot,wnt; + int dtlsf,dn,wnlsf; + // enable custom leap event + int leapen; +} ionoutc_t; + +typedef struct +{ + gpstime_t g; + double range; // pseudorange + double rate; + double d; // geometric distance + double azel[2]; + double iono_delay; +} range_t; + +/*! \brief Structure representing a Channel */ +typedef struct +{ + int prn; /*< PRN Number */ + int ca[CA_SEQ_LEN]; /*< C/A Sequence */ + double f_carr; /*< Carrier frequency */ + double f_code; /*< Code frequency */ +#ifdef FLOAT_CARR_PHASE + double carr_phase; +#else + unsigned int carr_phase; /*< Carrier phase */ + int carr_phasestep; /*< Carrier phasestep */ +#endif + double code_phase; /*< Code phase */ + gpstime_t g0; /*!< GPS time at start */ + unsigned long sbf[5][N_DWRD_SBF]; /*!< current subframe */ + unsigned long dwrd[N_DWRD]; /*!< Data words of sub-frame */ + int iword; /*!< initial word */ + int ibit; /*!< initial bit */ + int icode; /*!< initial code */ + int dataBit; /*!< current data bit */ + int codeCA; /*!< current C/A code */ + double azel[2]; + range_t rho0; +} channel_t; + +#endif diff --git a/mainwindow.cpp b/mainwindow.cpp index 110e7739..a5cf4c62 100644 --- a/mainwindow.cpp +++ b/mainwindow.cpp @@ -1,5 +1,9 @@ #include "mainwindow.h" #include "ui_mainwindow.h" +#include "uhd_device.h" +#include +#include +#include MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) @@ -7,9 +11,143 @@ MainWindow::MainWindow(QWidget *parent) { ui->setupUi(this); this->setWindowTitle(tr("gnss sim V1.0")); + psettings = new QSettings("settings.ini", QSettings::IniFormat); + loadSetting(); } MainWindow::~MainWindow() { + saveSetting(); + psettings->sync(); delete ui; } + +void MainWindow::on_pushButton_start_clicked() +{ + if(ui->lineEdit_gpsfile->text().length() == 0) + { + QMessageBox::warning(NULL,tr("星历文件错误"),tr("请设置星历文件"),QMessageBox::Ok,QMessageBox::Ok); + return; + } + if(!QFile(ui->lineEdit_gpsfile->text()).exists()) + { + QMessageBox::warning(NULL, tr("星历文件路径错误"), tr("没有这个文件，请确认路径是否正确。"), QMessageBox::Ok, QMessageBox::Ok); + return; + } + if(ui->lineEdit_motionfile->text().length() == 0) + { + QMessageBox::warning(NULL, tr("轨迹文件错误"), tr("请设置轨迹文件。"), QMessageBox::Ok, QMessageBox::Ok); + return; + } + if(!QFile(ui->lineEdit_motionfile->text()).exists()) + { + QMessageBox::warning(NULL, tr("轨迹文件路径错误"), tr("没有这个文件，请确认路径是否正确。"), QMessageBox::Ok, QMessageBox::Ok); + return; + } + if(ui->lineEdit_gain->text().length() == 0) + { + QMessageBox::warning(NULL, tr("增益错误"), tr("请设置增益。"), QMessageBox::Ok, QMessageBox::Ok); + return; + } + float radio_gain = ui->lineEdit_gain->text().toFloat(); + workThread.setGain(radio_gain); + workThread.setNavFilePath(ui->lineEdit_gpsfile->text()); + workThread.setUmfile(ui->lineEdit_motionfile->text()); + if(ui->lineEdit_motionfile->text().endsWith(".csv")){ + workThread.useNmeaGGA(0); + }else{ + workThread.useNmeaGGA(1); + } + if(ui->checkBox_currentTime->isChecked()){ + workThread.useCurrentTime(true); + }else{ + workThread.useCurrentTime(false); + } + + workThread.start(); +} + + +void MainWindow::on_pushButton_stop_clicked() +{ + workThread.stop(); +} + + +void MainWindow::on_pushButton_browsegps_clicked() +{ + QString path = ui->lineEdit_gpsfile->text(); + QString fileName = QFileDialog::getOpenFileName( + this, + "", + path, + "(brdc*)"); + if (!fileName.isEmpty()) { + ui->lineEdit_gpsfile->setText(fileName); + } +} + + +void MainWindow::on_pushButton_browsemotion_clicked() +{ + QString path = ui->lineEdit_motionfile->text(); + QString fileName = QFileDialog::getOpenFileName( + this, + "", + path, + "*.csv *.txt"); + if (!fileName.isEmpty()) { + ui->lineEdit_motionfile->setText(fileName); + } +} + +void MainWindow::loadSetting() +{ + QDir dir = QDir::current(); + QStringList files; + + if(psettings->contains("/setting/gps_file")) + { + QVariant gpsfile = psettings->value("/setting/gps_file"); + ui->lineEdit_gpsfile->setText(gpsfile.toString()); + } + + if(psettings->contains("/setting/motion_file")) + { + QVariant motionfile = psettings->value("/setting/motion_file"); + ui->lineEdit_motionfile->setText(motionfile.toString()); + } + + if(psettings->contains("/setting/radio_gain")) + { + QVariant radio_gain = psettings->value("/setting/radio_gain"); + ui->lineEdit_gain->setText(radio_gain.toString()); + } + + if(psettings->contains("/setting/use_timenow")) + { + bool ok; + QVariant external_clock = psettings->value("/setting/use_timenow"); + int val = external_clock.toInt(&ok); + if(ok && val == 1){ + ui->checkBox_currentTime->setCheckState(Qt::Checked); + }else{ + ui->checkBox_currentTime->setCheckState(Qt::Unchecked); + } + } +} + +void MainWindow::saveSetting() +{ + int val; + psettings->setValue("/setting/gps_file", ui->lineEdit_gpsfile->text()); + psettings->setValue("/setting/motion_file", ui->lineEdit_motionfile->text()); + psettings->setValue("/setting/radio_gain", ui->lineEdit_gain->text()); + if(ui->checkBox_currentTime->isChecked()){ + val = 1; + }else{ + val = 0; + } + psettings->setValue("/setting/use_timenow", val); +} + diff --git a/mainwindow.h b/mainwindow.h index f7a3da3a..b6f0e3f2 100644 --- a/mainwindow.h +++ b/mainwindow.h @@ -2,6 +2,8 @@ #define MAINWINDOW_H #include +#include "workThread.h" +#include QT_BEGIN_NAMESPACE namespace Ui { @@ -17,7 +19,17 @@ public: MainWindow(QWidget *parent = nullptr); ~MainWindow(); +private slots: + void on_pushButton_start_clicked(); + void on_pushButton_stop_clicked(); + void on_pushButton_browsegps_clicked(); + void on_pushButton_browsemotion_clicked(); + private: Ui::MainWindow *ui; + WorkThread workThread; + QSettings *psettings; + void loadSetting(); + void saveSetting(); }; #endif // MAINWINDOW_H diff --git a/uhd_device.cpp b/uhd_device.cpp new file mode 100644 index 00000000..3648e33f --- /dev/null +++ b/uhd_device.cpp @@ -0,0 +1,82 @@ +#include "uhd_device.h" +#include +#include +#include +#include +#include +#include +#include + +static uhd::usrp::multi_usrp::sptr usrp = nullptr; +static uhd::tx_streamer::sptr tx_stream; +bool start; + +int init_device(double rate, double freq, double gain) +{ + try { + usrp = uhd::usrp::multi_usrp::make("master_clock_rate=52e6"); + } catch (uhd::key_error e) { + std::cout<get_pp_string().c_str()); + printf("Setting TX Rate: %f Msps...\n",rate/1e6); + usrp->set_tx_rate(rate); + printf("Actual TX Rate: %f Msps...\n",usrp->get_tx_rate()/1e6); + double lo_offset = 0.0; + printf("Setting TX LO Offset: %f MHz...\n",lo_offset/1e6); + uhd::tune_request_t tune_request; + tune_request = uhd::tune_request_t(freq, lo_offset); + usrp->set_tx_freq(tune_request); + printf("Actual TX Freq: %f MHz...\n",usrp->get_tx_freq() / 1e6); + // set the rf gain + printf("Setting TX Gain: %f dB...\n",gain); + usrp->set_tx_gain(gain); + printf("Actual TX Gain: %f dB...\n",usrp->get_tx_gain()); + + QEventLoop loop; + QTimer::singleShot(1000,&loop,SLOT(quit())); + loop.exec(); + + std::vector sensor_names; + sensor_names = usrp->get_tx_sensor_names(0); + if (std::find(sensor_names.begin(), sensor_names.end(), "lo_locked") + != sensor_names.end()) { + uhd::sensor_value_t lo_locked = usrp->get_tx_sensor("lo_locked", 0); + printf("Checking TX: %s ...\n",lo_locked.to_pp_string().c_str()); + UHD_ASSERT_THROW(lo_locked.to_bool()); + } + + // create a transmit streamer + std::string cpu_format = "sc16"; + std::vector channel_nums; + std::string wirefmt = "sc16"; + uhd::stream_args_t stream_args(cpu_format, wirefmt); + channel_nums.push_back(0); + stream_args.channels = channel_nums; + tx_stream = usrp->get_tx_stream(stream_args); + start = true; + + return true; +} + +int device_transmit(short* sample, size_t samps_count) +{ + uhd::tx_metadata_t md; + md.start_of_burst = start; + if(start) + start = false; + md.end_of_burst = false; + + return tx_stream->send(sample, samps_count, md); +} + +void close_device() +{ + if(usrp){ + usrp.reset(); + } +} diff --git a/uhd_device.h b/uhd_device.h new file mode 100644 index 00000000..c5c8c5f9 --- /dev/null +++ b/uhd_device.h @@ -0,0 +1,8 @@ +#ifndef UHD_DEVICE_H +#define UHD_DEVICE_H + +int init_device(double rate,double freq,double gain); +void close_device(); +int device_transmit(short* sample, size_t samps_count); + +#endif // UHD_DEVICE_H diff --git a/workThread.cpp b/workThread.cpp new file mode 100644 index 00000000..64890f52 --- /dev/null +++ b/workThread.cpp @@ -0,0 +1,115 @@ +#include "workThread.h" +#include "uhd_device.h" +#include "gpssim.h" +#include +#include +#include +#include + +WorkThread::WorkThread() +{ + read_buff_pos = 0; + write_buff_pos = 0; + iq_buff_size = 260000; + for (int i=0; i future = QtConcurrent::run(this,&WorkThread::transmit); + + /*while(!bstop){ + m_lock.lock(); + wait_buffer_empty.wait(&m_lock); + read_buff_pos++; + m_lock.unlock(); + }*/ + + future.waitForFinished(); + close_device(); +} diff --git a/workThread.h b/workThread.h new file mode 100644 index 00000000..d43ca091 --- /dev/null +++ b/workThread.h @@ -0,0 +1,45 @@ +#ifndef WORKTHREAD_H +#define WORKTHREAD_H + +#include +#include +#include + +#define BUFFER_COUNT 32 + +typedef struct +{ + short* buff_ptr; + volatile int full; +}sample_buffer_t; + +class WorkThread : public QThread +{ + Q_OBJECT +public: + WorkThread(); + ~WorkThread(); + void run() override; + void transmit(); + void stop(); + void setGain(int g); + void setNavFilePath(QString path); + void setUmfile(QString path); + void useNmeaGGA(int use); + void useCurrentTime(int use); +private: + int read_buff_pos=0; + int write_buff_pos=0; + int iq_buff_size=260000; + bool bstop = true; + QMutex m_lock; + QWaitCondition wait_buffer_empty; + sample_buffer_t sample_buffers[BUFFER_COUNT]; + QString navfile; + QString umfile; + int gain=0; + int nmeaGGA; + bool useCurTime; +}; + +#endif // WORKTHREAD_H