mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-24 05:53:16 +00:00
Code Issues Releases Wiki Activity

Improving documentation and code cleaning

Browse Source 
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 2013-11-23 13:05:38 +00:00
parent 3adee701fd
commit 3daf65bfc4
20 changed files with 427 additions and 792 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
src/core/system_parameters/Galileo_E1.h
View File

@ -41,25 +41,24 @@
#include "MATH_CONSTANTS.h"
// Physical constants
const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD
const double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2]
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_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD
const double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2]
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)]
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_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_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]
const double Galileo_E1_SUB_CARRIER_B_RATE_HZ = 6.138e6; //!< Galileo E1 sub-carrier 'b' rate [Hz]
const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips]
const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate [bits/second]
const double Galileo_E1_C_SECONDARY_CODE_LENGTH = 25.0; //!< Galileo E1-C secondary code length [chips]
const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips]
const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate [bits/second]
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
@ -69,16 +68,16 @@ const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (
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_SECONDS = 2; //a full page last 2 sec
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 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;
const int GALILEO_TELEMETRY_RATE_BITS_SECOND = 250; //bps
const int GALILEO_PAGE_TYPE_BITS = 6;
const int GALILEO_DATA_JK_BITS =128;
const int GALILEO_DATA_FRAME_BITS =196;
const int GALILEO_DATA_FRAME_BYTES =25;
const int GALILEO_DATA_JK_BITS = 128;
const int GALILEO_DATA_FRAME_BITS = 196;
const int GALILEO_DATA_FRAME_BYTES = 25;
const double GALIELO_E1_CODE_PERIOD = 0.004;
const std::vector<std::pair<int,int>> type({{1,6}});
@ -189,106 +188,106 @@ 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}});
const std::vector<std::pair<int,int>>t0a_7_bit({{13,10}});
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}});
const std::vector<std::pair<int,int>> t0a_7_bit({{13,10}});
const double t0a_7_LSB = 600;
const std::vector<std::pair<int,int>>SVID1_7_bit({{23,6}});
const std::vector<std::pair<int,int>>DELTA_A_7_bit({{29,13}});
const std::vector<std::pair<int,int>> SVID1_7_bit({{23,6}});
const std::vector<std::pair<int,int>> DELTA_A_7_bit({{29,13}});
const double DELTA_A_7_LSB = TWO_N9;
const std::vector<std::pair<int,int>>e_7_bit({{42,11}});
const std::vector<std::pair<int,int>> e_7_bit({{42,11}});
const double e_7_LSB = TWO_N16;
const std::vector<std::pair<int,int>>omega_7_bit({{53,16}});
const std::vector<std::pair<int,int>> omega_7_bit({{53,16}});
const double omega_7_LSB = TWO_N15;
const std::vector<std::pair<int,int>>delta_i_7_bit({{69,11}});
const std::vector<std::pair<int,int>> delta_i_7_bit({{69,11}});
const double delta_i_7_LSB = TWO_N14;
const std::vector<std::pair<int,int>>Omega0_7_bit({{80,16}});
const std::vector<std::pair<int,int>> Omega0_7_bit({{80,16}});
const double Omega0_7_LSB = TWO_N15;
const std::vector<std::pair<int,int>>Omega_dot_7_bit({{96,11}});
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 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}});
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}});
const double af0_8_LSB = TWO_N19;
const std::vector<std::pair<int,int>>af1_8_bit({{27,13}});
const std::vector<std::pair<int,int>> af1_8_bit({{27,13}});
const double af1_8_LSB = TWO_N38;
const std::vector<std::pair<int,int>>E5b_HS_8_bit({{40,2}});
const std::vector<std::pair<int,int>>E1B_HS_8_bit({{42,2}});
const std::vector<std::pair<int,int>>SVID2_8_bit({{44,6}});
const std::vector<std::pair<int,int>>DELTA_A_8_bit({{50,13}});
const std::vector<std::pair<int,int>> E5b_HS_8_bit({{40,2}});
const std::vector<std::pair<int,int>> E1B_HS_8_bit({{42,2}});
const std::vector<std::pair<int,int>> SVID2_8_bit({{44,6}});
const std::vector<std::pair<int,int>> DELTA_A_8_bit({{50,13}});
const double DELTA_A_8_LSB = TWO_N9;
const std::vector<std::pair<int,int>>e_8_bit({{63,11}});
const std::vector<std::pair<int,int>> e_8_bit({{63,11}});
const double e_8_LSB = TWO_N16;
const std::vector<std::pair<int,int>>omega_8_bit({{74,16}});
const std::vector<std::pair<int,int>> omega_8_bit({{74,16}});
const double omega_8_LSB = TWO_N15;
const std::vector<std::pair<int,int>>delta_i_8_bit({{90,11}});
const std::vector<std::pair<int,int>> delta_i_8_bit({{90,11}});
const double delta_i_8_LSB = TWO_N14;
const std::vector<std::pair<int,int>>Omega0_8_bit({{101,16}});
const std::vector<std::pair<int,int>> Omega0_8_bit({{101,16}});
const double Omega0_8_LSB = TWO_N15;
const std::vector<std::pair<int,int>>Omega_dot_8_bit({{117,11}});
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}});
const std::vector<std::pair<int,int>>t0a_9_bit({{13,10}});
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}});
const std::vector<std::pair<int,int>> t0a_9_bit({{13,10}});
const double t0a_9_LSB = 600;
const std::vector<std::pair<int,int>>M0_9_bit({{23,16}});
const std::vector<std::pair<int,int>> M0_9_bit({{23,16}});
const double M0_9_LSB = TWO_N15;
const std::vector<std::pair<int,int>>af0_9_bit({{39,16}});
const std::vector<std::pair<int,int>> af0_9_bit({{39,16}});
const double af0_9_LSB = TWO_N19;
const std::vector<std::pair<int,int>>af1_9_bit({{55,13}});
const std::vector<std::pair<int,int>> af1_9_bit({{55,13}});
const double af1_9_LSB = TWO_N38;
const std::vector<std::pair<int,int>>E5b_HS_9_bit({{68,2}});
const std::vector<std::pair<int,int>>E1B_HS_9_bit({{70,2}});
const std::vector<std::pair<int,int>>SVID3_9_bit({{72,6}});
const std::vector<std::pair<int,int>>DELTA_A_9_bit({{78,13}});
const std::vector<std::pair<int,int>> E5b_HS_9_bit({{68,2}});
const std::vector<std::pair<int,int>> E1B_HS_9_bit({{70,2}});
const std::vector<std::pair<int,int>> SVID3_9_bit({{72,6}});
const std::vector<std::pair<int,int>> DELTA_A_9_bit({{78,13}});
const double DELTA_A_9_LSB = TWO_N9;
const std::vector<std::pair<int,int>>e_9_bit({{91,11}});
const std::vector<std::pair<int,int>> e_9_bit({{91,11}});
const double e_9_LSB = TWO_N16;
const std::vector<std::pair<int,int>>omega_9_bit({{102,16}});
const std::vector<std::pair<int,int>> omega_9_bit({{102,16}});
const double omega_9_LSB = TWO_N15;
const std::vector<std::pair<int,int>>delta_i_9_bit({{118,11}});
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}});
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}});
const double Omega0_10_LSB = TWO_N15;
const std::vector<std::pair<int,int>>Omega_dot_10_bit({{27,11}});
const std::vector<std::pair<int,int>> Omega_dot_10_bit({{27,11}});
const double Omega_dot_10_LSB = TWO_N33;
const std::vector<std::pair<int,int>>M0_10_bit({{38,16}});
const std::vector<std::pair<int,int>> M0_10_bit({{38,16}});
const double M0_10_LSB = TWO_N15;
const std::vector<std::pair<int,int>>af0_10_bit({{54,16}});
const std::vector<std::pair<int,int>> af0_10_bit({{54,16}});
const double af0_10_LSB = TWO_N19;
const std::vector<std::pair<int,int>>af1_10_bit({{70,13}});
const std::vector<std::pair<int,int>> af1_10_bit({{70,13}});
const double af1_10_LSB = TWO_N38;
const std::vector<std::pair<int,int>>E5b_HS_10_bit({{83,2}});
const std::vector<std::pair<int,int>>E1B_HS_10_bit({{85,2}});
const std::vector<std::pair<int,int>> E5b_HS_10_bit({{83,2}});
const std::vector<std::pair<int,int>> E1B_HS_10_bit({{85,2}});
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 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 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}});
const std::vector<std::pair<int,int>> WN_0G_10_bit({{123,6}});
/* Page 0 */
const std::vector<std::pair<int,int>>Time_0_bit({{7,2}});
const std::vector<std::pair<int,int>>WN_0_bit({{97,12}});
const std::vector<std::pair<int,int>> Time_0_bit({{7,2}});
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_ */

