mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-02-24 15:00:13 +00:00
Fix RINEX nav annotations
This commit is contained in:
Carles Fernandez
parent
950b4b607a
commit
76b3b932eb
@ -1104,317 +1104,316 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
|
||||
break;
|
||||
}
|
||||
}
|
||||
d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
|
||||
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>))
|
||||
{
|
||||
// ### GPS CNAV IONO ###
|
||||
std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono;
|
||||
gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
|
||||
DLOG(INFO) << "New CNAV IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>))
|
||||
{
|
||||
// ### GPS CNAV UTC MODEL ###
|
||||
std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model;
|
||||
gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
|
||||
DLOG(INFO) << "New CNAV UTC record has arrived ";
|
||||
}
|
||||
d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
|
||||
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>))
|
||||
{
|
||||
// ### GPS CNAV IONO ###
|
||||
std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono;
|
||||
gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
|
||||
DLOG(INFO) << "New CNAV IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>))
|
||||
{
|
||||
// ### GPS CNAV UTC MODEL ###
|
||||
std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model;
|
||||
gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
|
||||
DLOG(INFO) << "New CNAV UTC record has arrived ";
|
||||
}
|
||||
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Almanac>))
|
||||
{
|
||||
// ### GPS ALMANAC ###
|
||||
std::shared_ptr<Gps_Almanac> gps_almanac;
|
||||
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
|
||||
DLOG(INFO) << "New GPS almanac record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Almanac>))
|
||||
{
|
||||
// ### GPS ALMANAC ###
|
||||
std::shared_ptr<Gps_Almanac> gps_almanac;
|
||||
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
|
||||
DLOG(INFO) << "New GPS almanac record has arrived ";
|
||||
}
|
||||
|
||||
// **************** Galileo telemetry ********************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
|
||||
// **************** Galileo telemetry ********************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
|
||||
{
|
||||
// ### Galileo EPHEMERIS ###
|
||||
std::shared_ptr<Galileo_Ephemeris> galileo_eph;
|
||||
galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
|
||||
// insert new ephemeris record
|
||||
DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5
|
||||
<< ", GALILEO Week Number =" << galileo_eph->WN_5
|
||||
<< " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
{
|
||||
// ### Galileo EPHEMERIS ###
|
||||
std::shared_ptr<Galileo_Ephemeris> galileo_eph;
|
||||
galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
|
||||
// insert new ephemeris record
|
||||
DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5
|
||||
<< ", GALILEO Week Number =" << galileo_eph->WN_5
|
||||
<< " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->galileo_ephemeris_map.find(galileo_eph->i_satellite_PRN) == d_pvt_solver->galileo_ephemeris_map.cend())
|
||||
{
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->galileo_ephemeris_map.find(galileo_eph->i_satellite_PRN) == d_pvt_solver->galileo_ephemeris_map.cend())
|
||||
new_annotation = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN].t0e_1 != galileo_eph->t0e_1)
|
||||
{
|
||||
new_annotation = true;
|
||||
}
|
||||
else
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Galileo_Ephemeris> new_gal_eph;
|
||||
std::map<int32_t, Gps_CNAV_Ephemeris> new_cnav_eph;
|
||||
std::map<int32_t, Gps_Ephemeris> new_eph;
|
||||
std::map<int32_t, Glonass_Gnav_Ephemeris> new_glo_eph;
|
||||
new_gal_eph[galileo_eph->i_satellite_PRN] = *galileo_eph;
|
||||
switch (type_of_rx)
|
||||
{
|
||||
if (d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN].t0e_1 != galileo_eph->t0e_1)
|
||||
{
|
||||
new_annotation = true;
|
||||
}
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Galileo_Ephemeris> new_gal_eph;
|
||||
std::map<int32_t, Gps_CNAV_Ephemeris> new_cnav_eph;
|
||||
std::map<int32_t, Gps_Ephemeris> new_eph;
|
||||
std::map<int32_t, Glonass_Gnav_Ephemeris> new_glo_eph;
|
||||
new_gal_eph[galileo_eph->i_satellite_PRN] = *galileo_eph;
|
||||
switch (type_of_rx)
|
||||
{
|
||||
case 6: // Galileo E5b only
|
||||
rp->log_rinex_nav(rp->navGalFile, new_gal_eph);
|
||||
break;
|
||||
case 11: // GPS L1 C/A + Galileo E5b
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
case 13: // L5+E5a
|
||||
rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph);
|
||||
break;
|
||||
case 15: // Galileo E1B + Galileo E5b
|
||||
rp->log_rinex_nav(rp->navGalFile, new_gal_eph);
|
||||
break;
|
||||
case 27: // Galileo E1B + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 30: // Galileo E1B + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 32: // L1+E1+L5+E5a
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
case 33: // L1+E1+E5a
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
}
|
||||
case 6: // Galileo E5b only
|
||||
rp->log_rinex_nav(rp->navGalFile, new_gal_eph);
|
||||
break;
|
||||
case 11: // GPS L1 C/A + Galileo E5b
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
case 13: // L5+E5a
|
||||
rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph);
|
||||
break;
|
||||
case 15: // Galileo E1B + Galileo E5b
|
||||
rp->log_rinex_nav(rp->navGalFile, new_gal_eph);
|
||||
break;
|
||||
case 27: // Galileo E1B + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 30: // Galileo E1B + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 32: // L1+E1+L5+E5a
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
case 33: // L1+E1+E5a
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Iono>))
|
||||
{
|
||||
// ### Galileo IONO ###
|
||||
std::shared_ptr<Galileo_Iono> galileo_iono;
|
||||
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->galileo_iono = *galileo_iono;
|
||||
DLOG(INFO) << "New IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>))
|
||||
{
|
||||
// ### Galileo UTC MODEL ###
|
||||
std::shared_ptr<Galileo_Utc_Model> galileo_utc_model;
|
||||
galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->galileo_utc_model = *galileo_utc_model;
|
||||
DLOG(INFO) << "New UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>))
|
||||
{
|
||||
// ### Galileo Almanac ###
|
||||
std::shared_ptr<Galileo_Almanac_Helper> galileo_almanac_helper;
|
||||
galileo_almanac_helper = boost::any_cast<std::shared_ptr<Galileo_Almanac_Helper>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Iono>))
|
||||
{
|
||||
// ### Galileo IONO ###
|
||||
std::shared_ptr<Galileo_Iono> galileo_iono;
|
||||
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->galileo_iono = *galileo_iono;
|
||||
DLOG(INFO) << "New IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>))
|
||||
{
|
||||
// ### Galileo UTC MODEL ###
|
||||
std::shared_ptr<Galileo_Utc_Model> galileo_utc_model;
|
||||
galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->galileo_utc_model = *galileo_utc_model;
|
||||
DLOG(INFO) << "New UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>))
|
||||
{
|
||||
// ### Galileo Almanac ###
|
||||
std::shared_ptr<Galileo_Almanac_Helper> galileo_almanac_helper;
|
||||
galileo_almanac_helper = boost::any_cast<std::shared_ptr<Galileo_Almanac_Helper>>(pmt::any_ref(msg));
|
||||
|
||||
Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1);
|
||||
Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2);
|
||||
Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3);
|
||||
Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1);
|
||||
Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2);
|
||||
Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3);
|
||||
|
||||
if (sv1.i_satellite_PRN != 0)
|
||||
if (sv1.i_satellite_PRN != 0)
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
|
||||
}
|
||||
if (sv2.i_satellite_PRN != 0)
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
|
||||
}
|
||||
if (sv3.i_satellite_PRN != 0)
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
|
||||
}
|
||||
DLOG(INFO) << "New Galileo Almanac data have arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
|
||||
{
|
||||
// ### Galileo Almanac ###
|
||||
std::shared_ptr<Galileo_Almanac> galileo_alm;
|
||||
galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
|
||||
// update/insert new almanac record to the global almanac map
|
||||
d_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
|
||||
}
|
||||
|
||||
// **************** GLONASS GNAV Telemetry **************************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
|
||||
{
|
||||
// ### GLONASS GNAV EPHEMERIS ###
|
||||
std::shared_ptr<Glonass_Gnav_Ephemeris> glonass_gnav_eph;
|
||||
glonass_gnav_eph = boost::any_cast<std::shared_ptr<Glonass_Gnav_Ephemeris>>(pmt::any_ref(msg));
|
||||
// TODO Add GLONASS with gps week number and tow,
|
||||
// insert new ephemeris record
|
||||
DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW
|
||||
<< ", Week Number =" << glonass_gnav_eph->d_WN
|
||||
<< " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b)
|
||||
<< " from SV = " << glonass_gnav_eph->i_satellite_slot_number;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
{
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->glonass_gnav_ephemeris_map.find(glonass_gnav_eph->i_satellite_PRN) == d_pvt_solver->glonass_gnav_ephemeris_map.cend())
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
|
||||
new_annotation = true;
|
||||
}
|
||||
if (sv2.i_satellite_PRN != 0)
|
||||
else
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
|
||||
}
|
||||
if (sv3.i_satellite_PRN != 0)
|
||||
{
|
||||
d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
|
||||
}
|
||||
DLOG(INFO) << "New Galileo Almanac data have arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
|
||||
{
|
||||
// ### Galileo Almanac ###
|
||||
std::shared_ptr<Galileo_Almanac> galileo_alm;
|
||||
galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
|
||||
// update/insert new almanac record to the global almanac map
|
||||
d_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
|
||||
}
|
||||
|
||||
// **************** GLONASS GNAV Telemetry **************************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
|
||||
{
|
||||
// ### GLONASS GNAV EPHEMERIS ###
|
||||
std::shared_ptr<Glonass_Gnav_Ephemeris> glonass_gnav_eph;
|
||||
glonass_gnav_eph = boost::any_cast<std::shared_ptr<Glonass_Gnav_Ephemeris>>(pmt::any_ref(msg));
|
||||
// TODO Add GLONASS with gps week number and tow,
|
||||
// insert new ephemeris record
|
||||
DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW
|
||||
<< ", Week Number =" << glonass_gnav_eph->d_WN
|
||||
<< " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b)
|
||||
<< " from SV = " << glonass_gnav_eph->i_satellite_slot_number;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
{
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->glonass_gnav_ephemeris_map.find(glonass_gnav_eph->i_satellite_PRN) == d_pvt_solver->glonass_gnav_ephemeris_map.cend())
|
||||
if (d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN].d_t_b != glonass_gnav_eph->d_t_b)
|
||||
{
|
||||
new_annotation = true;
|
||||
}
|
||||
else
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Galileo_Ephemeris> new_gal_eph;
|
||||
std::map<int32_t, Gps_CNAV_Ephemeris> new_cnav_eph;
|
||||
std::map<int32_t, Gps_Ephemeris> new_eph;
|
||||
std::map<int32_t, Glonass_Gnav_Ephemeris> new_glo_eph;
|
||||
new_glo_eph[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
|
||||
switch (type_of_rx)
|
||||
{
|
||||
if (d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN].d_t_b != glonass_gnav_eph->d_t_b)
|
||||
case 25: // GLONASS L1 C/A + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navGloFile, new_glo_eph);
|
||||
break;
|
||||
case 26: // GPS L1 C/A + GLONASS L1 C/A
|
||||
if (d_rinex_version == 3)
|
||||
{
|
||||
new_annotation = true;
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph);
|
||||
}
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Galileo_Ephemeris> new_gal_eph;
|
||||
std::map<int32_t, Gps_CNAV_Ephemeris> new_cnav_eph;
|
||||
std::map<int32_t, Gps_Ephemeris> new_eph;
|
||||
std::map<int32_t, Glonass_Gnav_Ephemeris> new_glo_eph;
|
||||
new_glo_eph[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
|
||||
switch (type_of_rx)
|
||||
if (d_rinex_version == 2)
|
||||
{
|
||||
case 25: // GLONASS L1 C/A + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navGloFile, new_glo_eph);
|
||||
break;
|
||||
case 26: // GPS L1 C/A + GLONASS L1 C/A
|
||||
if (d_rinex_version == 3)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph);
|
||||
}
|
||||
if (d_rinex_version == 2)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navGloFile, new_glo_eph);
|
||||
}
|
||||
break;
|
||||
case 27: // Galileo E1B + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 28: // GPS L2C + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph);
|
||||
break;
|
||||
case 29: // GPS L1 C/A + GLONASS L2 C/A
|
||||
if (d_rinex_version == 3)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph);
|
||||
}
|
||||
if (d_rinex_version == 2)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navGloFile, new_glo_eph);
|
||||
}
|
||||
break;
|
||||
case 30: // Galileo E1B + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 31: // GPS L2C + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 27: // Galileo E1B + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 28: // GPS L2C + GLONASS L1 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph);
|
||||
break;
|
||||
case 29: // GPS L1 C/A + GLONASS L2 C/A
|
||||
if (d_rinex_version == 3)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph);
|
||||
}
|
||||
if (d_rinex_version == 2)
|
||||
{
|
||||
rp->log_rinex_nav(rp->navGloFile, new_glo_eph);
|
||||
}
|
||||
break;
|
||||
case 30: // Galileo E1B + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph);
|
||||
break;
|
||||
case 31: // GPS L2C + GLONASS L2 C/A
|
||||
rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Utc_Model>))
|
||||
{
|
||||
// ### GLONASS GNAV UTC MODEL ###
|
||||
std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model;
|
||||
glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
|
||||
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>))
|
||||
{
|
||||
// ### GLONASS GNAV Almanac ###
|
||||
std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac;
|
||||
glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
|
||||
DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "
|
||||
<< ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A;
|
||||
}
|
||||
d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Utc_Model>))
|
||||
{
|
||||
// ### GLONASS GNAV UTC MODEL ###
|
||||
std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model;
|
||||
glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
|
||||
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>))
|
||||
{
|
||||
// ### GLONASS GNAV Almanac ###
|
||||
std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac;
|
||||
glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
|
||||
DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "
|
||||
<< ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A;
|
||||
}
|
||||
|
||||
// ************* BeiDou telemetry *****************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Ephemeris>))
|
||||
// ************* BeiDou telemetry *****************
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Ephemeris>))
|
||||
{
|
||||
// ### Beidou EPHEMERIS ###
|
||||
std::shared_ptr<Beidou_Dnav_Ephemeris> bds_dnav_eph;
|
||||
bds_dnav_eph = boost::any_cast<std::shared_ptr<Beidou_Dnav_Ephemeris>>(pmt::any_ref(msg));
|
||||
DLOG(INFO) << "Ephemeris record has arrived from SAT ID "
|
||||
<< bds_dnav_eph->i_satellite_PRN << " (Block "
|
||||
<< bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")"
|
||||
<< "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week="
|
||||
<< bds_dnav_eph->i_BEIDOU_week;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
{
|
||||
// ### Beidou EPHEMERIS ###
|
||||
std::shared_ptr<Beidou_Dnav_Ephemeris> bds_dnav_eph;
|
||||
bds_dnav_eph = boost::any_cast<std::shared_ptr<Beidou_Dnav_Ephemeris>>(pmt::any_ref(msg));
|
||||
DLOG(INFO) << "Ephemeris record has arrived from SAT ID "
|
||||
<< bds_dnav_eph->i_satellite_PRN << " (Block "
|
||||
<< bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")"
|
||||
<< "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week="
|
||||
<< bds_dnav_eph->i_BEIDOU_week;
|
||||
// update/insert new ephemeris record to the global ephemeris map
|
||||
if (b_rinex_header_written) // The header is already written, we can now log the navigation message data
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->beidou_dnav_ephemeris_map.find(bds_dnav_eph->i_satellite_PRN) == d_pvt_solver->beidou_dnav_ephemeris_map.cend())
|
||||
{
|
||||
bool new_annotation = false;
|
||||
if (d_pvt_solver->beidou_dnav_ephemeris_map.find(bds_dnav_eph->i_satellite_PRN) == d_pvt_solver->beidou_dnav_ephemeris_map.cend())
|
||||
new_annotation = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN].d_Toc != bds_dnav_eph->d_Toc)
|
||||
{
|
||||
new_annotation = true;
|
||||
}
|
||||
else
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Beidou_Dnav_Ephemeris> new_bds_eph;
|
||||
new_bds_eph[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
|
||||
switch (type_of_rx)
|
||||
{
|
||||
if (d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN].d_Toc != bds_dnav_eph->d_Toc)
|
||||
{
|
||||
new_annotation = true;
|
||||
}
|
||||
}
|
||||
if (new_annotation == true)
|
||||
{
|
||||
// New record!
|
||||
std::map<int32_t, Beidou_Dnav_Ephemeris> new_bds_eph;
|
||||
new_bds_eph[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
|
||||
switch (type_of_rx)
|
||||
{
|
||||
case 50: // BDS B1I only
|
||||
rp->log_rinex_nav(rp->navFile, new_bds_eph);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
case 50: // BDS B1I only
|
||||
rp->log_rinex_nav(rp->navFile, new_bds_eph);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Iono>))
|
||||
{
|
||||
// ### BeiDou IONO ###
|
||||
std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono;
|
||||
bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
|
||||
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>))
|
||||
{
|
||||
// ### BeiDou UTC MODEL ###
|
||||
std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model;
|
||||
bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
|
||||
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>))
|
||||
{
|
||||
// ### BeiDou ALMANAC ###
|
||||
std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac;
|
||||
bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
|
||||
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived ";
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << "msg_handler_telemetry unknown object type!";
|
||||
}
|
||||
d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Iono>))
|
||||
{
|
||||
// ### BeiDou IONO ###
|
||||
std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono;
|
||||
bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
|
||||
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>))
|
||||
{
|
||||
// ### BeiDou UTC MODEL ###
|
||||
std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model;
|
||||
bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
|
||||
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
|
||||
}
|
||||
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>))
|
||||
{
|
||||
// ### BeiDou ALMANAC ###
|
||||
std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac;
|
||||
bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
|
||||
d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
|
||||
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived ";
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << "msg_handler_telemetry unknown object type!";
|
||||
}
|
||||
}
|
||||
catch (boost::bad_any_cast& e)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user