mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-15 19:55:47 +00:00
Code Issues Releases Wiki Activity

fixing order in MSM signal data

Browse Source
This commit is contained in:
Carles Fernandez 2015-12-05 11:27:15 +01:00
parent e47b5c003c
commit 4958874269
2 changed files with 183 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/core/system_parameters/rtcm.cc
View File

@ -74,7 +74,7 @@ std::string Rtcm::add_CRC (const std::string& message_without_crc)
boost::dynamic_bitset<unsigned char> frame_bits(message_without_crc);
std::vector<unsigned char> bytes;
boost::to_block_range(frame_bits, std::back_inserter(bytes));
std::reverse(bytes.begin(),bytes.end());
std::reverse(bytes.begin(), bytes.end());
// 2) Computes CRC
CRC_RTCM.process_bytes(bytes.data(), bytes.size());
@ -99,7 +99,7 @@ bool Rtcm::check_CRC(const std::string & message)
boost::dynamic_bitset<unsigned char> frame_bits(msg_without_crc);
std::vector<unsigned char> bytes;
boost::to_block_range(frame_bits, std::back_inserter(bytes));
std::reverse(bytes.begin(),bytes.end());
std::reverse(bytes.begin(), bytes.end());
CRC_RTCM_CHECK.process_bytes(bytes.data(), bytes.size());
std::bitset<24> computed_crc = std::bitset<24>(CRC_RTCM_CHECK.checksum());
@ -1144,18 +1144,18 @@ std::string Rtcm::get_MSM_1_content_signal_data(const std::map<int, Gnss_Synchro
unsigned int Ncells = pseudoranges.size();
std::vector<std::pair<int, Gnss_Synchro> > pseudoranges_vector;
std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
std::map<int, Gnss_Synchro>::const_iterator map_iter;
for(gnss_synchro_iter = pseudoranges.begin();
gnss_synchro_iter != pseudoranges.end();
gnss_synchro_iter++)
for(map_iter = pseudoranges.begin();
map_iter != pseudoranges.end();
map_iter++)
{
pseudoranges_vector.push_back(*gnss_synchro_iter);
pseudoranges_vector.push_back(*map_iter);
}
std::vector<std::pair<int, Gnss_Synchro> > ordered_by_PRN_pos = Rtcm::sort_by_PRN_mask(pseudoranges_vector);
std::vector<std::pair<int, Gnss_Synchro> > ordered_by_signal = Rtcm::sort_by_signal(ordered_by_PRN_pos);
std::vector<std::pair<int, Gnss_Synchro> > ordered_by_signal = Rtcm::sort_by_signal(pseudoranges_vector);
std::reverse(ordered_by_signal.begin(), ordered_by_signal.end());
std::vector<std::pair<int, Gnss_Synchro> > ordered_by_PRN_pos = Rtcm::sort_by_PRN_mask(ordered_by_signal);
for(unsigned int cell = 0; cell < Ncells ; cell++)
{
@ -1944,6 +1944,7 @@ int Rtcm::set_DF292(const Galileo_Ephemeris & gal_eph)
return 0;
}
int Rtcm::set_DF293(const Galileo_Ephemeris & gal_eph)
{
@ -2012,6 +2013,7 @@ int Rtcm::set_DF300(const Galileo_Ephemeris & gal_eph)
return 0;
}
int Rtcm::set_DF301(const Galileo_Ephemeris & gal_eph)
{
unsigned long int ecc = static_cast<unsigned long int>(std::round(gal_eph.e_1 / FNAV_e_2_LSB));
@ -2410,7 +2412,6 @@ int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
}
if (sig.compare("5X") == 0 )
{
lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
@ -2419,6 +2420,10 @@ int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
}
if (sig.compare("7X") == 0 )
{
lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
double rough_phase_range_ms = std::round(- gnss_synchro.Carrier_Doppler_hz / lambda);
DF399 = std::bitset<14>(static_cast<int>(rough_phase_range_ms));
@ -2472,7 +2477,6 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
}
if (sig.compare("5X") == 0 )
{
lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
@ -2481,6 +2485,10 @@ int Rtcm::set_DF401(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
}
if (sig.compare("7X") == 0 )
{
lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI ) * lambda - rough_range_m;
@ -2526,7 +2534,6 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
}
if (sig.compare("5X") == 0 )
{
lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
@ -2535,6 +2542,10 @@ int Rtcm::set_DF404(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
}
if (sig.compare("7X") == 0 )
{
lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
double phrr = std::round(- gnss_synchro.Carrier_Doppler_hz / lambda);
@ -2601,7 +2612,6 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
}
if (sig.compare("5X") == 0 )
{
lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
@ -2610,6 +2620,11 @@ int Rtcm::set_DF406(const Gnss_Synchro & gnss_synchro)
{
lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
}
if (sig.compare("7X") == 0 )
{
lambda = GPS_C_m_s / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI ) * lambda - rough_range_m;
if(phrng_m == 0.0)
{

158
src/tests/formats/rtcm_test.cc
View File

@ -269,6 +269,135 @@ TEST(Rtcm_Test, MT1001)
}
TEST(Rtcm_Test, MSMCell)
{
auto rtcm = std::make_shared<Rtcm>();
Gps_Ephemeris gps_eph = Gps_Ephemeris();
Galileo_Ephemeris gal_eph = Galileo_Ephemeris();
std::map<int, Gnss_Synchro> pseudoranges;
Gnss_Synchro gnss_synchro;
Gnss_Synchro gnss_synchro2;
Gnss_Synchro gnss_synchro3;
Gnss_Synchro gnss_synchro4;
Gnss_Synchro gnss_synchro5;
gnss_synchro.PRN = 4;
gnss_synchro2.PRN = 8;
gnss_synchro3.PRN = 32;
gnss_synchro4.PRN = 10;
gnss_synchro5.PRN = 10;
std::string gps = "G";
std::string gal = "E";
std::string c1 = "1C";
std::string s2 = "2S";
std::string x5 = "5X";
gnss_synchro.System = *gal.c_str();
gnss_synchro2.System = *gps.c_str();
gnss_synchro3.System = *gps.c_str();
gnss_synchro4.System = *gal.c_str();
gnss_synchro5.System = *gps.c_str();
std::memcpy((void*)gnss_synchro.Signal, x5.c_str(), 3);
std::memcpy((void*)gnss_synchro2.Signal, s2.c_str(), 3);
std::memcpy((void*)gnss_synchro3.Signal, c1.c_str(), 3);
std::memcpy((void*)gnss_synchro4.Signal, x5.c_str(), 3);
std::memcpy((void*)gnss_synchro5.Signal, c1.c_str(), 3);
gnss_synchro.Pseudorange_m = 20000000.0;
gnss_synchro2.Pseudorange_m = 20001010.0;
gnss_synchro3.Pseudorange_m = 24002020.0;
gnss_synchro4.Pseudorange_m = 20003010.1;
gnss_synchro5.Pseudorange_m = 22003010.1;
pseudoranges.insert(std::pair<int, Gnss_Synchro>(1, gnss_synchro));
pseudoranges.insert(std::pair<int, Gnss_Synchro>(2, gnss_synchro2));
pseudoranges.insert(std::pair<int, Gnss_Synchro>(3, gnss_synchro3));
pseudoranges.insert(std::pair<int, Gnss_Synchro>(4, gnss_synchro4));
pseudoranges.insert(std::pair<int, Gnss_Synchro>(5, gnss_synchro5));
unsigned int ref_id = 1234;
unsigned int clock_steering_indicator = 0;
unsigned int external_clock_indicator = 0;
int smooth_int = 0;
bool sync_flag = false;
bool divergence_free = false;
bool more_messages = false;
double obs_time = 25.0;
gps_eph.i_satellite_PRN = gnss_synchro.PRN;
std::string MSM1 = rtcm->print_MSM_1(gps_eph,
gal_eph,
obs_time,
pseudoranges,
ref_id,
clock_steering_indicator,
external_clock_indicator,
smooth_int,
sync_flag,
divergence_free,
more_messages);
std::string MSM1_bin = rtcm->hex_to_bin(MSM1);
unsigned int Nsat = 4;
unsigned int Nsig = 3;
unsigned int size_header = 14;
unsigned int size_msg_length = 10;
EXPECT_EQ(0, MSM1_bin.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("001010101100")); // check cell mask
std::map<int, Gnss_Synchro> pseudoranges2;
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(1, gnss_synchro5));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(2, gnss_synchro4));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(3, gnss_synchro3));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(4, gnss_synchro2));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(5, gnss_synchro));
std::string MSM1_2 = rtcm->print_MSM_1(gps_eph,
gal_eph,
obs_time,
pseudoranges2,
ref_id,
clock_steering_indicator,
external_clock_indicator,
smooth_int,
sync_flag,
divergence_free,
more_messages);
std::string MSM1_bin_2 = rtcm->hex_to_bin(MSM1_2);
EXPECT_EQ(0, MSM1_bin_2.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("001010101100")); // check cell mask
Gnss_Synchro gnss_synchro6;
gnss_synchro6.PRN = 10;
gnss_synchro6.System = *gps.c_str();
std::memcpy((void*)gnss_synchro6.Signal, s2.c_str(), 3);
gnss_synchro6.Pseudorange_m = 24000000.0;
std::map<int, Gnss_Synchro> pseudoranges3;
pseudoranges3.insert(std::pair<int, Gnss_Synchro>(1, gnss_synchro));
pseudoranges3.insert(std::pair<int, Gnss_Synchro>(2, gnss_synchro2));
pseudoranges3.insert(std::pair<int, Gnss_Synchro>(3, gnss_synchro6));
pseudoranges3.insert(std::pair<int, Gnss_Synchro>(4, gnss_synchro4));
pseudoranges3.insert(std::pair<int, Gnss_Synchro>(5, gnss_synchro5));
std::string MSM1_3 = rtcm->print_MSM_1(gps_eph,
gal_eph,
obs_time,
pseudoranges3,
ref_id,
clock_steering_indicator,
external_clock_indicator,
smooth_int,
sync_flag,
divergence_free,
more_messages);
std::string MSM1_bin_3 = rtcm->hex_to_bin(MSM1_3);
EXPECT_EQ(0, MSM1_bin_3.substr(size_header + size_msg_length + 169, (Nsat-1) * Nsig).compare("001010111")); // check cell mask
}
TEST(Rtcm_Test, MSM1)
{
auto rtcm = std::make_shared<Rtcm>();
@ -353,16 +482,37 @@ TEST(Rtcm_Test, MSM1)
unsigned int rough_range_1 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;
unsigned int rough_range_2 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro2.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;
unsigned int rough_range_4 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro3.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;
unsigned int read_pseudorange_1 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig , 10));
unsigned int read_pseudorange_2 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 10, 10));
unsigned int read_pseudorange_4 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 20, 10));
unsigned int read_pseudorange_1 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig , 10));
unsigned int read_pseudorange_2 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 10, 10));
unsigned int read_pseudorange_4 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 20, 10));
EXPECT_EQ(rough_range_1, read_pseudorange_1);
EXPECT_EQ(rough_range_2, read_pseudorange_2);
EXPECT_EQ(rough_range_4, read_pseudorange_4);
int psrng4_s = static_cast<int>(std::round( (gnss_synchro3.Pseudorange_m - std::round(gnss_synchro3.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10)/ meters_to_miliseconds / TWO_N24));
int read_psrng4_s = rtcm->bin_to_int( MSM1_bin.substr(size_header + size_msg_length + 169 + (Nsat * Nsig) + 30 + 15 * 3, 15));
int read_psrng4_s = rtcm->bin_to_int( MSM1_bin.substr(size_header + size_msg_length + 169 + (Nsat * Nsig) + 30 + 15 * 3, 15));
EXPECT_EQ(psrng4_s, read_psrng4_s);
std::map<int, Gnss_Synchro> pseudoranges2;
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(1, gnss_synchro4));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(2, gnss_synchro3));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(3, gnss_synchro2));
pseudoranges2.insert(std::pair<int, Gnss_Synchro>(4, gnss_synchro));
std::string MSM1_2 = rtcm->print_MSM_1(gps_eph,
gal_eph,
obs_time,
pseudoranges2,
ref_id,
clock_steering_indicator,
external_clock_indicator,
smooth_int,
sync_flag,
divergence_free,
more_messages);
std::string MSM1_bin2 = rtcm->hex_to_bin(MSM1_2);
int read_psrng4_s_2 = rtcm->bin_to_int( MSM1_bin2.substr(size_header + size_msg_length + 169 + (Nsat * Nsig) + 30 + 15 * 3, 15));
EXPECT_EQ(psrng4_s, read_psrng4_s_2);
}