diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc index da6c07af1..d88682c96 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -40,60 +40,69 @@ using google::LogMessage; -galileo_e1_pvt_cc_sptr -galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) +galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, + int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, + std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) { - return galileo_e1_pvt_cc_sptr(new galileo_e1_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_dump_devname)); + return galileo_e1_pvt_cc_sptr(new galileo_e1_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, + flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, + flag_rtcm_server, flag_rtcm_tty_port, rtcm_dump_devname)); } void galileo_e1_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) { try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo EPHEMERIS ### - std::shared_ptr galileo_eph; - galileo_eph= boost::any_cast>(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 - d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN]=*galileo_eph; - }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo IONO ### - std::shared_ptr galileo_iono; - galileo_iono= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->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 ### - std::shared_ptr galileo_utc_model; - galileo_utc_model= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->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 ### - std::shared_ptr galileo_almanac; - galileo_almanac= boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_almanac=*galileo_almanac; - DLOG(INFO) << "New Galileo Almanac has arrived "; - } + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo EPHEMERIS ### + std::shared_ptr galileo_eph; + galileo_eph = boost::any_cast>(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 + d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo IONO ### + std::shared_ptr galileo_iono; + galileo_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->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 ### + std::shared_ptr galileo_utc_model; + galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->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 ### + std::shared_ptr galileo_almanac; + galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_almanac = *galileo_almanac; + DLOG(INFO) << "New Galileo Almanac has arrived "; + } - } - catch(boost::bad_any_cast& e) - { - DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; - } + } + catch(boost::bad_any_cast& e) + { + DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; + } } -galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) : - gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(0, 0, sizeof(gr_complex))) + + +galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, + bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, + bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) : + gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, sizeof(gr_complex))) { d_output_rate_ms = output_rate_ms; @@ -146,7 +155,7 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr(); - d_last_status_print_seg=0; + d_last_status_print_seg = 0; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 07f45ba3f..ed451ca19 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -52,91 +52,95 @@ gps_l1_ca_make_pvt_cc(unsigned int nchannels, boost::shared_ptr q void gps_l1_ca_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) { try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS EPHEMERIS ### - std::shared_ptr gps_eph; - gps_eph= boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph->i_satellite_PRN << " (Block " - << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" - << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" - << gps_eph->i_GPS_week; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN]=*gps_eph; - }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS IONO ### - std::shared_ptr gps_iono; - gps_iono= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_iono=*gps_iono; - DLOG(INFO) << "New IONO record has arrived "; - }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS UTC MODEL ### - std::shared_ptr gps_utc_model; - gps_utc_model= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_utc_model=*gps_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### SBAS IONO ### - std::shared_ptr sbas_iono; - sbas_iono= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->sbas_iono=*sbas_iono; - DLOG(INFO) << "New SBAS IONO record has arrived "; - } + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS IONO ### + std::shared_ptr gps_iono; + gps_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_iono = *gps_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_utc_model = *gps_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### SBAS IONO ### + std::shared_ptr sbas_iono; + sbas_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->sbas_iono = *sbas_iono; + DLOG(INFO) << "New SBAS IONO record has arrived "; + } - //TODO: add SBAS correction maps here - //d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy(); - //d_ls_pvt->sbas_ephemeris_map = global_sbas_ephemeris_map.get_map_copy(); + //TODO: add SBAS correction maps here + //d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy(); + //d_ls_pvt->sbas_ephemeris_map = global_sbas_ephemeris_map.get_map_copy(); - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - std::shared_ptr sbas_raw_msg_ptr; - sbas_raw_msg_ptr= boost::any_cast>(pmt::any_ref(msg)); - Sbas_Raw_Msg sbas_raw_msg=*sbas_raw_msg_ptr; - // read SBAS raw messages directly from queue and write them into rinex file - // create the header of not yet done - if(!b_rinex_sbs_header_writen) - { - rp->rinex_sbs_header(rp->sbsFile); - b_rinex_sbs_header_writen = true; - } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + std::shared_ptr sbas_raw_msg_ptr; + sbas_raw_msg_ptr = boost::any_cast>(pmt::any_ref(msg)); + Sbas_Raw_Msg sbas_raw_msg = *sbas_raw_msg_ptr; + // read SBAS raw messages directly from queue and write them into rinex file + // create the header of not yet done + if(!b_rinex_sbs_header_writen) + { + rp->rinex_sbs_header(rp->sbsFile); + b_rinex_sbs_header_writen = true; + } - // Define the RX time of the SBAS message by using the GPS time. - // It has only an effect if there has not been yet a SBAS MT12 available - // when the message was received. - if(sbas_raw_msg.get_rx_time_obj().is_related() == false - && gnss_pseudoranges_map.size() > 0 - && d_ls_pvt->gps_ephemeris_map.size() > 0) - { - // doesn't matter which channel/satellite we choose - Gnss_Synchro gs = gnss_pseudoranges_map.begin()->second; - Gps_Ephemeris eph = d_ls_pvt->gps_ephemeris_map.begin()->second; + // Define the RX time of the SBAS message by using the GPS time. + // It has only an effect if there has not been yet a SBAS MT12 available + // when the message was received. + if(sbas_raw_msg.get_rx_time_obj().is_related() == false + && gnss_pseudoranges_map.size() > 0 + && d_ls_pvt->gps_ephemeris_map.size() > 0) + { + // doesn't matter which channel/satellite we choose + Gnss_Synchro gs = gnss_pseudoranges_map.begin()->second; + Gps_Ephemeris eph = d_ls_pvt->gps_ephemeris_map.begin()->second; - double relative_rx_time = gs.Tracking_timestamp_secs; - int gps_week = eph.i_GPS_week; - double gps_sec = gs.d_TOW_at_current_symbol; + double relative_rx_time = gs.Tracking_timestamp_secs; + int gps_week = eph.i_GPS_week; + double gps_sec = gs.d_TOW_at_current_symbol; - Sbas_Time_Relation time_rel(relative_rx_time, gps_week, gps_sec); - sbas_raw_msg.relate(time_rel); - } + Sbas_Time_Relation time_rel(relative_rx_time, gps_week, gps_sec); + sbas_raw_msg.relate(time_rel); + } - // send the message to the rinex logger if it has a valid GPS time stamp - if(sbas_raw_msg.get_rx_time_obj().is_related()) - { - rp->log_rinex_sbs(rp->sbsFile, sbas_raw_msg); - } - } + // send the message to the rinex logger if it has a valid GPS time stamp + if(sbas_raw_msg.get_rx_time_obj().is_related()) + { + rp->log_rinex_sbs(rp->sbsFile, sbas_raw_msg); + } + } } catch(boost::bad_any_cast& e) { - DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; + DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; } } + gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, @@ -237,14 +241,14 @@ bool pseudoranges_pairCompare_min(const std::pair& a, const st void gps_l1_ca_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data) { // Print the current receiver status using std::cout every second - int current_rx_seg=floor(channels_synchronization_data[0][0].Tracking_timestamp_secs); + int current_rx_seg = floor(channels_synchronization_data[0][0].Tracking_timestamp_secs); if ( current_rx_seg!= d_last_status_print_seg) - { - d_last_status_print_seg = current_rx_seg; - std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl<< std::flush; - //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; - } + { + d_last_status_print_seg = current_rx_seg; + std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl<< std::flush; + //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + } } int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 355a31fb0..4a38fd079 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -51,84 +51,87 @@ hybrid_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queu void hybrid_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) { try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS EPHEMERIS ### - std::shared_ptr gps_eph; - gps_eph= boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph->i_satellite_PRN << " (Block " - << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" - << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" - << gps_eph->i_GPS_week; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN]=*gps_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS IONO ### - std::shared_ptr gps_iono; - gps_iono= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_iono=*gps_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS UTC MODEL ### - std::shared_ptr gps_utc_model; - gps_utc_model= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_utc_model=*gps_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo EPHEMERIS ### - std::shared_ptr galileo_eph; - galileo_eph= boost::any_cast>(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 - d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN]=*galileo_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo IONO ### - std::shared_ptr galileo_iono; - galileo_iono= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->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 ### - std::shared_ptr galileo_utc_model; - galileo_utc_model= boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->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 ### - std::shared_ptr galileo_almanac; - galileo_almanac= boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_almanac=*galileo_almanac; - DLOG(INFO) << "New Galileo Almanac has arrived "; - } + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS IONO ### + std::shared_ptr gps_iono; + gps_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_iono = *gps_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_utc_model = *gps_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo EPHEMERIS ### + std::shared_ptr galileo_eph; + galileo_eph = boost::any_cast>(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 + d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo IONO ### + std::shared_ptr galileo_iono; + galileo_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->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 ### + std::shared_ptr galileo_utc_model; + galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->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 ### + std::shared_ptr galileo_almanac; + galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_almanac = *galileo_almanac; + DLOG(INFO) << "New Galileo Almanac has arrived "; + } } catch(boost::bad_any_cast& e) { - DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; + DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; } } -hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) : - gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(0, 0, sizeof(gr_complex))) +hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, + int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, + std::string nmea_dump_filename, std::string nmea_dump_devname, + bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) : + gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(0, 0, sizeof(gr_complex))) { d_output_rate_ms = output_rate_ms; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index a3fe43b15..abf444d43 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -250,28 +250,28 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols,int if (d_nav.have_new_ephemeris() == true) { // get object for this SV (mandatory) - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_nav.get_ephemeris();//notice that the read operation will clear the valid flag + std::shared_ptr tmp_obj= std::make_shared(d_nav.get_ephemeris()); + //*tmp_obj = d_nav.get_ephemeris();//notice that the read operation will clear the valid flag this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_nav.have_new_iono_and_GST() == true) { // get object for this SV (mandatory) - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_nav.get_iono(); //notice that the read operation will clear the valid flag + std::shared_ptr tmp_obj= std::make_shared(d_nav.get_iono()); + //*tmp_obj = d_nav.get_iono(); //notice that the read operation will clear the valid flag this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_nav.have_new_utc_model() == true) { // get object for this SV (mandatory) - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_nav.get_utc_model(); //notice that the read operation will clear the valid flag + std::shared_ptr tmp_obj= std::make_shared(d_nav.get_utc_model()); + //*tmp_obj = d_nav.get_utc_model(); //notice that the read operation will clear the valid flag this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_nav.have_new_almanac() == true) { - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_nav.get_almanac(); + std::shared_ptr tmp_obj= std::make_shared(d_nav.get_almanac()); + //*tmp_obj = d_nav.get_almanac(); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); //debug std::cout << "Galileo almanac received!" << std::endl; @@ -407,10 +407,10 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut //2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) //update TOW at the preamble instant - // JAVI: 30/06/2014 - // TOW, in Galileo, is referred to the START of the PAGE PART, that is, THE FIRST SYMBOL OF THAT PAGE, NOT THE PREAMBLE. - // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_page_start. - // Since we detected the preamble, then, we are in the last symbol of that preamble, or just at the start of the first page symbol. + // JAVI: 30/06/2014 + // TOW, in Galileo, is referred to the START of the PAGE PART, that is, THE FIRST SYMBOL OF THAT PAGE, NOT THE PREAMBLE. + // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_page_start. + // Since we detected the preamble, then, we are in the last symbol of that preamble, or just at the start of the first page symbol. //flag preamble is true after the all page (even and odd) is received. I/NAV page period is 2 SECONDS { Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 28caea364..2b423a9fe 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -216,7 +216,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ } else { - if (preamble_diff_ms > GPS_SUBFRAME_MS+1) + if (preamble_diff_ms > GPS_SUBFRAME_MS + 1) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff_ms= " << preamble_diff_ms; d_stat = 0; // lost of frame sync @@ -273,42 +273,42 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0; d_GPS_FSM.Event_gps_word_valid(); // send TLM data to PVT using asynchronous message queues - if (d_GPS_FSM.d_flag_new_subframe==true) - { - switch (d_GPS_FSM.d_subframe_ID) + if (d_GPS_FSM.d_flag_new_subframe == true) { - case 3: //we have a new set of ephemeris data for the current SV - if (d_GPS_FSM.d_nav.satellite_validation() == true) - { - // get ephemeris object for this SV (mandatory) - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_GPS_FSM.d_nav.get_ephemeris(); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - break; - case 4: // Possible IONOSPHERE and UTC model update (page 18) - if (d_GPS_FSM.d_nav.flag_iono_valid == true) - { - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_GPS_FSM.d_nav.get_iono(); //notice that the read operation will clear the valid flag - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - if (d_GPS_FSM.d_nav.flag_utc_model_valid == true) - { - std::shared_ptr tmp_obj= std::make_shared(); - *tmp_obj = d_GPS_FSM.d_nav.get_utc_model(); //notice that the read operation will clear the valid flag - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - break; - case 5: - // get almanac (if available) - //TODO: implement almanac reader in navigation_message - break; - default: - break; + switch (d_GPS_FSM.d_subframe_ID) + { + case 3: //we have a new set of ephemeris data for the current SV + if (d_GPS_FSM.d_nav.satellite_validation() == true) + { + // get ephemeris object for this SV (mandatory) + std::shared_ptr tmp_obj = std::make_shared(d_GPS_FSM.d_nav.get_ephemeris()); + // *tmp_obj = d_GPS_FSM.d_nav.get_ephemeris(); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 4: // Possible IONOSPHERE and UTC model update (page 18) + if (d_GPS_FSM.d_nav.flag_iono_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared( d_GPS_FSM.d_nav.get_iono()); + // *tmp_obj = d_GPS_FSM.d_nav.get_iono(); //notice that the read operation will clear the valid flag + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + if (d_GPS_FSM.d_nav.flag_utc_model_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_GPS_FSM.d_nav.get_utc_model()); + //*tmp_obj = d_GPS_FSM.d_nav.get_utc_model(); //notice that the read operation will clear the valid flag + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 5: + // get almanac (if available) + //TODO: implement almanac reader in navigation_message + break; + default: + break; + } + d_GPS_FSM.clear_flag_new_subframe(); } - d_GPS_FSM.clear_flag_new_subframe(); - } d_flag_parity = true; }