diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 2ffe95d24..23fd9605d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1658,13 +1658,10 @@ void rtklib_pvt_gs::msg_handler_osnma(const pmt::pmt_t& msg) // so with ADKD0 and ADKD12 validated), their corresponding TOW at the beginning // of the authenticated subframe, and maybe the COP. const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); - if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) + if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) { - // Act according to NMA data - if (d_osnma_strict) - { - // TODO - } + const auto osnma_data = wht::any_cast>(pmt::any_ref(msg)); + d_auth_nav_data_map[osnma_data->get_prn_d()].insert(osnma_data->get_IOD_nav()); } } catch (const wht::bad_any_cast& e) @@ -2051,7 +2048,14 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_osnma_strict && ((std::string(in[i][epoch].Signal, 2) == std::string("1B")) || ((std::string(in[i][epoch].Signal, 2) == std::string("7X"))))) { // Pick up only authenticated satellites - // TODO + auto IOD_nav_list = d_auth_nav_data_map.find(tmp_eph_iter_gal->second.PRN); + if (IOD_nav_list != d_auth_nav_data_map.cend()) + { + if (IOD_nav_list->second.find(tmp_eph_iter_gal->second.IOD_nav) != IOD_nav_list->second.cend()) + { + store_valid_observable = true; + } + } } else { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 4acc14728..d6f61f1c6 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -20,6 +20,7 @@ #include "gnss_block_interface.h" #include "gnss_synchro.h" #include "gnss_time.h" +#include "osnma_data.h" #include "rtklib.h" #include #include @@ -203,6 +204,7 @@ private: std::map d_gnss_observables_map; std::map d_gnss_observables_map_t0; std::map d_gnss_observables_map_t1; + std::map> d_auth_nav_data_map; std::queue d_TimeChannelTagTimestamps;