From 99989d472cf572411679c6f9fdddda36713f3074 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 5 Nov 2018 15:39:56 +0100 Subject: [PATCH] Fix NMEA GPGSV message for GPS L1 --- src/algorithms/PVT/libs/nmea_printer.cc | 88 ++---------------------- src/algorithms/PVT/libs/rtklib_solver.cc | 50 +++++++++++++- src/algorithms/PVT/libs/rtklib_solver.h | 1 + 3 files changed, 57 insertions(+), 82 deletions(-) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 1337526b9..26b18e3d0 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -34,6 +34,7 @@ */ #include "nmea_printer.h" +#include "rtklib_solution.h" #include #include // for create_directories, exists #include // for path, operator<< @@ -560,88 +561,13 @@ std::string Nmea_Printer::get_GPGSA() std::string Nmea_Printer::get_GPGSV() { // GSV-GNSS Satellites in View - // Notice that NMEA 2.1 only supports 12 channels - int n_sats_used = d_PVT_data->get_num_valid_observations(); - std::stringstream sentence_str; - std::stringstream frame_str; - std::string sentence_header; - sentence_header = "$GPGSV,"; - char checksum; - std::string tmpstr; - - // 1st step: How many GPGSV frames we need? (up to 3) - // Each frame contains up to 4 satellites - int n_frames; - n_frames = std::ceil((static_cast(n_sats_used)) / 4.0); - - // generate the frames - int current_satellite = 0; - for (int i = 1; i < (n_frames + 1); i++) - { - frame_str.str(""); - frame_str << sentence_header; - - // number of messages - frame_str << n_frames; - - // message number - frame_str << ","; - frame_str << i; - - // total number of satellites in view - frame_str << ","; - frame_str.width(2); - frame_str.fill('0'); - frame_str << std::dec << n_sats_used; - - // satellites info - for (int j = 0; j < 4; j++) - { - // write satellite info - frame_str << ","; - frame_str.width(2); - frame_str.fill('0'); - frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite); - - frame_str << ","; - frame_str.width(2); - frame_str.fill('0'); - frame_str << std::dec << static_cast(d_PVT_data->get_visible_satellites_El(current_satellite)); - - frame_str << ","; - frame_str.width(3); - frame_str.fill('0'); - frame_str << std::dec << static_cast(d_PVT_data->get_visible_satellites_Az(current_satellite)); - - frame_str << ","; - frame_str.width(2); - frame_str.fill('0'); - frame_str << std::dec << static_cast(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite)); - - current_satellite++; - - if (current_satellite == n_sats_used) - { - break; - } - } - - // frame checksum - tmpstr = frame_str.str(); - checksum = checkSum(tmpstr.substr(1)); - frame_str << "*"; - frame_str.width(2); - frame_str.fill('0'); - frame_str << std::hex << static_cast(checksum); - - // end NMEA sentence - frame_str << "\r\n"; - - //add frame to sentence - sentence_str << frame_str.str(); - } - return sentence_str.str(); // $GPGSV,2,1,07,07,79,048,42,02,51,062,43,26,36,256,42,27,27,138,42*71 + // Notice that NMEA 2.1 only supports 12 channels + std::stringstream sentence_str; + unsigned char buff[200]; + outnmea_gsv(buff, &d_PVT_data->pvt_sol, *d_PVT_data->pvt_ssat); + sentence_str << buff; + return sentence_str.str(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 12d316196..1ef53bc58 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -53,6 +53,7 @@ #include "rtklib_solver.h" #include "rtklib_conversions.h" +#include "rtklib_solution.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" #include "GLONASS_L1_L2_CA.h" @@ -809,8 +810,55 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ index_aux++; } } - if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_); + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + { + switch (gnss_observables_iter->second.System) + { + case 'E': + break; + + case 'G': + { + // GPS L1 + std::string sig_(gnss_observables_iter->second.Signal); + if (sig_.compare("1C") == 0) + { + unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); + rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; //newobs.SNR[0]; + pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; + } + // GPS L2 + if (sig_.compare("2S") == 0) + { + } + // GPS L5 + if (sig_.compare("L5") == 0) + { + } + break; + } + case 'R': + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if (sig_.compare("1G") == 0) + { + } + // GLONASS GNAV L2 + if (sig_.compare("2G") == 0) + { + } + break; + } + default: + break; + } + } + + if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_); this->set_valid_position(true); arma::vec rx_position_and_time(4); rx_position_and_time(0) = pvt_sol.rr[0]; // [m] diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index b584c4826..1e9684685 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -86,6 +86,7 @@ private: public: sol_t pvt_sol; + ssat_t* pvt_ssat[MAXSAT]; rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t& rtk); ~rtklib_solver();