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 a9717e568..0b17a2b2a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -198,6 +198,16 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, bool dump, std::str } } } + + // Create Sys V message queue + first_fix = true; + sysv_msg_key = 1101; + int msgflg = IPC_CREAT | 0666; + if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) + { + std::cout << "GNSS-SDR can not create message queues!" << std::endl; + throw new std::exception(); + } } @@ -227,6 +237,21 @@ void galileo_e1_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchroniz } +bool galileo_e1_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) +{ + /* Fill Sys V message structures */ + int msgsend_size; + ttff_msgbuf msg; + msg.ttff = ttff.ttff; + msgsend_size = sizeof(msg.ttff); + msg.mtype = 1; /* default message ID */ + + /* SEND SOLUTION OVER A MESSAGE QUEUE */ + /* non-blocking Sys V message send */ + msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); + return true; +} + int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { @@ -267,6 +292,17 @@ int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)), if (pvt_result == true) { + if( first_fix == true) + { + std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + ttff_msgbuf ttff; + ttff.mtype = 1; + ttff.ttff = d_sample_counter; + send_sys_v_ttff_msg(ttff); + first_fix = false; + } d_kml_dump->print_position(d_ls_pvt, d_flag_averaging); d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h index bcb13137f..638417905 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h @@ -33,6 +33,9 @@ #include #include +#include +#include +#include #include #include #include "nmea_printer.h" @@ -134,6 +137,15 @@ private: std::shared_ptr d_ls_pvt; bool pseudoranges_pairCompare_min(const std::pair& a, const std::pair& b); + bool first_fix; + key_t sysv_msg_key; + int sysv_msqid; + typedef struct { + long mtype;//required by sys v message + double ttff; + } ttff_msgbuf; + bool send_sys_v_ttff_msg(ttff_msgbuf ttff); + public: ~galileo_e1_pvt_cc (); //!< Default destructor diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index af5f0b236..9f4aabe13 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -275,6 +275,16 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump } } } + + // Create Sys V message queue + first_fix = true; + sysv_msg_key = 1101; + int msgflg = IPC_CREAT | 0666; + if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) + { + std::cout << "GNSS-SDR can not create message queues!" << std::endl; + throw new std::exception(); + } } @@ -304,6 +314,22 @@ void hybrid_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronizatio } +bool hybrid_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) +{ + /* Fill Sys V message structures */ + int msgsend_size; + ttff_msgbuf msg; + msg.ttff = ttff.ttff; + msgsend_size = sizeof(msg.ttff); + msg.mtype = 1; /* default message ID */ + + /* SEND SOLUTION OVER A MESSAGE QUEUE */ + /* non-blocking Sys V message send */ + msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); + return true; +} + + int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h index 10e7b4224..21ccfeff4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h @@ -34,6 +34,9 @@ #include #include #include +#include +#include +#include #include #include "nmea_printer.h" #include "kml_printer.h" @@ -137,6 +140,15 @@ private: std::map gnss_pseudoranges_map; bool pseudoranges_pairCompare_min(const std::pair& a, const std::pair& b); + bool first_fix; + key_t sysv_msg_key; + int sysv_msqid; + typedef struct { + long mtype;//required by sys v message + double ttff; + } ttff_msgbuf; + bool send_sys_v_ttff_msg(ttff_msgbuf ttff); + public: /*! * \brief Get latest set of GPS L1 ephemeris from PVT block