6
src/core/system_parameters/galileo_almanac.cc
View File

@ -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
*
* -------------------------------------------------------------------------

12
src/core/system_parameters/galileo_almanac.h
View File

@ -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

61
src/core/system_parameters/galileo_ephemeris.cc
View File

@ -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]
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,24 +96,23 @@ 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
double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Satellite Time Correction Algorithm, ICD 5.1.4
{
double tk;
double a;
@ -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;
@ -146,13 +139,13 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) //Sate
M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
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++)
for (int ii = 1; ii < 20; ii++)
{
E_old = E;
E = M + e_1 * sin(E);
@ -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,23 +164,21 @@ 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)
{
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)
// 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 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 nu; // True anomaly
double phi; // Argument of Latitude
double u; // Correct argument of latitude
double r; // Correct radius
double i;
double Omega;
@ -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

155
src/core/system_parameters/galileo_ephemeris.h
View File

@ -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,63 +42,64 @@
* \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]
/*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;
/*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]
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]
/*GST*/
//Not belong to ephemeris set (page 1 to 4)
double WN_5; //!< Week number
double TOW_5; //!< Time of Week
double Galileo_satClkDrift;
double Galileo_dtr; //!< relativistic clock correction term
/*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]
// 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).
/*GST*/
//Not belong to ephemeris set (page 1 to 4)
double WN_5; //Week number
double TOW_5; //Time of Week
double Galileo_satClkDrift;
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]
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>
\\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.
@ -108,60 +109,46 @@ public:
using boost::serialization::make_nvp;
archive & make_nvp("i_satellite_PRN",i_satellite_PRN); // SV PRN NUMBER
archive & make_nvp("d_TOW",d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
archive & make_nvp("d_Crs",d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
archive & make_nvp("d_Delta_n",d_Delta_n); //!< Mean Motion Difference From Computed Value [semi-circles/s]
archive & make_nvp("d_M_0",d_M_0); //!< Mean Anomaly at Reference Time [semi-circles]
archive & make_nvp("d_Cuc",d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
archive & make_nvp("d_TOW",d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
archive & make_nvp("d_Crs",d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
archive & make_nvp("d_Delta_n",d_Delta_n); //!< Mean Motion Difference From Computed Value [semi-circles/s]
archive & make_nvp("d_M_0",d_M_0); //!< Mean Anomaly at Reference Time [semi-circles]
archive & make_nvp("d_Cuc",d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
archive & make_nvp("d_e_eccentricity",d_e_eccentricity); //!< Eccentricity [dimensionless]
archive & make_nvp("d_Cus",d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
archive & make_nvp("d_sqrt_A",d_sqrt_A); //!< Square Root of the Semi-Major Axis [sqrt(m)]
archive & make_nvp("d_Toe",d_Toe); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
archive & make_nvp("d_Toc",d_Toe); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s]
archive & make_nvp("d_Cic",d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
archive & make_nvp("d_OMEGA0",d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
archive & make_nvp("d_Cis",d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
archive & make_nvp("d_i_0",d_i_0); //!< Inclination Angle at Reference Time [semi-circles]
archive & make_nvp("d_Crc",d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
archive & make_nvp("d_OMEGA",d_OMEGA); //!< Argument of Perigee [semi-cicles]
archive & make_nvp("d_Cus",d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
archive & make_nvp("d_sqrt_A",d_sqrt_A); //!< Square Root of the Semi-Major Axis [sqrt(m)]
archive & make_nvp("d_Toe",d_Toe); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
archive & make_nvp("d_Toc",d_Toe); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s]
archive & make_nvp("d_Cic",d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
archive & make_nvp("d_OMEGA0",d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
archive & make_nvp("d_Cis",d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
archive & make_nvp("d_i_0",d_i_0); //!< Inclination Angle at Reference Time [semi-circles]
archive & make_nvp("d_Crc",d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
archive & make_nvp("d_OMEGA",d_OMEGA); //!< Argument of Perigee [semi-cicles]
archive & make_nvp("d_OMEGA_DOT",d_OMEGA_DOT); //!< Rate of Right Ascension [semi-circles/s]
archive & make_nvp("d_IDOT",d_IDOT); //!< Rate of Inclination Angle [semi-circles/s]
archive & make_nvp("d_IDOT",d_IDOT); //!< Rate of Inclination Angle [semi-circles/s]
archive & make_nvp("i_code_on_L2",i_code_on_L2); //!< If 1, P code ON in L2; if 2, C/A code ON in L2;
archive & make_nvp("i_GPS_week",i_GPS_week); //!< GPS week number, aka WN [week]
archive & make_nvp("b_L2_P_data_flag",b_L2_P_data_flag); //!< When true, indicates that the NAV data stream was commanded OFF on the P-code of the L2 channel
archive & make_nvp("i_SV_accuracy",i_SV_accuracy); //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200E)
archive & make_nvp("i_SV_health",i_SV_health);
archive & make_nvp("d_TGD",d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
archive & make_nvp("d_IODC",d_IODC); //!< Issue of Data, Clock
archive & make_nvp("i_AODO",i_AODO); //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
archive & make_nvp("d_TGD",d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
archive & make_nvp("d_IODC",d_IODC); //!< Issue of Data, Clock
archive & make_nvp("i_AODO",i_AODO); //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
archive & make_nvp("b_fit_interval_flag",b_fit_interval_flag);//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
archive & make_nvp("d_spare1",d_spare1);
archive & make_nvp("d_spare2",d_spare2);
archive & make_nvp("d_A_f0",d_A_f0); //!< Coefficient 0 of code phase offset model [s]
archive & make_nvp("d_A_f1",d_A_f1); //!< Coefficient 1 of code phase offset model [s/s]
archive & make_nvp("d_A_f2",d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2]
archive & make_nvp("d_A_f0",d_A_f0); //!< Coefficient 0 of code phase offset model [s]
archive & make_nvp("d_A_f1",d_A_f1); //!< Coefficient 1 of code phase offset model [s/s]
archive & make_nvp("d_A_f2",d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2]
archive & make_nvp("b_integrity_status_flag",b_integrity_status_flag);
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

19
src/core/system_parameters/galileo_iono.cc
View File

@ -33,22 +33,19 @@
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]
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]
/* Ionospheric disturbance flag */
Region1_flag_5 = false; // Ionospheric Disturbance Flag for region 1
Region2_flag_5 = false; // Ionospheric Disturbance Flag for region 2
Region3_flag_5 = false; // Ionospheric Disturbance Flag for region 3
Region4_flag_5 = false; // Ionospheric Disturbance Flag for region 4
Region5_flag_5 = false; // Ionospheric Disturbance Flag for region 5
Region1_flag_5 = false; // Ionospheric Disturbance Flag for region 1
Region2_flag_5 = false; // Ionospheric Disturbance Flag for region 2
Region3_flag_5 = false; // Ionospheric Disturbance Flag for region 3
Region4_flag_5 = false; // Ionospheric Disturbance Flag for region 4
Region5_flag_5 = false; // Ionospheric Disturbance Flag for region 5
TOW_5 = 0;
WN_5 = 0;
}

34
src/core/system_parameters/galileo_iono.h
View File

@ -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,32 +40,24 @@
*/
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
*/
Galileo_Iono();

329
src/core/system_parameters/galileo_navigation_message.cc
View File

@ -113,15 +113,15 @@ void Galileo_Navigation_Message::reset()
ai1_5 = 0; //
ai2_5 = 0; //
/*Ionospheric disturbance flag*/
Region1_flag_5 = 0; //Region1_flag_5;
Region1_flag_5 = 0; // Region1_flag_5;
Region2_flag_5 = 0; //
Region3_flag_5 = 0; //
Region4_flag_5 = 0; //
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 page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page
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;
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;
TLM_word_for_CRC = TLM_word_for_CRC_stream.str().substr(0, GALILEO_DATA_FRAME_BITS);
std::bitset<GALILEO_DATA_FRAME_BITS> TLM_word_for_CRC_bits(TLM_word_for_CRC);
std::bitset<24> checksum(CRC_data);
@ -426,35 +409,27 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
// std::cout << "Tail odd is not correct!" << std::endl;
//else std::cout<<"Tail odd is correct!"<<std::endl;
if (CRC_test(TLM_word_for_CRC_bits,checksum.to_ulong())==true)
if (CRC_test(TLM_word_for_CRC_bits, checksum.to_ulong()) == true)
{
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;
//
//}
//
//
//

9
src/core/system_parameters/galileo_utc_model.h
View File

@ -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
*/

9
src/core/system_parameters/gps_ephemeris.cc
View File

@ -60,9 +60,9 @@ Gps_Ephemeris::Gps_Ephemeris()
i_SV_health = 0;
d_TGD = 0; //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
d_IODC = 0; //!< Issue of Data, Clock
i_AODO = 0; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
i_AODO = 0; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
b_fit_interval_flag = false;//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
b_fit_interval_flag = false; //!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
d_spare1 = 0;
d_spare2 = 0;
@ -71,10 +71,9 @@ Gps_Ephemeris::Gps_Ephemeris()
d_A_f2 = 0; //!< Coefficient 2 of code phase offset model [s/s^2]
b_integrity_status_flag = false;
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_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)
{

15
src/core/system_parameters/gps_ephemeris.h
View File

@ -55,10 +55,9 @@ 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]
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles]
@ -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.
@ -107,12 +105,12 @@ public:
* accompanying alert, is less than 1E-8 per hour.
*/
bool b_integrity_status_flag;
bool 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.
bool 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.
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)

13
src/core/system_parameters/gps_iono.h
View File

@ -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)

14
src/core/system_parameters/sbas_ephemeris.h
View File

@ -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)
};

126
src/core/system_parameters/sbas_ionospheric_correction.cc
View File

@ -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,35 +42,46 @@ 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)
{
int band = it_band-d_bands.begin();
int band = it_band - d_bands.begin();
out << "<<I>> Band" << band << ":" << std::endl;
for(std::vector<Igp>::const_iterator it_igp = it_band->d_igps.begin(); it_igp != it_band->d_igps.end(); ++it_igp)
{
int igp = it_igp-it_band->d_igps.begin();
out << "<<I>> -IGP" << igp << ":";
//std::cout << " valid=" << it_igp->d_valid;
out << " t0=" << it_igp->t0;
out << " lat=" << it_igp->d_latitude;
out << " lon=" << it_igp->d_longitude;
out << " give=" << it_igp->d_give;
out << " delay=" << it_igp->d_delay;
out << std::endl;
}
for(std::vector<Igp>::const_iterator it_igp = it_band->d_igps.begin(); it_igp != it_band->d_igps.end(); ++it_igp)
{
int igp = it_igp-it_band->d_igps.begin();
out << "<<I>> -IGP" << igp << ":";
//std::cout << " valid=" << it_igp->d_valid;
out << " t0=" << it_igp->t0;
out << " lat=" << it_igp->d_latitude;
out << " lon=" << it_igp->d_longitude;
out << " give=" << it_igp->d_give;
out << " delay=" << it_igp->d_delay;
out << std::endl;
}
}
}
/*
* -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;
@ -134,25 +144,25 @@ void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, dou
const double *A, const double *B, double beta, double *C)
{
double d;
int i, j, x, f = tr[0]=='N' ? (tr[1]=='N'?1:2):(tr[1]=='N'?3:4);
int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);
for (i=0; i<n; i++) for (j=0; j<k; j++)
for (i = 0; i < n; i++) for (j = 0; j < k; j++)
{
d = 0.0;
switch (f)
{
case 1: for (x=0; x<m; x++) d += A[i+x*n]*B[x+j*m]; break;
case 2: for (x=0; x<m; x++) d += A[i+x*n]*B[j+x*k]; break;
case 3: for (x=0; x<m; x++) d += A[x+i*m]*B[x+j*m]; break;
case 4: for (x=0; x<m; x++) d += A[x+i*m]*B[j+x*k]; break;
case 1: for (x = 0; x < m; x++) d += A[i + x*n]*B[x + j*m]; break;
case 2: for (x = 0; x < m; x++) d += A[i + x*n]*B[j + x*k]; break;
case 3: for (x = 0; x < m; x++) d += A[x + i*m]*B[x + j*m]; break;
case 4: for (x = 0; x < m; x++) d += A[x + i*m]*B[j + x*k]; break;
}
if (beta == 0.0)
{
C[i+j*n] = alpha*d;
C[i + j*n] = alpha*d;
}
else
{
C[i+j*n] = alpha*d + beta*C[i+j*n];
C[i + j*n] = alpha*d + beta*C[i + j*n];
}
}
}
@ -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 -------------------------------------------
@ -213,11 +220,11 @@ double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, d
if (pos[2] > -RE_WGS84)
{
ecef2enu(pos, e, enu);
az = dot(enu ,enu, 2) < 1E-12 ? 0.0:atan2(enu[0], enu[1]);
if (az < 0.0) az += 2*PI;
el = asin(enu[2]);
}
ecef2enu(pos, e, enu);
az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]);
if (az < 0.0) az += 2*PI;
el = asin(enu[2]);
}
if (azel)
{
azel[0] = az;
@ -252,10 +259,9 @@ 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);
rp = re/(re + hion)*cos(azel[1]);
ap = PI/2.0 - azel[1] - asin(rp);
sinap = sin(ap);
tanap = tan(ap);
cosaz = cos(azel[0]);
@ -291,8 +297,7 @@ 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 */
const double R2D = (180.0/PI); /* rad to deg */
double lat = pos[0]*R2D;
double lon = pos[1]*R2D;
@ -304,24 +309,24 @@ void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, d
if (-55.0 <= lat && lat < 55.0)
{
latp[0] = (int)floor(lat/5.0)*5;
latp[1] = latp[0]+5;
latp[1] = latp[0] + 5;
lonp[0] = lonp[1] = (int)floor(lon/5.0)*5;
lonp[2] = lonp[3] = lonp[0] + 5;
*x = (lon-lonp[0])/5.0;
*y = (lat-latp[0])/5.0;
*x = (lon - lonp[0])/5.0;
*y = (lat - latp[0])/5.0;
}
else
{
latp[0] = (int)floor((lat-5.0)/10.0)*10+5;
latp[1] = latp[0]+10;
latp[1] = latp[0] + 10;
lonp[0] = lonp[1] = (int)floor(lon/10.0)*10;
lonp[2] = lonp[3] = lonp[0]+10;
lonp[2] = lonp[3] = lonp[0] + 10;
*x = (lon - lonp[0])/10.0;
*y = (lat - latp[0])/10.0;
if (75.0 <= lat && lat < 85.0)
{
lonp[1] = (int)floor(lon/90.0)*90;
lonp[3] = lonp[1]+90;
lonp[3] = lonp[1] + 90;
}
else if (-85.0 <= lat && lat < -75.0)
{
@ -330,15 +335,15 @@ void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, d
}
else if (lat >= 85.0)
{
for (i=0; i<4; i++) lonp[i] = (int)floor(lon/90.0)*90;
for (i = 0; i < 4; i++) lonp[i] = (int)floor(lon/90.0)*90;
}
else if (lat <- 85.0)
{
for (i=0; i<4 ;i++) lonp[i] = (int)floor((lon - 50.0)/90.0)*90 + 40;
for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0)/90.0)*90 + 40;
}
}
for (i=0; i<4; i++) if (lonp[i] == 180) lonp[i] = -180;
for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180;
// find the correction data for the grid points in latp[] and lonp[]
// iterate over bands
@ -356,10 +361,10 @@ void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, d
int lat = igp_it->d_latitude;
int lon = igp_it->d_longitude;
//ss << " lat=" << lat << " lon=" << lon;
if (lat==latp[0] && lon==lonp[0]) igp[0] = igp_it.base();
else if (lat==latp[1] && lon==lonp[1]) igp[1] = igp_it.base();
else if (lat==latp[0] && lon==lonp[2]) igp[2] = igp_it.base();
else if (lat==latp[1] && lon==lonp[3]) igp[3] = igp_it.base();
if (lat == latp[0] && lon == lonp[0]) igp[0] = igp_it.base();
else if (lat == latp[1] && lon == lonp[1]) igp[1] = igp_it.base();
else if (lat == latp[0] && lon == lonp[2]) igp[2] = igp_it.base();
else if (lat == latp[1] && lon == lonp[3]) igp[3] = igp_it.base();
}
//VLOG(MORE) << ss.str();
}
@ -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);
@ -451,7 +455,7 @@ int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const doub
trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f", posp[0]*R2D, posp[1]*R2D);
return 0;
}
for (int i=0; i<4; i++)
for (int i = 0; i <4 ; i++)
{
if (!igp[i]) continue;
t = (sample_stamp - igp[i]->t0); // time diff between now and reception of the igp data in seconds

90
src/core/system_parameters/sbas_ionospheric_correction.h
View File

@ -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
/*!
* \brief Class that handles valid SBAS ionospheric 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
/* 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);

6
src/core/system_parameters/sbas_satellite_correction.cc
View File

@ -287,10 +287,10 @@ int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double
{
return 0;
}
for (i=0; i<3; i++) rs[i] += drs[i];
for (i = 0; i < 3; i++) rs[i] += drs[i];
dts[0] += dclk + prc/CLIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f",
d_prn,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var);
trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f",
d_prn, drs[0], drs[1], drs[2], dclk,prc/CLIGHT, *var);
return 1;
}

21
src/core/system_parameters/sbas_satellite_correction.h
View File

@ -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, ...);

118
src/core/system_parameters/sbas_telemetry_data.cc
View File

@ -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
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;
@ -121,7 +127,7 @@ int Sbas_Telemetry_Data::update(Sbas_Raw_Msg sbas_raw_msg)
int i = it - msg_bytes.begin();
sbas_raw_msg_rtklib.msg[i] = *it;
}
parsing_result = sbsupdatecorr(&sbas_raw_msg_rtklib, &d_nav);
parsing_result = sbsupdatecorr(&sbas_raw_msg_rtklib, &d_nav);
VLOG(FLOW) << "<<T>> RTKLIB parsing result: " << parsing_result;
}
@ -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,20 +151,25 @@ 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
unsigned char * msg = sbas_raw_msg.get_msg().data();
uint32_t gps_tow = getbitu(msg, 121, 20);
uint32_t gps_week = getbitu(msg, 141, 10)+1024; // consider last gps time week overflow
uint32_t gps_week = getbitu(msg, 141, 10) + 1024; // consider last gps time week overflow
double gps_tow_rx = double(gps_tow) + rx_delay;
mt12_time_ref = Sbas_Time_Relation(sbas_raw_msg.get_sample_stamp(), gps_week, gps_tow_rx);
VLOG(FLOW) << "<<T>> extracted GPS time from MT12: gps_tow=" << gps_tow << " gps_week=" << gps_week;
@ -170,13 +181,10 @@ 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;
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);
}
@ -327,7 +335,7 @@ void Sbas_Telemetry_Data::updated_satellite_corrections()
// check if fast corrections got updated
std::map<int, Fast_Correction>::iterator it_old_fcorr = emitted_fast_corrections.find(prn);
if(it_old_fcorr == emitted_fast_corrections.end() || !are_equal<Fast_Correction>(fcorr, it_old_fcorr->second ))
if(it_old_fcorr == emitted_fast_corrections.end() || !are_equal < Fast_Correction>(fcorr, it_old_fcorr->second ))
{
// got updated
ss << " fast_correction_updated=" << true;
@ -339,7 +347,7 @@ void Sbas_Telemetry_Data::updated_satellite_corrections()
// check if long term corrections got updated
std::map<int, Long_Term_Correction>::iterator it_old_lcorr = emitted_long_term_corrections.find(prn);
if(it_old_lcorr == emitted_long_term_corrections.end() || !are_equal<Long_Term_Correction>(lcorr, it_old_lcorr->second ))
if(it_old_lcorr == emitted_long_term_corrections.end() || !are_equal < Long_Term_Correction>(lcorr, it_old_lcorr->second ))
{
// got updated
ss << " long_term_correction_updated=" << true;
@ -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,...)
@ -434,7 +444,7 @@ unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, in
{
unsigned int bits = 0;
int i;
for (i=pos; i<pos+len; i++) bits = (bits<<1) + ((buff[i/8]>>(7-i%8))&1u);
for (i = pos; i < pos + len; i++) bits = (bits << 1) + ((buff[i/8] >> (7 - i % 8)) & 1u);
return bits;
}
@ -442,8 +452,8 @@ unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, in
int Sbas_Telemetry_Data::getbits(const unsigned char *buff, int pos, int len)
{
unsigned int bits=getbitu(buff,pos,len);
if (len<=0 || 32<=len || !(bits&(1u<<(len-1)))) return (int)bits;
unsigned int bits = getbitu(buff,pos,len);
if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) return (int)bits;
return (int)(bits|(~0u << len)); /* extend sign */
}
@ -505,27 +515,27 @@ Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::gpst2time(int week, double sec
/* sbas igp definition -------------------------------------------------------*/
const short
Sbas_Telemetry_Data::x1[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20,
Sbas_Telemetry_Data::x1[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75, 85},
Sbas_Telemetry_Data::x2[]={-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30,
Sbas_Telemetry_Data::x2[] = {-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30,
35, 40, 45, 50, 55},
Sbas_Telemetry_Data::x3[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20,
Sbas_Telemetry_Data::x3[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75},
Sbas_Telemetry_Data::x4[]={-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15,
Sbas_Telemetry_Data::x4[] = {-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15,
20, 25, 30, 35, 40, 45, 50, 55, 65, 75},
Sbas_Telemetry_Data::x5[]={-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115,
Sbas_Telemetry_Data::x5[] = {-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115,
-110,-105,-100,- 95,- 90,- 85,- 80,- 75,- 70,- 65,- 60,- 55,- 50,- 45,
- 40,- 35,- 30,- 25,- 20,- 15,- 10,- 5, 0, 5, 10, 15, 20, 25,
30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95,
100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165,
170, 175},
Sbas_Telemetry_Data::x6[]={-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50,
Sbas_Telemetry_Data::x6[] = {-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50,
-40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90,
100, 110, 120, 130, 140, 150, 160, 170},
Sbas_Telemetry_Data::x7[]={-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150},
Sbas_Telemetry_Data::x8[]={-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160};
Sbas_Telemetry_Data::x7[] = {-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150},
Sbas_Telemetry_Data::x8[] = {-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160};
const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband1[9][8]={ /* band 0-8 */
const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband1[9][8] = { /* band 0-8 */
{{-180,x1, 1, 28},{-175,x2, 29, 51},{-170,x3, 52, 78},{-165,x2, 79,101},
{-160,x3,102,128},{-155,x2,129,151},{-150,x3,152,178},{-145,x2,179,201}},
{{-140,x4, 1, 28},{-135,x2, 29, 51},{-130,x3, 52, 78},{-125,x2, 79,101},
@ -548,7 +558,7 @@ const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband1[9][8]={ /*
const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5]={ /* band 9-10 */
const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5] = { /* band 9-10 */
{{ 60,x5, 1, 72},{ 65,x6, 73,108},{ 70,x6,109,144},{ 75,x6,145,180},
{ 85,x7,181,192}},
{{- 60,x5, 1, 72},{- 65,x6, 73,108},{- 70,x6,109,144},{- 75,x6,145,180},
@ -562,20 +572,20 @@ int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
int i, n, sat;
// see figure A-6: i corresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number
trace(4,"decode_sbstype1:");
trace(4, "decode_sbstype1:");
for (i=1, n=0; i<=210 && n<MAXSAT; i++)
for (i = 1, n = 0; i <= 210 && n < MAXSAT; i++)
{
if (getbitu(msg->msg, 13+i, 1))
if (getbitu(msg->msg, 13 + i, 1))
{
if (i <= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */
else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */
else if (i <= 119) sat = 0; /* 62-119: future gnss */
else if (i <= 138) sat = satno(SYS_SBS, i); /* 120-138: geo/waas */
else if (i <= 182) sat = 0; /* 139-182: reserved */
else if (i <= 192) sat = satno(SYS_SBS, i + 10); /* 183-192: qzss ref [2] */
else if (i <= 202) sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */
else sat = 0; /* 203- : reserved */
if (i <= 37) sat = satno(SYS_GPS, i); /* 0 - 37: gps */
else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38 - 61: glonass */
else if (i <= 119) sat = 0; /* 62 - 119: future gnss */
else if (i <= 138) sat = satno(SYS_SBS, i); /* 120 - 138: geo/waas */
else if (i <= 182) sat = 0; /* 139 - 182: reserved */
else if (i <= 192) sat = satno(SYS_SBS, i + 10); /* 183 - 192: qzss ref [2] */
else if (i <= 202) sat = satno(SYS_QZS, i); /* 193 - 202: qzss ref [2] */
else sat = 0; /* 203 - : reserved */
sbssat->sat[n++].sat = sat;
}
}
@ -749,8 +759,8 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
for (i=1, n=0; i <= 201; i++)
{
if (!getbitu(msg->msg, 23+i, 1)) continue;
for (j=0; j<m; j++)
if (!getbitu(msg->msg, 23 + i, 1)) continue;
for (j = 0; j < m; j++)
{
if (i < p[j].bits || p[j].bite < i) continue;
sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x;
@ -778,7 +788,7 @@ int Sbas_Telemetry_Data::decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
for (i=0; i<3; i++)
for (i = 0; i < 3; i++)
{
sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9*i, 9)*0.125;
sbssat->sat[n - 1].lcorr.dvel[i] = 0.0;
@ -804,7 +814,7 @@ int Sbas_Telemetry_Data::decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *
trace(4,"decode_longcorr1:");
if (n==0 || n>MAXSAT) return 0;
if (n == 0 || n > MAXSAT) return 0;
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
@ -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)
{
@ -903,7 +914,7 @@ int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
block = getbitu(msg->msg, 18, 4);
for (i=0; i<15; i++)
for (i = 0; i < 15; i++)
{
if ((j = block*15 + i) >= sbsion[band].nigp) continue;
give = getbitu(msg->msg, 2 + i*13 + 9, 4);
@ -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;

33
src/core/system_parameters/sbas_telemetry_data.h
View File

@ -39,40 +39,34 @@
#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)
/*!
* \brief Represents a raw SBAS message of 250cbits + 6 bits padding
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding)
*/
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(){ rx_time = Sbas_Time(0); i_prn = -1; };
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);
}
Sbas_Time get_rx_time_obj(){return rx_time;}
int get_prn() {return i_prn;}
std::vector<unsigned char> get_msg() const {return d_msg;}
Sbas_Time get_rx_time_obj(){ return rx_time; }
int get_prn() { return i_prn; }
std::vector<unsigned char> get_msg() const { return d_msg; }
int get_preamble()
{
return d_msg[0];
@ -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

1
src/core/system_parameters/sbas_time.h
View File

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