mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-04-27 05:03:15 +00:00
Improving documentation and code cleaning
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@448 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez
parent
3adee701fd
commit
3daf65bfc4
@ -46,10 +46,10 @@ const double GALILEO_GM = 3.986004418e14; //!< Geocentric
|
||||
const double GALILEO_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Mean angular velocity of the Earth [rad/s]
|
||||
const double GALILEO_C_m_s = 299792458.0; //!< The speed of light, [m/s]
|
||||
const double GALILEO_C_m_ms = 299792.4580; //!< The speed of light, [m/ms]
|
||||
|
||||
const double GALILEO_F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
|
||||
|
||||
// carrier and code frequencies
|
||||
const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< E1 [Hz]
|
||||
const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< Galileo E1 carrier frequency [Hz]
|
||||
const double Galileo_E1_CODE_CHIP_RATE_HZ = 1.023e6; //!< Galileo E1 code rate [chips/s]
|
||||
const double Galileo_E1_CODE_PERIOD = 0.004; //!< Galileo E1 code period [s]
|
||||
const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz]
|
||||
@ -59,7 +59,6 @@ const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate
|
||||
const double Galileo_E1_C_SECONDARY_CODE_LENGTH = 25.0; //!< Galileo E1-C secondary code length [chips]
|
||||
const int Galileo_E1_NUMBER_OF_CODES = 50;
|
||||
|
||||
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
|
||||
const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
|
||||
|
||||
// Galileo INAV Telemetry structure
|
||||
@ -70,7 +69,7 @@ const int GALILEO_INAV_PREAMBLE_LENGTH_BITS = 10;
|
||||
const int GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS = 250;
|
||||
const int GALILEO_INAV_PAGE_PART_SYMBOLS = 250; //!< Each Galileo INAV pages are composed of two parts (even and odd) each of 250 symbols, including preamble. See Galileo ICD 4.3.2
|
||||
const int GALILEO_INAV_PAGE_SYMBOLS = 500; //!< The complete Galileo INAV page length
|
||||
const int GALILEO_INAV_PAGE_PART_SECONDS = 1; //a page part last 2 sec
|
||||
const int GALILEO_INAV_PAGE_PART_SECONDS = 1; // a page part last 1 sec
|
||||
const int GALILEO_INAV_PAGE_SECONDS = 2; // a full page last 2 sec
|
||||
const int GALILEO_INAV_INTERLEAVER_ROWS = 8;
|
||||
const int GALILEO_INAV_INTERLEAVER_COLS = 30;
|
||||
@ -189,6 +188,7 @@ const std::vector<std::pair<int,int>> DN_6_bit({{95,3}});
|
||||
const std::vector<std::pair<int,int>> Delta_tLSF_6_bit({{97,8}});
|
||||
const std::vector<std::pair<int,int>> TOW_6_bit({{106,20}});
|
||||
|
||||
|
||||
/* Page 7 */
|
||||
const std::vector<std::pair<int,int>> IOD_a_7_bit({{7,4}});
|
||||
const std::vector<std::pair<int,int>> WN_a_7_bit({{11,2}});
|
||||
@ -209,6 +209,8 @@ const std::vector<std::pair<int,int>>Omega_dot_7_bit({{96,11}});
|
||||
const double Omega_dot_7_LSB = TWO_N33;
|
||||
const std::vector<std::pair<int,int>> M0_7_bit({{107,16}});
|
||||
const double M0_7_LSB = TWO_N15;
|
||||
|
||||
|
||||
/* Page 8 */
|
||||
const std::vector<std::pair<int,int>> IOD_a_8_bit({{7,4}});
|
||||
const std::vector<std::pair<int,int>> af0_8_bit({{11,16}});
|
||||
@ -231,6 +233,7 @@ const double Omega0_8_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> Omega_dot_8_bit({{117,11}});
|
||||
const double Omega_dot_8_LSB = TWO_N33;
|
||||
|
||||
|
||||
/* Page 9 */
|
||||
const std::vector<std::pair<int,int>> IOD_a_9_bit({{7,4}});
|
||||
const std::vector<std::pair<int,int>> WN_a_9_bit({{11,2}});
|
||||
@ -254,6 +257,7 @@ const double omega_9_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> delta_i_9_bit({{118,11}});
|
||||
const double delta_i_9_LSB = TWO_N14;
|
||||
|
||||
|
||||
/* Page 10 */
|
||||
const std::vector<std::pair<int,int>> IOD_a_10_bit({{7,4}});
|
||||
const std::vector<std::pair<int,int>> Omega0_10_bit({{11,16}});
|
||||
@ -272,7 +276,6 @@ const std::vector<std::pair<int,int>> A_0G_10_bit({{87,16}});
|
||||
const double A_0G_10_LSB = TWO_N35;
|
||||
const std::vector<std::pair<int,int>> A_1G_10_bit({{103,12}});
|
||||
const double A_1G_10_LSB = TWO_N51;
|
||||
|
||||
const std::vector<std::pair<int,int>> t_0G_10_bit({{115,8}});
|
||||
const double t_0G_10_LSB = 3600;
|
||||
const std::vector<std::pair<int,int>> WN_0G_10_bit({{123,6}});
|
||||
@ -284,11 +287,7 @@ const std::vector<std::pair<int,int>>WN_0_bit({{97,12}});
|
||||
const std::vector<std::pair<int,int>> TOW_0_bit({{109,20}});
|
||||
|
||||
|
||||
|
||||
|
||||
// Galileo E1 primary codes
|
||||
|
||||
|
||||
const std::string Galileo_E1_B_PRIMARY_CODE[Galileo_E1_NUMBER_OF_CODES] = {
|
||||
"F5D710130573541B9DBD4FD9E9B20A0D59D144C54BC7935539D2E75810FB51E494093A0A19DD79C70C5A98E5657AA578097777E86BCC4651CC72F2F974DC766E07AEA3D0B557EF42FF57E6A58E805358CE9257669133B18F80FDBDFB38C5524C7FB1DE079842482990DF58F72321D9201F8979EAB159B2679C9E95AA6D53456C0DF75C2B4316D1E2309216882854253A1FA60CA2C94ECE013E2A8C943341E7D9E5A8464B3AD407E0AE465C3E3DD1BE60A8C3D50F831536401E776BE02A6042FC4A27AF653F0CFC4D4D013F115310788D68CAEAD3ECCCC5330587EB3C22A1459FC8E6FCCE9CDE849A5205E70C6D66D125814D698DD0EEBFEAE52CC65C5C84EEDF207379000E169D318426516AC5D1C31F2E18A65E07AE6E33FDD724B13098B3A444688389EFBBB5EEAB588742BB083B679D42FB26FF77919EAB21DE0389D9997498F967AE05AF0F4C7E177416E18C4D5E6987ED3590690AD127D872F14A8F4903A12329732A9768F82F295BEE391879293E3A97D51435A7F03ED7FBE275F102A83202DC3DE94AF4C712E9D006D182693E9632933E6EB773880CF147B922E74539E4582F79E39723B4C80E42EDCE4C08A8D02221BAE6D17734817D5B531C0D3C1AE723911F3FFF6AAC02E97FEA69E376AF4761E6451CA61FDB2F9187642EFCD63A09AAB680770C1593EEDD4FF4293BFFD6DD2C3367E85B14A654C834B6699421A",
|
||||
"96B856A629F581D1344FEF597835FE60434625D077ECF0D95FBE1155EA0431979E5AFF544AF591A332FDAEF98AB1EDD847A73F3AF15AAEE7E9A05C9D82C59EC325EF4CF264B8ADF2A8E8BA459354CB4B415CC50BF239ADBC31B3A9C87B0843CF3B9E6D646BA43F866276B053826F3A2334CC5E2EFB9F8F195B382E75EEA63F58A06B3F82A3B5C77C1800FD9498F803E524435B321210BB84690BED0BBBE16D363B3A90656A73720E27008852FB7DACC8284411B177728D9527C560859084A395A6F11A96AD9DB6B43E00642B000ED12BFD967868EAB1108552CD4FC89FBC408ACE7678C381EC91DD000319124EB5D5EF52C4CAC9AADEE2FA045C16CE492D7F43743CA77924C78696FCBF2F9F7F36D8E623752200C6FCBBD71ABBB6877F3C5D6E6740AB0389458A6B66440858B2D383244E853646FE2714211DEA9E6196252815BB704A20BFE556AC474F8998944E0CABBBE21A6400B87BFDCF937D12B2821D59298AF4AD378F0F42BD8C41693B8D993CF37C8B478F3BB5D33AD2A9FA24AD7B8FA895FDBC04964192F7BA3FF74E0E3A435B5DFE042E3115CACF29624C0645E9C917534A2EBC1F5665E4E1B1BC56208DBCD8A27CCB6474D5D0E20CA4072C960E5ACE41BDA3770DF3B681F2B318F6F8E1CB17C2857350FB6009AED665E13B2780D79217F73FAC7A8A48048DB0FB8A8A5007CDDC9A7B2DA8257C99F1CB605A18204",
|
||||
@ -395,7 +394,6 @@ const std::string Galileo_E1_C_PRIMARY_CODE[Galileo_E1_NUMBER_OF_CODES] = {
|
||||
"ADDCEDB50E907D20E826E6E8A0D30C20C74B2DF204EA784BAE9F618CAE33A3C937729DF9CB10BA2A4C33E0182A37200C0CC509729D828B8A2A20F283AC4F9306596684EA3FB5492A4C9F2DB459E7531C9F9C0950E7D9E93B3EE5912AE7E39AC8F4EC14B18F24E325003F477E347C5AC1B67CDB11AF3BBBBCD0AC3703024B5767AA67A208254F798684BFD1D3EACD757EEC77254950A146620400DB95E694574F739A991EBA771EBBDFF1056BB39A77DBE0636A032E17141332F951C57C6C90F348F165E3ABDD60D429D5D6BEC7E3E3463806F819EB2D212B3528A5EDE51F235AD100A35E890955F8A1DC51FDCB53EABCA2540997DD054C1F5B29462995B876B44D085904E55E1B838BEF600A992EB49CE078DF75AF3D0F137685AC0D07F0BE1EB87B63A41E74DDE869C8A683BDE60AF5D77FF18F7137495BCEFD0ED28F62F9C3E25D332B5F861D999FCDC0B4851A984A4DBB53401FD40351ADA4335C702BCC8D900C737507B990BDDBE91D201E3A0946DC968D43FD10D04B0B76667FF5B4291C2124B0124C6B710A6D1BCFAEB016B9DEEB0F7A4FE044CA4EA0CCD84B7682617C3A545071EC295B0663B3F577D562DE1D9DD80DE6A1EFD6D5991EB5246F1597B86D0E9A90CF6DB0EB2B8E7BAE9431E567F01AA98502C773742246467ABF911A91A51F6C1B9E0C3233DC1A37D17DB91A5F0F661B0EB5886964456C7818601BD0C"
|
||||
};
|
||||
|
||||
const std::string Galileo_E1_C_SECONDARY_CODE =
|
||||
"0011100000001010110110010";
|
||||
const std::string Galileo_E1_C_SECONDARY_CODE = "0011100000001010110110010";
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E1_H_ */
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
/*!
|
||||
* \file gps_almanac.cc
|
||||
* \brief Interface of a GPS ALMANAC storage
|
||||
*
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* \file galileo_almanac.cc
|
||||
* \brief Implementation of a Galileo ALMANAC storage
|
||||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
|
||||
@ -33,14 +33,13 @@
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class is a storage for the GALIELO ALMANAC data as described in GALILEO ICD
|
||||
* \brief This class is a storage for the GALILEO ALMANAC data as described in GALILEO ICD
|
||||
*
|
||||
* See http:http://ec.europa.eu/enterprise/policies/satnav/galileo/files/galileo-os-sis-icd-issue1-revision1_en.pdf paragraph 5.1.10
|
||||
* See http://ec.europa.eu/enterprise/policies/satnav/galileo/files/galileo-os-sis-icd-issue1-revision1_en.pdf paragraph 5.1.10
|
||||
*/
|
||||
class Galileo_Almanac
|
||||
{
|
||||
public:
|
||||
|
||||
/*Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/
|
||||
int IOD_a_7;
|
||||
double WN_a_7;
|
||||
@ -83,7 +82,6 @@ public:
|
||||
double omega_9;
|
||||
double delta_i_9;
|
||||
|
||||
|
||||
/*Word type 10: Almanac for SVID3 (2/2)*/
|
||||
int IOD_a_10;
|
||||
double Omega0_10;
|
||||
@ -93,10 +91,8 @@ public:
|
||||
double af1_10;
|
||||
double E5b_HS_10;
|
||||
double E1B_HS_10;
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
Galileo_Almanac();
|
||||
|
||||
Galileo_Almanac(); //!< Default constructor
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
/*!
|
||||
* \file galileo_ephemeris.cc
|
||||
* \brief Interface of a GPS EPHEMERIS storage and orbital model functions
|
||||
*
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* \brief Interface of a Galileo EPHEMERIS storage and orbital model functions
|
||||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||||
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
|
||||
* -------------------------------------------------------------------------
|
||||
@ -38,7 +36,6 @@ Galileo_Ephemeris::Galileo_Ephemeris()
|
||||
flag_all_ephemeris = false;
|
||||
IOD_ephemeris = 0;
|
||||
IOD_nav_1 = 0;
|
||||
|
||||
SV_ID_PRN_4 = 0;
|
||||
M0_1 = 0; // Mean anomaly at reference time [semi-circles]
|
||||
delta_n_3 = 0; // Mean motion difference from computed value [semi-circles/sec]
|
||||
@ -56,21 +53,19 @@ Galileo_Ephemeris::Galileo_Ephemeris()
|
||||
C_ic_4 = 0; // Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
|
||||
C_is_4 = 0; // Amplitude of the sine harmonic correction term to the angle of inclination [radians]
|
||||
t0e_1 = 0; // Ephemeris reference time [s]
|
||||
|
||||
/*Clock correction parameters*/
|
||||
t0c_4 = 0; // Clock correction data reference Time of Week [sec]
|
||||
af0_4 = 0; // SV clock bias correction coefficient [s]
|
||||
af1_4 = 0; // SV clock drift correction coefficient [s/s]
|
||||
af2_4 = 0; // SV clock drift rate correction coefficient [s/s^2]
|
||||
|
||||
/*GST*/
|
||||
WN_5 = 0;
|
||||
TOW_5 = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW){
|
||||
double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW)
|
||||
{
|
||||
/* GALIELO SYSTEM TIME, ICD 5.1.2
|
||||
* input parameter:
|
||||
* WN: The Week Number is an integer counter that gives the sequential week number
|
||||
@ -101,22 +96,21 @@ double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW){
|
||||
double sec_in_day = 86400;
|
||||
double day_in_week = 7;
|
||||
t = WN*sec_in_day*day_in_week + TOW; // second from the origin of the Galileo time
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
|
||||
|
||||
double Galileo_Ephemeris::sv_clock_drift(double transmitTime){
|
||||
/* Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
*
|
||||
*/
|
||||
double Galileo_Ephemeris::sv_clock_drift(double transmitTime)
|
||||
{
|
||||
// Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
double dt;
|
||||
dt = transmitTime - t0c_4;
|
||||
Galileo_satClkDrift = af0_4 + af1_4*dt + (af2_4 * dt)*(af2_4 * dt) + Galileo_dtr;
|
||||
return Galileo_satClkDrift;
|
||||
}
|
||||
|
||||
|
||||
// compute the relativistic correction term
|
||||
double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
{
|
||||
@ -135,7 +129,6 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) //Sate
|
||||
n0 = sqrt(GALILEO_GM / (a*a*a));
|
||||
|
||||
// Time from ephemeris reference epoch
|
||||
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||||
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||||
tk = transmitTime - t0e_1;
|
||||
|
||||
@ -164,7 +157,6 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) //Sate
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Compute relativistic correction term
|
||||
Galileo_dtr = GALILEO_F * e_1* A_1 * sin(E);
|
||||
return Galileo_dtr;
|
||||
@ -172,12 +164,10 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) //Sate
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
|
||||
void Galileo_Ephemeris::satellitePosition(double transmitTime)
|
||||
{
|
||||
// when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
|
||||
double tk; // Time from ephemeris reference epoch
|
||||
//double t; // Galileo System Time (ICD, paragraph 5.1.2)
|
||||
double a; // Semi-major axis
|
||||
double n; // Corrected mean motion
|
||||
double n0; // Computed mean motion
|
||||
@ -186,7 +176,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this funct
|
||||
double E_old;
|
||||
double dE;
|
||||
double nu; // True anomaly
|
||||
double phi; //argument of Latitude
|
||||
double phi; // Argument of Latitude
|
||||
double u; // Correct argument of latitude
|
||||
double r; // Correct radius
|
||||
double i;
|
||||
@ -201,11 +191,8 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this funct
|
||||
n0 = sqrt(GALILEO_GM / (a*a*a));
|
||||
|
||||
// Time from ephemeris reference epoch
|
||||
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||||
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||||
tk = transmitTime - t0e_1;
|
||||
|
||||
//std::cout<<"Diff t_tx-t_oe="<<tk<<std::endl;
|
||||
// Corrected mean motion
|
||||
n = n0 + delta_n_3;
|
||||
|
||||
@ -260,7 +247,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this funct
|
||||
|
||||
// --- Compute satellite coordinates in Earth-fixed coordinates
|
||||
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
|
||||
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); //***********************NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
|
||||
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); // ********NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
|
||||
d_satpos_Z = sin(u) * r * sin(i);
|
||||
|
||||
// Satellite's velocity. Can be useful for Vector Tracking loops
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/*!
|
||||
* \file galileo_navigation_message.h
|
||||
* \brief Interface of a GPS EPHEMERIS storage
|
||||
* \brief Interface of a Galileo EPHEMERIS storage
|
||||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||||
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
|
||||
* -------------------------------------------------------------------------
|
||||
@ -42,61 +42,62 @@
|
||||
* \brief This class is a storage and orbital model functions for the Galileo SV ephemeris data as described in Galileo ICD paragraph 5.1.1
|
||||
*
|
||||
*/
|
||||
|
||||
class Galileo_Ephemeris
|
||||
{
|
||||
private:
|
||||
|
||||
public:
|
||||
|
||||
/* Galileo ephemeris are 16 parameters and here are reported following the ICD order, paragraph 5.1.1.
|
||||
The number in the name after underscore (_1, _2, _3 and so on) refers to the page were we can find that parameter */
|
||||
bool flag_all_ephemeris;
|
||||
int IOD_ephemeris;
|
||||
int IOD_nav_1;
|
||||
|
||||
int SV_ID_PRN_4;
|
||||
double M0_1; // Mean anomaly at reference time [semi-circles]
|
||||
double delta_n_3; // Mean motion difference from computed value [semi-circles/sec]
|
||||
double e_1; // Eccentricity
|
||||
double A_1; // Square root of the semi-major axis [metres^1/2]
|
||||
double OMEGA_0_2; // Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
|
||||
double i_0_2; // Inclination angle at reference time [semi-circles]
|
||||
double omega_2; // Argument of perigee [semi-circles]
|
||||
double OMEGA_dot_3; // Rate of right ascension [semi-circles/sec]
|
||||
double iDot_2; // Rate of inclination angle [semi-circles/sec]
|
||||
double C_uc_3; // Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
|
||||
double C_us_3; // Amplitude of the sine harmonic correction term to the argument of latitude [radians]
|
||||
double C_rc_3; // Amplitude of the cosine harmonic correction term to the orbit radius [meters]
|
||||
double C_rs_3; // Amplitude of the sine harmonic correction term to the orbit radius [meters]
|
||||
double C_ic_4; // Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
|
||||
double C_is_4; // Amplitude of the sine harmonic correction term to the angle of inclination [radians]
|
||||
double t0e_1; // Ephemeris reference time [s]
|
||||
double M0_1; //!< Mean anomaly at reference time [semi-circles]
|
||||
double delta_n_3; //!< Mean motion difference from computed value [semi-circles/sec]
|
||||
double e_1; //!< Eccentricity
|
||||
double A_1; //!< Square root of the semi-major axis [metres^1/2]
|
||||
double OMEGA_0_2; //!< Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
|
||||
double i_0_2; //!< Inclination angle at reference time [semi-circles]
|
||||
double omega_2; //!< Argument of perigee [semi-circles]
|
||||
double OMEGA_dot_3; //!< Rate of right ascension [semi-circles/sec]
|
||||
double iDot_2; //!< Rate of inclination angle [semi-circles/sec]
|
||||
double C_uc_3; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
|
||||
double C_us_3; //!< Amplitude of the sine harmonic correction term to the argument of latitude [radians]
|
||||
double C_rc_3; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters]
|
||||
double C_rs_3; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters]
|
||||
double C_ic_4; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
|
||||
double C_is_4; //!< Amplitude of the sine harmonic correction term to the angle of inclination [radians]
|
||||
double t0e_1; //!< Ephemeris reference time [s]
|
||||
|
||||
/*Clock correction parameters*/
|
||||
double t0c_4; //Clock correction data reference Time of Week [sec]
|
||||
double af0_4; //SV clock bias correction coefficient [s]
|
||||
double af1_4; //SV clock drift correction coefficient [s/s]
|
||||
double af2_4; //SV clock drift rate correction coefficient [s/s^2]
|
||||
double t0c_4; //!< Clock correction data reference Time of Week [sec]
|
||||
double af0_4; //!< SV clock bias correction coefficient [s]
|
||||
double af1_4; //!< SV clock drift correction coefficient [s/s]
|
||||
double af2_4; //!< SV clock drift rate correction coefficient [s/s^2]
|
||||
|
||||
/*GST*/
|
||||
//Not belong to ephemeris set (page 1 to 4)
|
||||
double WN_5; //Week number
|
||||
double TOW_5; //Time of Week
|
||||
|
||||
double WN_5; //!< Week number
|
||||
double TOW_5; //!< Time of Week
|
||||
double Galileo_satClkDrift;
|
||||
double Galileo_dtr; // relativistic clock correction term
|
||||
double Galileo_dtr; //!< relativistic clock correction term
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
|
||||
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
|
||||
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
|
||||
|
||||
// Satellite velocity
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
|
||||
|
||||
unsigned int i_satellite_PRN; // SV PRN NUMBER
|
||||
unsigned int i_satellite_PRN; //!< SV PRN NUMBER
|
||||
|
||||
void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity
|
||||
double Galileo_System_Time(double WN, double TOW); //!< Galileo System Time (GST), ICD paragraph 5.1.2
|
||||
double sv_clock_drift(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
double sv_clock_relativistic_term(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
Galileo_Ephemeris();
|
||||
|
||||
/*
|
||||
template<class Archive>
|
||||
@ -147,21 +148,7 @@ public:
|
||||
archive & make_nvp("b_alert_flag",b_alert_flag); //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
|
||||
archive & make_nvp("b_antispoofing_flag",b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV
|
||||
}
|
||||
|
||||
|
||||
\\brief Compute the ECEF SV coordinates and ECEF velocity
|
||||
\\http://ec.europa.eu/enterprise/policies/satnav/galileo/open-service/
|
||||
*/
|
||||
void satellitePosition(double transmitTime);
|
||||
|
||||
double Galileo_System_Time(double WN, double TOW); // Galileo System Time (GST), ICD paragraph 5.1.2
|
||||
double sv_clock_drift(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
double sv_clock_relativistic_term(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
//Default constructor
|
||||
|
||||
|
||||
|
||||
Galileo_Ephemeris();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -33,9 +33,7 @@
|
||||
|
||||
Galileo_Iono::Galileo_Iono()
|
||||
{
|
||||
//valid = false;
|
||||
/* Ionospheric correction */
|
||||
/* Az */
|
||||
ai0_5 = 0; // Effective Ionisation Level 1st order parameter [sfu]
|
||||
ai1_5 = 0; // Effective Ionisation Level 2st order parameter [sfu/degree]
|
||||
ai2_5 = 0; // Effective Ionisation Level 3st order parameter [sfu/degree]
|
||||
@ -49,6 +47,5 @@ Galileo_Iono::Galileo_Iono()
|
||||
|
||||
TOW_5 = 0;
|
||||
WN_5 = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/*!
|
||||
* \file gps_iono.h
|
||||
* \brief Interface of a GPS IONOSPHERIC MODEL storage
|
||||
* \file galileo_iono.h
|
||||
* \brief Interface of a Galileo Ionospheric Model storage
|
||||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||||
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
|
||||
* -------------------------------------------------------------------------
|
||||
@ -40,30 +40,22 @@
|
||||
*/
|
||||
class Galileo_Iono
|
||||
{
|
||||
private:
|
||||
|
||||
public:
|
||||
|
||||
// valid flag
|
||||
//bool valid;
|
||||
|
||||
/*Ionospheric correction*/
|
||||
/*Az*/
|
||||
double ai0_5; //Effective Ionisation Level 1st order parameter [sfu]
|
||||
double ai1_5; //Effective Ionisation Level 2st order parameter [sfu/degree]
|
||||
double ai2_5; //Effective Ionisation Level 3st order parameter [sfu/degree]
|
||||
double ai0_5; //!< Effective Ionisation Level 1st order parameter [sfu]
|
||||
double ai1_5; //!< Effective Ionisation Level 2st order parameter [sfu/degree]
|
||||
double ai2_5; //!< Effective Ionisation Level 3st order parameter [sfu/degree]
|
||||
|
||||
/*Ionospheric disturbance flag*/
|
||||
bool Region1_flag_5; // Ionospheric Disturbance Flag for region 1
|
||||
bool Region2_flag_5; // Ionospheric Disturbance Flag for region 2
|
||||
bool Region3_flag_5; // Ionospheric Disturbance Flag for region 3
|
||||
bool Region4_flag_5; // Ionospheric Disturbance Flag for region 4
|
||||
bool Region5_flag_5; // Ionospheric Disturbance Flag for region 5
|
||||
bool Region1_flag_5; //!< Ionospheric Disturbance Flag for region 1
|
||||
bool Region2_flag_5; //!< Ionospheric Disturbance Flag for region 2
|
||||
bool Region3_flag_5; //!< Ionospheric Disturbance Flag for region 3
|
||||
bool Region4_flag_5; //!< Ionospheric Disturbance Flag for region 4
|
||||
bool Region5_flag_5; //!< Ionospheric Disturbance Flag for region 5
|
||||
|
||||
/*from page 5 (UTC) to have a timestamp*/
|
||||
double TOW_5;//UTC data reference Time of Week [s]
|
||||
double WN_5; //UTC data reference Week number [week]
|
||||
|
||||
double TOW_5; //!< UTC data reference Time of Week [s]
|
||||
double WN_5; //!< UTC data reference Week number [week]
|
||||
|
||||
/*!
|
||||
* Default constructor
|
||||
|
||||
@ -120,8 +120,8 @@ void Galileo_Navigation_Message::reset()
|
||||
Region5_flag_5 = 0; //
|
||||
BGD_E1E5a_5 = 0; //
|
||||
BGD_E1E5b_5 = 0; //
|
||||
E5b_HS_5 = 0; //
|
||||
E1B_HS_5 = 0; //
|
||||
E5b_HS_5 = 0;
|
||||
E1B_HS_5 = 0;
|
||||
E5b_DVS_5 = 0; //
|
||||
E1B_DVS_5 = 0; //
|
||||
/*GST*/
|
||||
@ -356,12 +356,6 @@ bool Galileo_Navigation_Message::read_navigation_bool(std::bitset<GALILEO_DATA_J
|
||||
}
|
||||
|
||||
|
||||
/*void Galileo_Navigation_Message::print_galileo_word_bytes(unsigned int GPS_word)
|
||||
{
|
||||
std::cout << " Word =";
|
||||
std::cout << std::bitset<32>(GPS_word);
|
||||
std::cout << std::endl;
|
||||
}*/
|
||||
|
||||
|
||||
void Galileo_Navigation_Message::split_page(std::string page_string, int flag_even_word)
|
||||
@ -376,20 +370,15 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
|
||||
if(page_string.at(0) == '1')// if page is odd
|
||||
{
|
||||
//std::cout<< "page_string.at(0) split page="<<page_string.at(0) << std::endl;
|
||||
std::string page_Odd = page_string; //chiamo la stringa sembre page_Odd
|
||||
std::string page_Odd = page_string;
|
||||
//std::cout<<"Page odd string in split page"<< std::endl << page_Odd << std::endl;
|
||||
|
||||
if (flag_even_word == 1)/*Under this condition An odd page has been received but the previous even page is kept in memory and it is considered to join pages*/
|
||||
if (flag_even_word == 1) // An odd page has been received but the previous even page is kept in memory and it is considered to join pages
|
||||
{
|
||||
//std::cout<<"previous page even "<< std::endl << page_Even << std::endl;
|
||||
std::string page_INAV_even = page_Even;
|
||||
//std::cout << "page inav solo even" << page_INAV_even << std::endl;
|
||||
std::string page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page
|
||||
//std::cout << "page inav eve +odd " << page_INAV<< std::endl;
|
||||
std::string Even_bit = page_INAV.substr (0,1);
|
||||
//std::cout << "Even bit = " << Even_bit << endl;
|
||||
std::string Page_type_even = page_INAV.substr (1,1);
|
||||
//std::cout << "Page type even = " << Page_type_even << endl;
|
||||
std::string nominal = "0";
|
||||
|
||||
//if (Page_type_even.compare(nominal) != 0)
|
||||
@ -397,12 +386,9 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
|
||||
//else std::cout << "Nominal Page" << std::endl;
|
||||
|
||||
std::string Data_k = page_INAV.substr (2,112);
|
||||
//std::cout << "Data_k " << endl << Data_k << endl;
|
||||
std::string Odd_bit = page_INAV.substr (114,1);
|
||||
std::string Page_type_Odd = page_INAV.substr (115,1);
|
||||
//std::cout << "Page_type_Odd: " << Page_type_Odd << endl;
|
||||
std::string Data_j = page_INAV.substr (116,16);
|
||||
//std::cout << "Data_j: " << Data_j << endl;
|
||||
|
||||
std::string Reserved_1 = page_INAV.substr (132,40);
|
||||
std::string SAR = page_INAV.substr (172,22);
|
||||
@ -413,12 +399,9 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
|
||||
|
||||
//************ CRC checksum control *******/
|
||||
std::stringstream TLM_word_for_CRC_stream;
|
||||
|
||||
TLM_word_for_CRC_stream << page_INAV;
|
||||
std::string TLM_word_for_CRC;
|
||||
TLM_word_for_CRC = TLM_word_for_CRC_stream.str().substr(0, GALILEO_DATA_FRAME_BITS);
|
||||
|
||||
//std::cout<<"Complete word for CRC test: "<<TLM_word_for_CRC;
|
||||
std::bitset<GALILEO_DATA_FRAME_BITS> TLM_word_for_CRC_bits(TLM_word_for_CRC);
|
||||
std::bitset<24> checksum(CRC_data);
|
||||
|
||||
@ -431,30 +414,22 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
|
||||
flag_CRC_test = true;
|
||||
// CRC correct: Decode word
|
||||
std::string page_number_bits = Data_k.substr (0,6);
|
||||
//std::cout << "Page number bits from Data k" << std::endl << page_number_bits << std::endl;
|
||||
|
||||
std::bitset<GALILEO_PAGE_TYPE_BITS> page_type_bits (page_number_bits); // from string to bitset
|
||||
Page_type = (int)read_page_type_unsigned(page_type_bits, type);
|
||||
Page_type_time_stamp = Page_type;
|
||||
//std::cout << "Page number (first 6 bits of Data k converted to decimal) = " << Page_type << std::endl;
|
||||
std::string Data_jk_ephemeris = Data_k + Data_j;
|
||||
//std::cout<<"Data j k ephemeris" << endl << Data_jk_ephemeris << endl;
|
||||
|
||||
page_jk_decoder(Data_jk_ephemeris.c_str()); // Corresponding to ephemeris_decode.m in matlab code
|
||||
page_jk_decoder(Data_jk_ephemeris.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
// CRC wrong.. discard frame
|
||||
// Wrong CRC... discard frame
|
||||
flag_CRC_test = false;
|
||||
}
|
||||
//********** end of CRC checksum control ***/
|
||||
}
|
||||
|
||||
} /*end if (page_string.at(0)=='1') */
|
||||
} // end of CRC checksum control
|
||||
} // end if (page_string.at(0)=='1')
|
||||
else
|
||||
{
|
||||
page_Even = page_string.substr (0,114);
|
||||
//std::cout << "Page even in split page" << std::endl << page_Even << std::endl;
|
||||
std::string tail_Even = page_string.substr (114,6);
|
||||
//std::cout << "tail_even_string: " << tail_Even <<std::endl;
|
||||
//if (tail_Even.compare(correct_tail) != 0)
|
||||
@ -519,7 +494,7 @@ bool Galileo_Navigation_Message::have_new_almanac() //Check if we have a new alm
|
||||
{
|
||||
if ((flag_almanac_1 == true) and (flag_almanac_2 == true) and (flag_almanac_3 == true) and (flag_almanac_4 == true))
|
||||
{
|
||||
//std::cout<< "All almanac have been received"<< std::endl;
|
||||
//All almanac have been received
|
||||
flag_almanac_1 = false;
|
||||
flag_almanac_2 = false;
|
||||
flag_almanac_3 = false;
|
||||
@ -1020,279 +995,3 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
}
|
||||
|
||||
|
||||
|
||||
//void Galileo_Navigation_Message::satellitePosition(double transmitTime) //when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
|
||||
//{
|
||||
//
|
||||
// double tk; // Time from ephemeris reference epoch
|
||||
// //double t; // Galileo System Time (ICD, paragraph 5.1.2)
|
||||
// double a; // Semi-major axis
|
||||
// double n; // Corrected mean motion
|
||||
// double n0; // Computed mean motion
|
||||
// double M; // Mean anomaly
|
||||
// double E; //Eccentric Anomaly (to be solved by iteration)
|
||||
// double E_old;
|
||||
// double dE;
|
||||
// double nu; //True anomaly
|
||||
// double phi; //argument of Latitude
|
||||
// double u; // Correct argument of latitude
|
||||
// double r; // Correct radius
|
||||
// double i;
|
||||
// double Omega;
|
||||
//
|
||||
// // Find Galileo satellite's position ----------------------------------------------
|
||||
//
|
||||
// // Restore semi-major axis
|
||||
// a = A_1*A_1;
|
||||
//
|
||||
// // Computed mean motion
|
||||
// n0 = sqrt(GALILEO_GM / (a*a*a));
|
||||
//
|
||||
// // Time from ephemeris reference epoch
|
||||
// //tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||||
// //t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||||
// tk = transmitTime - t0e_1;
|
||||
//
|
||||
// // Corrected mean motion
|
||||
// n = n0 + delta_n_3;
|
||||
//
|
||||
// // Mean anomaly
|
||||
// M = M0_1 + n * tk;
|
||||
//
|
||||
// // Reduce mean anomaly to between 0 and 2pi
|
||||
// M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
|
||||
//
|
||||
// // Initial guess of eccentric anomaly
|
||||
// E = M;
|
||||
//
|
||||
// // --- Iteratively compute eccentric anomaly ----------------------------
|
||||
// for (int ii = 1; ii<20; ii++)
|
||||
// {
|
||||
// E_old = E;
|
||||
// E = M + e_1 * sin(E);
|
||||
// dE = fmod(E - E_old, 2*GALILEO_PI);
|
||||
// if (fabs(dE) < 1e-12)
|
||||
// {
|
||||
// //Necessary precision is reached, exit from the loop
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // Compute the true anomaly
|
||||
//
|
||||
// double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E);
|
||||
// double tmp_X = cos(E) - e_1;
|
||||
// nu = atan2(tmp_Y, tmp_X);
|
||||
//
|
||||
// // Compute angle phi (argument of Latitude)
|
||||
// phi = nu + omega_2;
|
||||
//
|
||||
// // Reduce phi to between 0 and 2*pi rad
|
||||
// phi = fmod((phi), (2*GALILEO_PI));
|
||||
//
|
||||
// // Correct argument of latitude
|
||||
// u = phi + C_uc_3 * cos(2*phi) + C_us_3 * sin(2*phi);
|
||||
//
|
||||
// // Correct radius
|
||||
// r = a * (1 - e_1*cos(E)) + C_rc_3 * cos(2*phi) + C_rs_3 * sin(2*phi);
|
||||
//
|
||||
// // Correct inclination
|
||||
// i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2*phi) + C_is_4 * sin(2*phi);
|
||||
//
|
||||
// // Compute the angle between the ascending node and the Greenwich meridian
|
||||
// Omega = OMEGA_0_2 + (OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT)*tk - GALILEO_OMEGA_EARTH_DOT * t0e_1;
|
||||
//
|
||||
// // Reduce to between 0 and 2*pi rad
|
||||
// Omega = fmod((Omega + 2*GALILEO_PI), (2*GALILEO_PI));
|
||||
//
|
||||
// // --- Compute satellite coordinates in Earth-fixed coordinates
|
||||
// galileo_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
|
||||
// galileo_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); //***********************NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
|
||||
// galileo_satpos_Z = sin(u) * r * sin(i);
|
||||
//
|
||||
// std::cout << "Galileo satellite position X [m]: " << galileo_satpos_X << std::endl;
|
||||
// std::cout << "Galileo satellite position Y [m]: " << galileo_satpos_Y << std::endl;
|
||||
// std::cout << "Galileo satellite position Z [m]: " << galileo_satpos_Z << std::endl;
|
||||
// double vector_position = sqrt(galileo_satpos_X*galileo_satpos_X + galileo_satpos_Y*galileo_satpos_Y + galileo_satpos_Z*galileo_satpos_Z);
|
||||
// std::cout << "Vector Earth Center-Satellite [Km]: " << vector_position/1000 << std::endl;
|
||||
//
|
||||
// // Satellite's velocity. Can be useful for Vector Tracking loops
|
||||
// double Omega_dot = OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT;
|
||||
// galileo_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + galileo_satpos_X * cos(Omega) - galileo_satpos_Y * cos(i) * sin(Omega);
|
||||
// galileo_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + galileo_satpos_X * sin(Omega) + galileo_satpos_Y * cos(i) * cos(Omega);
|
||||
// galileo_satvel_Z = galileo_satpos_Y * sin(i);
|
||||
//
|
||||
//}
|
||||
//
|
||||
//
|
||||
//double Galileo_Navigation_Message::Galileo_System_Time(double WN, double TOW){
|
||||
// /* GALIELO SYSTEM TIME, ICD 5.1.2
|
||||
// * input parameter:
|
||||
// * WN: The Week Number is an integer counter that gives the sequential week number
|
||||
// from the origin of the Galileo time. It covers 4096 weeks (about 78 years).
|
||||
// Then the counter is reset to zero to cover additional period modulo 4096
|
||||
//
|
||||
// TOW: The Time of Week is defined as the number of seconds that have occurred since
|
||||
// the transition from the previous week. The TOW covers an entire week from 0 to
|
||||
// 604799 seconds and is reset to zero at the end of each week
|
||||
//
|
||||
// WN and TOW are received in page 5
|
||||
//
|
||||
// output:
|
||||
// t: it is the transmitted time in Galileo System Time (expressed in seconds)
|
||||
//
|
||||
// The GST start epoch shall be 00:00 UT on Sunday 22nd August 1999 (midnight between 21st and 22nd August).
|
||||
// At the start epoch, GST shall be ahead of UTC by thirteen (13)
|
||||
// leap seconds. Since the next leap second was inserted at 01.01.2006, this implies that
|
||||
// as of 01.01.2006 GST is ahead of UTC by fourteen (14) leap seconds.
|
||||
//
|
||||
// The epoch denoted in the navigation messages by TOW and WN
|
||||
// will be measured relative to the leading edge of the first chip of the
|
||||
// first code sequence of the first page symbol. The transmission timing of the navigation
|
||||
// message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST).
|
||||
// *
|
||||
// */
|
||||
// double t=0;
|
||||
// double sec_in_day = 86400;
|
||||
// double day_in_week = 7;
|
||||
// t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time
|
||||
//
|
||||
// return t;
|
||||
//
|
||||
//}
|
||||
//
|
||||
//
|
||||
//
|
||||
//double Galileo_Navigation_Message::sv_clock_drift(double transmitTime){
|
||||
// /* Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
// *
|
||||
// */
|
||||
// double dt;
|
||||
// dt = transmitTime - t0c_4;
|
||||
// Galileo_satClkDrift = af0_4 + af1_4*dt + (af2_4 * dt)*(af2_4 * dt) + Galileo_dtr;
|
||||
// return Galileo_satClkDrift;
|
||||
//}
|
||||
//
|
||||
//// compute the relativistic correction term
|
||||
//double Galileo_Navigation_Message::sv_clock_relativistic_term(double transmitTime) //Satellite Time Correction Algorithm, ICD 5.1.4
|
||||
//{
|
||||
// double tk;
|
||||
// double a;
|
||||
// double n;
|
||||
// double n0;
|
||||
// double E;
|
||||
// double E_old;
|
||||
// double dE;
|
||||
// double M;
|
||||
//
|
||||
// // Restore semi-major axis
|
||||
// a = A_1*A_1;
|
||||
//
|
||||
// n0 = sqrt(GALILEO_GM / (a*a*a));
|
||||
//
|
||||
// // Time from ephemeris reference epoch
|
||||
// //tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||||
// //t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||||
// tk = transmitTime - t0e_1;
|
||||
//
|
||||
// // Corrected mean motion
|
||||
// n = n0 + delta_n_3;
|
||||
//
|
||||
// // Mean anomaly
|
||||
// M = M0_1 + n * tk;
|
||||
//
|
||||
// // Reduce mean anomaly to between 0 and 2pi
|
||||
// M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
|
||||
//
|
||||
// // Initial guess of eccentric anomaly
|
||||
// E = M;
|
||||
//
|
||||
// // --- Iteratively compute eccentric anomaly ----------------------------
|
||||
// for (int ii = 1; ii<20; ii++)
|
||||
// {
|
||||
// E_old = E;
|
||||
// E = M + e_1 * sin(E);
|
||||
// dE = fmod(E - E_old, 2*GALILEO_PI);
|
||||
// if (fabs(dE) < 1e-12)
|
||||
// {
|
||||
// //Necessary precision is reached, exit from the loop
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//
|
||||
// // Compute relativistic correction term
|
||||
// Galileo_dtr = GALILEO_F * e_1* A_1 * sin(E);
|
||||
// return Galileo_dtr;
|
||||
//}
|
||||
|
||||
//double Galileo_Navigation_Message::GST_to_UTC_time(double t_e, int WN) //t_e is GST (WN+TOW) in second
|
||||
//{
|
||||
// double t_Utc;
|
||||
// double t_Utc_daytime;
|
||||
// double Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
|
||||
//
|
||||
// // Determine if the effectivity time of the leap second event is in the past
|
||||
// int weeksToLeapSecondEvent = WN_LSF_6 - WN;
|
||||
//
|
||||
// if ((weeksToLeapSecondEvent) >= 0) // is not in the past
|
||||
// {
|
||||
// //Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
|
||||
// int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60;
|
||||
// if (weeksToLeapSecondEvent > 0)
|
||||
// {
|
||||
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
|
||||
// }
|
||||
// else //we are in the same week than the leap second event
|
||||
// {
|
||||
// if (abs(t_e - secondOfLeapSecondEvent) > 21600)
|
||||
// {
|
||||
// /* 5.1.7a
|
||||
// * Whenever the leap second adjusted time indicated by the WN_LSF and the DN values
|
||||
// * is not in the past (relative to the user's present time), and the user's
|
||||
// * present time does not fall in the time span which starts at six hours prior
|
||||
// * to the effective time and ends at six hours after the effective time,
|
||||
// * the GST/Utc relationship is given by
|
||||
// */
|
||||
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// /* 5.1.7b
|
||||
// * Whenever the user's current time falls within the time span of six hours
|
||||
// * prior to the leap second adjustment to six hours after the adjustment time, ,
|
||||
// * the effective time is computed according to the following equations:
|
||||
// */
|
||||
//
|
||||
// int W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
|
||||
// t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
|
||||
// //implement something to handle a leap second event!
|
||||
// }
|
||||
// if ( (t_e - secondOfLeapSecondEvent) > 21600)
|
||||
// {
|
||||
// Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800*(double)(WN - WNot_6));
|
||||
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// else // the effectivity time is in the past
|
||||
// {
|
||||
// /* 5.1.7c
|
||||
// * Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values,
|
||||
// * is in the past (relative to the user’s current time) and the user’s present time does not
|
||||
// * fall in the time span which starts six hours prior to the leap second adjustment time and
|
||||
// * ends six hours after the adjustment time, the effective time is computed according to
|
||||
// * the following equation:
|
||||
// */
|
||||
// Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
|
||||
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
|
||||
// }
|
||||
//
|
||||
// double secondsOfWeekBeforeToday = 43200 * floor(t_e / 43200);
|
||||
// t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime;
|
||||
// return t_Utc;
|
||||
//
|
||||
//}
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
@ -41,24 +41,21 @@
|
||||
* http://ec.europa.eu/enterprise/policies/satnav/galileo/files/galileo-os-sis-icd-issue1-revision1_en.pdf
|
||||
* paragraph 5.1.7
|
||||
*/
|
||||
|
||||
class Galileo_Utc_Model
|
||||
{
|
||||
public:
|
||||
//bool valid;
|
||||
/*Word type 6: GST-UTC conversion parameters*/
|
||||
double A0_6;
|
||||
double A1_6;
|
||||
double Delta_tLS_6;
|
||||
double t0t_6; //UTC data reference Time of Week [s]
|
||||
double WNot_6; //UTC data reference Week number [week]
|
||||
double t0t_6; //!< UTC data reference Time of Week [s]
|
||||
double WNot_6; //!< UTC data reference Week number [week]
|
||||
double WN_LSF_6;
|
||||
double DN_6;
|
||||
double Delta_tLSF_6;
|
||||
bool flag_utc_model;
|
||||
//double TOW_6;
|
||||
|
||||
double GST_to_UTC_time(double t_e, int WN);
|
||||
double GST_to_UTC_time(double t_e, int WN); //!< GST-UTC Conversion Algorithm and Parameters
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
|
||||
@ -74,7 +74,6 @@ Gps_Ephemeris::Gps_Ephemeris()
|
||||
b_alert_flag = false; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
|
||||
b_antispoofing_flag = false; //!< If true, the AntiSpoofing mode is ON in that SV
|
||||
|
||||
|
||||
//Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus)
|
||||
satelliteBlock[9] = "IIA";
|
||||
satelliteBlock[31] = "IIR-M";
|
||||
@ -113,7 +112,6 @@ Gps_Ephemeris::Gps_Ephemeris()
|
||||
satelliteBlock[13] = "IIR";
|
||||
satelliteBlock[23] = "IIR";
|
||||
satelliteBlock[26] = "IIA";
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -143,6 +141,7 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime)
|
||||
return d_satClkDrift;
|
||||
}
|
||||
|
||||
|
||||
// compute the relativistic correction term
|
||||
double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
|
||||
{
|
||||
|
||||
@ -55,7 +55,6 @@ private:
|
||||
* \param[out] - corrected time, in seconds
|
||||
*/
|
||||
double check_t(double time);
|
||||
|
||||
public:
|
||||
unsigned int i_satellite_PRN; // SV PRN NUMBER
|
||||
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
|
||||
@ -93,7 +92,6 @@ public:
|
||||
double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s]
|
||||
double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2]
|
||||
|
||||
|
||||
// Flags
|
||||
|
||||
/*! \brief If true, enhanced level of integrity assurance.
|
||||
@ -111,8 +109,8 @@ public:
|
||||
bool b_antispoofing_flag; //!< If true, the AntiSpoofing mode is ON in that SV
|
||||
|
||||
// clock terms derived from ephemeris data
|
||||
double d_satClkDrift; // GPS clock error
|
||||
double d_dtr; // relativistic clock correction term
|
||||
double d_satClkDrift; //!< GPS clock error
|
||||
double d_dtr; //!< relativistic clock correction term
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
|
||||
@ -124,12 +122,11 @@ public:
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
|
||||
|
||||
|
||||
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
|
||||
|
||||
|
||||
template<class Archive>
|
||||
/*
|
||||
|
||||
/*!
|
||||
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
|
||||
*/
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
|
||||
@ -44,11 +44,8 @@
|
||||
*/
|
||||
class Gps_Iono
|
||||
{
|
||||
private:
|
||||
|
||||
public:
|
||||
// valid flag
|
||||
bool valid;
|
||||
bool valid; //!< Valid flag
|
||||
// Ionospheric parameters
|
||||
double d_alpha0; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
|
||||
double d_alpha1; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
|
||||
@ -58,14 +55,12 @@ public:
|
||||
double d_beta1; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
|
||||
double d_beta2; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
|
||||
double d_beta3; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
Gps_Iono();
|
||||
|
||||
Gps_Iono(); //!< Default constructor
|
||||
|
||||
template<class Archive>
|
||||
/*
|
||||
|
||||
/*!
|
||||
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
|
||||
*/
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
|
||||
@ -42,17 +42,17 @@ class Sbas_Ephemeris
|
||||
public:
|
||||
void print(std::ostream &out);
|
||||
int i_prn; //!< PRN number
|
||||
//gtime_t t0; //!< reference epoch time (GPST)
|
||||
//gtime_t t0; // reference epoch time (GPST)
|
||||
int i_t0;
|
||||
//gtime_t tof; // time of message frame (GPST)
|
||||
double d_tof;
|
||||
int i_sv_ura; //!< SV accuracy (URA index), not standardized
|
||||
bool b_sv_do_not_use; //!< health status (false:do not use / true:usable)
|
||||
double d_pos[3]; //!< satellite position (m) (ECEF)
|
||||
double d_vel[3]; //!< satellite velocity (m/s) (ECEF)
|
||||
double d_acc[3]; //!< satellite acceleration (m/s^2) (ECEF)
|
||||
double d_af0; //!< satellite clock-offset (s)
|
||||
double d_af1; //!< satellite drift (s/s)
|
||||
bool b_sv_do_not_use; //!< Health status (false:do not use / true:usable)
|
||||
double d_pos[3]; //!< Satellite position (m) (ECEF)
|
||||
double d_vel[3]; //!< Satellite velocity (m/s) (ECEF)
|
||||
double d_acc[3]; //!< Satellite acceleration (m/s^2) (ECEF)
|
||||
double d_af0; //!< Satellite clock-offset (s)
|
||||
double d_af1; //!< Satellite drift (s/s)
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -35,7 +35,6 @@
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "sbas_ionospheric_correction.h"
|
||||
|
||||
enum V_Log_Level {EVENT = 2, // logs important events which don't occur every update() call
|
||||
@ -43,8 +42,7 @@ enum V_Log_Level {EVENT = 2, // logs important events which don't occur every up
|
||||
MORE = 4}; // very detailed stuff
|
||||
|
||||
|
||||
void
|
||||
Sbas_Ionosphere_Correction::print(std::ostream &out)
|
||||
void Sbas_Ionosphere_Correction::print(std::ostream &out)
|
||||
{
|
||||
for(std::vector<Igp_Band>::const_iterator it_band = d_bands.begin(); it_band != d_bands.end(); ++it_band)
|
||||
{
|
||||
@ -65,13 +63,25 @@ Sbas_Ionosphere_Correction::print(std::ostream &out)
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* -receiver position (degree) is in terms of WGS84
|
||||
* -azimuth is the angle of the satellite from the userÕs location measured clockwise from north
|
||||
* -elevation is the angle of the satellite from the user's location measured with respect to the local-tangent-plane
|
||||
|
||||
/* Applies SBAS ionosphric delay correction
|
||||
* \param[out] delay Slant ionospheric delay (L1) (m)
|
||||
* \param[out] var Variance of ionospheric delay (m^2)
|
||||
* \param[in] sample_stamp Sample stamp of observable on which the correction will be applied
|
||||
* \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree)
|
||||
* \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree)
|
||||
* \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of
|
||||
* the satellite from the userÕs location measured clockwise from north
|
||||
* \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured
|
||||
* with respect to the local-tangent-plane
|
||||
*/
|
||||
bool Sbas_Ionosphere_Correction::apply(double sample_stamp, double latitude_d, double longitude_d,
|
||||
double azimut_d, double evaluation_d, double &delay, double &var)
|
||||
bool Sbas_Ionosphere_Correction::apply(double sample_stamp,
|
||||
double latitude_d,
|
||||
double longitude_d,
|
||||
double azimut_d,
|
||||
double elevation_d,
|
||||
double &delay,
|
||||
double &var)
|
||||
{
|
||||
const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
|
||||
int result;
|
||||
@ -83,9 +93,9 @@ bool Sbas_Ionosphere_Correction::apply(double sample_stamp, double latitude_d, d
|
||||
pos[1] = longitude_d * GPS_PI / 180.0;
|
||||
pos[2] = 0; // is not used by sbsioncorr, for ionocorrection is a fixed earth radius assumed
|
||||
|
||||
// convert satellite azimut and evaluation from degrees to rad , use topocent to obtain it in pvt block
|
||||
// convert satellite azimut and elevation from degrees to rad , use topocent to obtain it in pvt block
|
||||
azel[0] = azimut_d * GPS_PI / 180.0;
|
||||
azel[1] = evaluation_d * GPS_PI / 180.0;
|
||||
azel[1] = elevation_d * GPS_PI / 180.0;
|
||||
|
||||
result = sbsioncorr(sample_stamp, pos, azel, &delay, &var);
|
||||
return (bool)result;
|
||||
@ -191,10 +201,7 @@ void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, do
|
||||
|
||||
|
||||
const double PI = 3.1415926535897932; /* pi */
|
||||
//const double D2R = (PI/180.0); /* deg to rad */
|
||||
//const double R2D = (180.0/PI); /* rad to deg */
|
||||
//const double MAXBAND = 10; /* max SBAS band of IGP */
|
||||
//const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */
|
||||
|
||||
|
||||
|
||||
/* satellite azimuth/elevation angle -------------------------------------------
|
||||
@ -252,7 +259,6 @@ double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel,
|
||||
{
|
||||
double cosaz, rp, ap, sinap, tanap;
|
||||
const double D2R = (PI/180.0); /* deg to rad */
|
||||
//const double R2D = (180.0/PI); /* rad to deg */
|
||||
|
||||
rp = re/(re + hion)*cos(azel[1]);
|
||||
ap = PI/2.0 - azel[1] - asin(rp);
|
||||
@ -291,7 +297,6 @@ void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, d
|
||||
int i;
|
||||
int latp[2];
|
||||
int lonp[4];
|
||||
//const double D2R = (PI/180.0); /* deg to rad */
|
||||
const double R2D = (180.0/PI); /* rad to deg */
|
||||
|
||||
double lat = pos[0]*R2D;
|
||||
@ -395,7 +400,6 @@ int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const doub
|
||||
double t;
|
||||
double w[4] = {0};
|
||||
const Igp *igp[4] = {0}; /* {ws,wn,es,en} */
|
||||
//const double D2R = (PI/180.0); /* deg to rad */
|
||||
const double R2D = (180.0/PI); /* rad to deg */
|
||||
|
||||
trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);
|
||||
|
||||
@ -40,7 +40,9 @@
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Struct that represents a Ionospheric Grid Point (IGP)
|
||||
*/
|
||||
struct Igp
|
||||
{
|
||||
public:
|
||||
@ -50,7 +52,6 @@ public:
|
||||
int d_longitude;
|
||||
int d_give;
|
||||
double d_delay;
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
@ -65,13 +66,14 @@ private:
|
||||
};
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Struct that represents the band of a Ionospheric Grid Point (IGP)
|
||||
*/
|
||||
struct Igp_Band
|
||||
{
|
||||
//int d_iodi;
|
||||
//int d_nigp; // number if IGPs in this band (defined by IGP mask from MT18)
|
||||
std::vector<Igp> d_igps;
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
@ -81,50 +83,26 @@ private:
|
||||
}
|
||||
};
|
||||
|
||||
// valid ionosphere correction for GPS
|
||||
class Sbas_Ionosphere_Correction
|
||||
{
|
||||
private:
|
||||
// /* type definitions ----------------------------------------------------------*/
|
||||
//#define MAXBAND 10 /* max SBAS band of IGP */
|
||||
//#define MAXNIGP 201 /* max number of IGP in SBAS band */
|
||||
//
|
||||
// typedef struct { /* time struct */
|
||||
// time_t time; /* time (s) expressed by standard time_t */
|
||||
// double sec; /* fraction of second under 1 s */
|
||||
// } gtime_t;
|
||||
//
|
||||
// typedef struct { /* SBAS ionospheric correction type */
|
||||
// gtime_t t0; /* correction time */
|
||||
// short lat,lon; /* latitude/longitude (deg) */
|
||||
// short give; /* GIVI+1 */
|
||||
// float delay; /* vertical delay estimate (m) */
|
||||
// } sbsigp_t;
|
||||
//
|
||||
// typedef struct { /* SBAS ionospheric corrections type */
|
||||
// int iodi; /* IODI (issue of date ionos corr) */
|
||||
// int nigp; /* number of igps */
|
||||
// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
|
||||
// } sbsion_t;
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Inner product of vectors
|
||||
* \brief Class that handles valid SBAS ionospheric correction for GPS
|
||||
*/
|
||||
class Sbas_Ionosphere_Correction
|
||||
{
|
||||
private:
|
||||
/* Inner product of vectors
|
||||
* params : double *a,*b I vector a,b (n x 1)
|
||||
* int n I size of vector a,b
|
||||
* return : a'*b
|
||||
*/
|
||||
double dot(const double *a, const double *b, int n);
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief multiply matrix
|
||||
*/
|
||||
/* Multiply matrix */
|
||||
void matmul(const char *tr, int n, int k, int m, double alpha,
|
||||
const double *A, const double *B, double beta, double *C);
|
||||
/*!
|
||||
* \brief EFEC to local coordinate transfomartion matrix
|
||||
|
||||
/* EFEC to local coordinate transfomartion matrix
|
||||
* Compute ecef to local coordinate transfomartion matrix
|
||||
* params : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *E O ecef to local coord transformation matrix (3x3)
|
||||
@ -133,8 +111,7 @@ private:
|
||||
*/
|
||||
void xyz2enu(const double *pos, double *E);
|
||||
|
||||
/*!
|
||||
* \brief Transforms ECEF vector into local tangential coordinates
|
||||
/* Transforms ECEF vector into local tangential coordinates
|
||||
* params : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *r I vector in ecef coordinate {x,y,z}
|
||||
* double *e O vector in local tangental coordinate {e,n,u}
|
||||
@ -142,8 +119,7 @@ private:
|
||||
*/
|
||||
void ecef2enu(const double *pos, const double *r, double *e);
|
||||
|
||||
/*!
|
||||
* \brief Compute satellite azimuth/elevation angle
|
||||
/* Compute satellite azimuth/elevation angle
|
||||
* params : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
* double *e I receiver-to-satellilte unit vevtor (ecef)
|
||||
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
|
||||
@ -152,9 +128,7 @@ private:
|
||||
*/
|
||||
double satazel(const double *pos, const double *e, double *azel);
|
||||
|
||||
/*!
|
||||
* \brief debug trace functions
|
||||
*/
|
||||
/* Debug trace functions */
|
||||
void trace(int level, const char *format, ...);
|
||||
|
||||
/* time difference -------------------------------------------------------------
|
||||
@ -164,8 +138,7 @@ private:
|
||||
*-----------------------------------------------------------------------------*/
|
||||
//double timediff(gtime_t t1, gtime_t t2);
|
||||
|
||||
/*!
|
||||
* \brief Compute ionospheric pierce point (ipp) position and slant factor
|
||||
/* Compute Ionospheric Pierce Point (IPP) position and slant factor
|
||||
* params : double *pos I receiver position {lat,lon,h} (rad,m)
|
||||
* double *azel I azimuth/elevation angle {az,el} (rad)
|
||||
* double re I earth radius (km)
|
||||
@ -178,18 +151,13 @@ private:
|
||||
double ionppp(const double *pos, const double *azel, double re,
|
||||
double hion, double *posp);
|
||||
|
||||
/*!
|
||||
* \brief Variance of ionosphere correction (give=GIVEI+1)
|
||||
*/
|
||||
/* Variance of ionosphere correction (give = GIVEI + 1) */
|
||||
double varicorr(int give);
|
||||
|
||||
/*!
|
||||
* \brief Search igps
|
||||
*/
|
||||
/* Search igps */
|
||||
void searchigp(const double *pos, const Igp **igp, double *x, double *y);
|
||||
|
||||
/*!
|
||||
* \brief Compute sbas ionosphric delay correction
|
||||
/* Compute sbas ionosphric delay correction
|
||||
* params : long sample_stamp I sample stamp of observable on which the correction will be applied
|
||||
* sbsion_t *ion I ionospheric correction data (implicit)
|
||||
* double *pos I receiver position {lat,lon,height} (rad/m)
|
||||
@ -211,6 +179,20 @@ private:
|
||||
public:
|
||||
std::vector<Igp_Band> d_bands;
|
||||
void print(std::ostream &out);
|
||||
|
||||
/*!
|
||||
* \brief Computes SBAS ionospheric delay correction.
|
||||
*
|
||||
* \param[out] delay Slant ionospheric delay (L1) (m)
|
||||
* \param[out] var Variance of ionospheric delay (m^2)
|
||||
* \param[in] sample_stamp Sample stamp of observable on which the correction will be applied
|
||||
* \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree)
|
||||
* \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree)
|
||||
* \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of
|
||||
* the satellite from the userÕs location measured clockwise from north
|
||||
* \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured
|
||||
* with respect to the local-tangent-plane
|
||||
*/
|
||||
bool apply(double sample_stamp, double latitude_d, double longitude_d,
|
||||
double azimut_d, double evaluation_d, double &delay, double &var);
|
||||
|
||||
|
||||
@ -44,19 +44,23 @@ struct Fast_Correction
|
||||
int d_tlat;
|
||||
};
|
||||
|
||||
|
||||
struct Long_Term_Correction
|
||||
{
|
||||
double d_trx; // time when message was received
|
||||
int i_tapp; // time of applicability (only valid if vel=1, equals the sent t0)
|
||||
int i_vel; // use velocity corrections if vel=1
|
||||
double d_trx; //!< Time when message was received
|
||||
int i_tapp; //!< Time of applicability (only valid if vel=1, equals the sent t0)
|
||||
int i_vel; //!< Use velocity corrections if vel=1
|
||||
int d_iode;
|
||||
double d_dpos[3]; // position correction
|
||||
double d_dvel[3]; // velocity correction
|
||||
double d_daf0; // clock offset correction
|
||||
double d_daf1; // clock drift correction
|
||||
double d_dpos[3]; //!< position correction
|
||||
double d_dvel[3]; //!< velocity correction
|
||||
double d_daf0; //!< clock offset correction
|
||||
double d_daf1; //!< clock drift correction
|
||||
};
|
||||
|
||||
// valid long and fast term correction for one SV
|
||||
|
||||
/*!
|
||||
* \brief Valid long and fast term SBAS corrections for one SV
|
||||
*/
|
||||
class Sbas_Satellite_Correction
|
||||
{
|
||||
public:
|
||||
@ -70,7 +74,6 @@ public:
|
||||
int apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var);
|
||||
int apply_long_term_sv_clk(double sample_stamp, double &dts, double &var);
|
||||
bool alarm();
|
||||
|
||||
private:
|
||||
/* debug trace functions -----------------------------------------------------*/
|
||||
void trace(int level, const char *format, ...);
|
||||
|
||||
@ -45,42 +45,48 @@
|
||||
#define FLOW 3 // logs the function calls of block processing functions
|
||||
#define DETAIL 4
|
||||
|
||||
|
||||
|
||||
Sbas_Telemetry_Data::Sbas_Telemetry_Data()
|
||||
{
|
||||
fp_trace = nullptr;; /* file pointer of trace */
|
||||
level_trace = 0; /* level of trace */
|
||||
tick_trace = 0; /* tick time at traceopen (ms) */
|
||||
fp_trace = nullptr; // file pointer of trace
|
||||
level_trace = 0; // level of trace
|
||||
tick_trace = 0; // tick time at traceopen (ms)
|
||||
|
||||
raw_msg_queue = NULL;
|
||||
iono_queue = NULL;
|
||||
sat_corr_queue = NULL;
|
||||
ephemeris_queue = NULL;
|
||||
raw_msg_queue = nullptr;
|
||||
iono_queue = nullptr;
|
||||
sat_corr_queue = nullptr;
|
||||
ephemeris_queue = nullptr;
|
||||
|
||||
d_nav.sbssat.iodp = -1; // make sure that in any case iodp is not equal to the received one
|
||||
prn_mask_changed(); // invalidate all satellite corrections
|
||||
|
||||
for(size_t band = 0; band < sizeof(d_nav.sbsion)/sizeof(sbsion_t); band++)
|
||||
{
|
||||
d_nav.sbsion[band].iodi = -1; // make sure that in any case iodi is not euqual to the received one
|
||||
d_nav.sbsion[band].iodi = -1; // make sure that in any case iodi is not equal to the received one
|
||||
igp_mask_changed(band);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Sbas_Telemetry_Data::set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue)
|
||||
{
|
||||
this->raw_msg_queue = raw_msg_queue;
|
||||
}
|
||||
|
||||
|
||||
void Sbas_Telemetry_Data::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
|
||||
{
|
||||
this->iono_queue = iono_queue;
|
||||
}
|
||||
|
||||
|
||||
void Sbas_Telemetry_Data::set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue)
|
||||
{
|
||||
this->sat_corr_queue = sat_corr_queue;
|
||||
}
|
||||
|
||||
|
||||
void Sbas_Telemetry_Data::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
|
||||
{
|
||||
this->ephemeris_queue = ephemeris_queue;
|
||||
@ -137,7 +143,7 @@ int Sbas_Telemetry_Data::update(Sbas_Raw_Msg sbas_raw_msg)
|
||||
case 7:
|
||||
case 24:
|
||||
case 25: updated_satellite_corrections(); break;
|
||||
case 18: break; // new iono band mask recieved -> dont update iono corrections because delays are not
|
||||
case 18: break; // new iono band mask received -> dont update iono corrections because delays are not
|
||||
case 26: received_iono_correction(); break;
|
||||
case 9: /*updated_sbas_ephemeris(sbas_raw_msg);*/ break;
|
||||
|
||||
@ -145,14 +151,19 @@ int Sbas_Telemetry_Data::update(Sbas_Raw_Msg sbas_raw_msg)
|
||||
}
|
||||
|
||||
// send it to raw message queue
|
||||
if(raw_msg_queue != NULL) raw_msg_queue->push(sbas_raw_msg);
|
||||
if(raw_msg_queue != nullptr) raw_msg_queue->push(sbas_raw_msg);
|
||||
return parsing_result;
|
||||
}
|
||||
|
||||
|
||||
unsigned int getbitu(const unsigned char *buff, int pos, int len);
|
||||
|
||||
|
||||
|
||||
int getbits(const unsigned char *buff, int pos, int len);
|
||||
|
||||
|
||||
|
||||
int Sbas_Telemetry_Data::decode_mt12(Sbas_Raw_Msg sbas_raw_msg)
|
||||
{
|
||||
const double rx_delay = 38000.0/300000.0; // estimated sbas signal geosat to ground signal travel time
|
||||
@ -171,12 +182,9 @@ int Sbas_Telemetry_Data::decode_mt12(Sbas_Raw_Msg sbas_raw_msg)
|
||||
void Sbas_Telemetry_Data::updated_sbas_ephemeris(Sbas_Raw_Msg msg)
|
||||
{
|
||||
VLOG(FLOW) << "<<T>> updated_sbas_ephemeris():" << std::endl;
|
||||
|
||||
Sbas_Ephemeris seph;
|
||||
|
||||
int satidx = msg.get_prn() - MINPRNSBS;
|
||||
seph_t seph_rtklib = d_nav.seph[satidx];
|
||||
|
||||
// copy data
|
||||
seph.i_prn = msg.get_prn();
|
||||
seph.i_t0 = seph_rtklib.t0;
|
||||
@ -188,12 +196,12 @@ void Sbas_Telemetry_Data::updated_sbas_ephemeris(Sbas_Raw_Msg msg)
|
||||
memcpy(seph.d_acc, seph_rtklib.acc, sizeof(seph.d_acc));
|
||||
seph.d_af0 = seph_rtklib.af0;
|
||||
seph.d_af1 = seph_rtklib.af1;
|
||||
|
||||
// print ephemeris for debugging purposes
|
||||
std::stringstream ss;
|
||||
seph.print(ss);
|
||||
VLOG(FLOW) << ss.str();
|
||||
|
||||
if(ephemeris_queue != NULL) ephemeris_queue->push(seph);
|
||||
if(ephemeris_queue != nullptr) ephemeris_queue->push(seph);
|
||||
}
|
||||
|
||||
|
||||
@ -234,7 +242,7 @@ void Sbas_Telemetry_Data::received_iono_correction()
|
||||
VLOG(EVENT) << ss.str();
|
||||
|
||||
// send to SBAS ionospheric correction queue
|
||||
if(iono_queue != NULL) iono_queue->push(iono_corr);
|
||||
if(iono_queue != nullptr) iono_queue->push(iono_corr);
|
||||
}
|
||||
|
||||
|
||||
@ -368,7 +376,7 @@ void Sbas_Telemetry_Data::updated_satellite_corrections()
|
||||
|
||||
if(fast_correction_updated || long_term_correction_updated)
|
||||
{
|
||||
if(sat_corr_queue != NULL) sat_corr_queue->push(sbas_satelite_correction);
|
||||
if(sat_corr_queue != nullptr) sat_corr_queue->push(sbas_satelite_correction);
|
||||
}
|
||||
}
|
||||
VLOG(FLOW) << ss.str(); ss.str("");
|
||||
@ -390,6 +398,8 @@ void Sbas_Telemetry_Data::trace(int level, const char *format, ...)
|
||||
VLOG(FLOW) << "<<T>> " << std::string(str);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* satellite system+prn/slot number to satellite number ------------------------
|
||||
* convert satellite system+prn/slot number to satellite number
|
||||
* args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
|
||||
@ -887,11 +897,12 @@ int Sbas_Telemetry_Data::decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
|
||||
int Sbas_Telemetry_Data::decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
|
||||
{
|
||||
trace(4,"decode_sbstype25:");
|
||||
|
||||
return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* decode type 26: ionospheric delay corrections -----------------------------*/
|
||||
int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
|
||||
{
|
||||
@ -915,7 +926,6 @@ int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
|
||||
sbsion[band].igp[j].give = give;
|
||||
|
||||
if(sbsion[band].igp[j].give > 15) sbsion[band].igp[j].give = 15; // give is not higher than 15, but to be sure
|
||||
|
||||
}
|
||||
trace(5, "decode_sbstype26: band=%d block=%d", band, block);
|
||||
return 1;
|
||||
|
||||
@ -39,21 +39,18 @@
|
||||
#include <bitset>
|
||||
#include "boost/assign.hpp"
|
||||
#include <cmath>
|
||||
|
||||
//#include "sbas_satellite_correction.h"
|
||||
//#include "sbas_ionospheric_correction.h"
|
||||
|
||||
#include "concurrent_queue.h"
|
||||
|
||||
#include "sbas_time.h"
|
||||
|
||||
|
||||
class Sbas_Ionosphere_Correction;
|
||||
class Sbas_Satellite_Correction;
|
||||
struct Fast_Correction;
|
||||
struct Long_Term_Correction;
|
||||
class Sbas_Ephemeris;
|
||||
|
||||
/*
|
||||
|
||||
/*!
|
||||
* \brief Represents a raw SBAS message of 250cbits + 6 bits padding
|
||||
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding)
|
||||
*/
|
||||
@ -61,11 +58,8 @@ class Sbas_Raw_Msg
|
||||
{
|
||||
public:
|
||||
Sbas_Raw_Msg(){ rx_time = Sbas_Time(0); i_prn = -1; };
|
||||
//Sbas_Raw_Msg(int week, int tow, int prn, const std::vector<unsigned char> msg) : d_week(week), d_tow(tow), d_prn(prn), d_msg(msg) {}
|
||||
Sbas_Raw_Msg(double sample_stamp, int prn, const std::vector<unsigned char> msg) : rx_time(sample_stamp), i_prn(prn), d_msg(msg) {}
|
||||
//int get_week() {return d_week;}
|
||||
//int get_tow() {return d_tow;}
|
||||
double get_sample_stamp() {return rx_time.get_time_stamp();} // time of reception sample stamp (first sample of preample)
|
||||
double get_sample_stamp() { return rx_time.get_time_stamp(); } //!< Time of reception sample stamp (first sample of preample)
|
||||
void relate(Sbas_Time_Relation time_relation)
|
||||
{
|
||||
rx_time.relate(time_relation);
|
||||
@ -88,10 +82,7 @@ public:
|
||||
unsigned char crc_first_byte = (d_msg[28] << 2) && (d_msg[29] >> 6);
|
||||
return ((unsigned int)(crc_first_byte) << 16) && ((unsigned int)(crc_middle_byte) << 8) && crc_last_byte;
|
||||
}
|
||||
|
||||
private:
|
||||
//int d_week; /* reception time */
|
||||
//int d_tow; /* reception time */
|
||||
Sbas_Time rx_time;
|
||||
int i_prn; /* SBAS satellite PRN number */
|
||||
std::vector<unsigned char> d_msg; /* SBAS message (226 bit) padded by 0 */
|
||||
@ -411,6 +402,7 @@ private:
|
||||
* return : satellite number (0:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int satno(int sys, int prn);
|
||||
|
||||
/* extract unsigned/signed bits ------------------------------------------------
|
||||
* extract unsigned/signed bits from byte data
|
||||
* args : unsigned char *buff I byte data
|
||||
@ -419,6 +411,7 @@ private:
|
||||
* return : extracted unsigned/signed bits
|
||||
*-----------------------------------------------------------------------------*/
|
||||
unsigned int getbitu(const unsigned char *buff, int pos, int len);
|
||||
|
||||
int getbits(const unsigned char *buff, int pos, int len);
|
||||
|
||||
/* convert calendar day/time to time -------------------------------------------
|
||||
@ -428,12 +421,14 @@ private:
|
||||
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
gtime_t epoch2time(const double *ep);
|
||||
|
||||
/* time difference -------------------------------------------------------------
|
||||
* difference between gtime_t structs
|
||||
* args : gtime_t t1,t2 I gtime_t structs
|
||||
* return : time difference (t1-t2) (s)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double timediff(gtime_t t1, gtime_t t2);
|
||||
|
||||
/* gps time to time ------------------------------------------------------------
|
||||
* convert week and tow in gps time to gtime_t struct
|
||||
* args : int week I week number in gps time
|
||||
|
||||
@ -54,7 +54,6 @@ public:
|
||||
i_gps_week = gps_week;
|
||||
d_delta_sec = gps_sec - time_stamp_sec;
|
||||
b_valid = true;
|
||||
|
||||
VLOG(FLOW) << "<<R>> new time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user