Created
This commit is contained in:
parent
887079b309
commit
83f111059d
174
gpssim.c
174
gpssim.c
@ -11,76 +11,7 @@
|
|||||||
#else
|
#else
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#endif
|
#endif
|
||||||
|
#include "gpssim.h"
|
||||||
#ifndef bool
|
|
||||||
typedef int bool;
|
|
||||||
#define true 1
|
|
||||||
#define false 0
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/*! \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 */
|
|
||||||
#define USER_MOTION_SIZE (3000) // max duration at 10Hz
|
|
||||||
|
|
||||||
/*! \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
|
|
||||||
|
|
||||||
// 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 (13) // for daily GPS broadcast ephemers file (brdc)
|
|
||||||
|
|
||||||
int sinTable512[] = {
|
int sinTable512[] = {
|
||||||
2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47,
|
2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47,
|
||||||
@ -162,94 +93,6 @@ double ant_pat_db[37] = {
|
|||||||
|
|
||||||
int allocatedSat[MAX_SAT];
|
int allocatedSat[MAX_SAT];
|
||||||
|
|
||||||
/*! \file gpssim.c
|
|
||||||
* \brief GPS Satellite Simulator
|
|
||||||
*/
|
|
||||||
|
|
||||||
/*! \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 */
|
|
||||||
// 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
|
|
||||||
{
|
|
||||||
gpstime_t g;
|
|
||||||
double range; // pseudorange
|
|
||||||
double rate;
|
|
||||||
double d; // geometric distance
|
|
||||||
double azel[2];
|
|
||||||
} 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 */
|
|
||||||
unsigned int carr_phase; /*< Carrier phase */
|
|
||||||
int carr_phasestep; /*< Carrier phasestep */
|
|
||||||
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;
|
|
||||||
|
|
||||||
/*! \brief Subtract two vectors of double
|
/*! \brief Subtract two vectors of double
|
||||||
* \param[out] y Result of subtraction
|
* \param[out] y Result of subtraction
|
||||||
* \param[in] x1 Minuend of subtracion
|
* \param[in] x1 Minuend of subtracion
|
||||||
@ -1575,8 +1418,8 @@ int main(int argc, char *argv[])
|
|||||||
char umfile[MAX_CHAR];
|
char umfile[MAX_CHAR];
|
||||||
double xyz[USER_MOTION_SIZE][3];
|
double xyz[USER_MOTION_SIZE][3];
|
||||||
|
|
||||||
bool staticLocationMode = false;
|
int staticLocationMode = FALSE;
|
||||||
bool nmeaGGA = false;
|
int nmeaGGA = FALSE;
|
||||||
|
|
||||||
char navfile[MAX_CHAR];
|
char navfile[MAX_CHAR];
|
||||||
char outfile[MAX_CHAR];
|
char outfile[MAX_CHAR];
|
||||||
@ -1631,16 +1474,16 @@ int main(int argc, char *argv[])
|
|||||||
break;
|
break;
|
||||||
case 'u':
|
case 'u':
|
||||||
strcpy(umfile, optarg);
|
strcpy(umfile, optarg);
|
||||||
nmeaGGA = false;
|
nmeaGGA = FALSE;
|
||||||
break;
|
break;
|
||||||
case 'g':
|
case 'g':
|
||||||
strcpy(umfile, optarg);
|
strcpy(umfile, optarg);
|
||||||
nmeaGGA = true;
|
nmeaGGA = TRUE;
|
||||||
break;
|
break;
|
||||||
case 'l':
|
case 'l':
|
||||||
// Static geodetic coordinates input mode
|
// Static geodetic coordinates input mode
|
||||||
// Added by scateu@gmail.com
|
// Added by scateu@gmail.com
|
||||||
staticLocationMode = true;
|
staticLocationMode = TRUE;
|
||||||
sscanf(optarg,"%lf,%lf,%lf",&llh[0],&llh[1],&llh[2]);
|
sscanf(optarg,"%lf,%lf,%lf",&llh[0],&llh[1],&llh[2]);
|
||||||
llh[0] = llh[0] / R2D; // convert to RAD
|
llh[0] = llh[0] / R2D; // convert to RAD
|
||||||
llh[1] = llh[1] / R2D; // convert to RAD
|
llh[1] = llh[1] / R2D; // convert to RAD
|
||||||
@ -1723,7 +1566,7 @@ int main(int argc, char *argv[])
|
|||||||
if (!staticLocationMode)
|
if (!staticLocationMode)
|
||||||
{
|
{
|
||||||
// Read user motion file
|
// Read user motion file
|
||||||
if (nmeaGGA==true)
|
if (nmeaGGA==TRUE)
|
||||||
numd = readNmeaGGA(xyz, umfile);
|
numd = readNmeaGGA(xyz, umfile);
|
||||||
else
|
else
|
||||||
numd = readUserMotion(xyz, umfile);
|
numd = readUserMotion(xyz, umfile);
|
||||||
@ -2039,9 +1882,10 @@ int main(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
else // data_format==SC16
|
else // data_format==SC16
|
||||||
{
|
{
|
||||||
|
/*
|
||||||
for (isamp=0; isamp<2*iq_buff_size; isamp++)
|
for (isamp=0; isamp<2*iq_buff_size; isamp++)
|
||||||
iq_buff[isamp] = iq_buff[isamp]>0?1000:-1000; // Emulated 1-bit I/Q
|
iq_buff[isamp] = iq_buff[isamp]>0?1000:-1000; // Emulated 1-bit I/Q
|
||||||
|
*/
|
||||||
fwrite(iq_buff, 2, 2*iq_buff_size, fp);
|
fwrite(iq_buff, 2, 2*iq_buff_size, fp);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
155
gpssim.h
Normal file
155
gpssim.h
Normal file
@ -0,0 +1,155 @@
|
|||||||
|
#ifndef GPSSIM_H
|
||||||
|
#define GPSSIM_H
|
||||||
|
|
||||||
|
#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 */
|
||||||
|
#define USER_MOTION_SIZE (3000) // max duration at 10Hz
|
||||||
|
|
||||||
|
/*! \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
|
||||||
|
|
||||||
|
// 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 (13) // 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 */
|
||||||
|
// 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
|
||||||
|
{
|
||||||
|
gpstime_t g;
|
||||||
|
double range; // pseudorange
|
||||||
|
double rate;
|
||||||
|
double d; // geometric distance
|
||||||
|
double azel[2];
|
||||||
|
} 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 */
|
||||||
|
unsigned int carr_phase; /*< Carrier phase */
|
||||||
|
int carr_phasestep; /*< Carrier phasestep */
|
||||||
|
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
|
Loading…
Reference in New Issue
Block a user