Fix NMEA printer

src/algorithms/PVT/libs/nmea_printer.cc

@@ -34,6 +34,7 @@
  */
 
 #include "nmea_printer.h"
+#include "rtklib_solution.h"
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <boost/filesystem/operations.hpp>   // for create_directories, exists
 #include <boost/filesystem/path.hpp>         // for path, operator<<
@@ -376,99 +377,10 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
 std::string Nmea_Printer::get_GPRMC()
 {
     // Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
-    bool valid_fix = d_PVT_data->is_valid_position();
-
-    // ToDo: Compute speed and course over ground
-    double speed_over_ground_knots = 0;
-    double course_over_ground_deg = 0;
-
-    // boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
-
     std::stringstream sentence_str;
-
+    unsigned char buff[1024] = {0};
-    // GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
+    outnmea_rmc(buff, &d_PVT_data->pvt_sol);
-    std::string sentence_header;
+    sentence_str << buff;
-    sentence_header = "$GPRMC,";
-    sentence_str << sentence_header;
-
-    // UTC Time: hhmmss.sss
-    sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
-
-    // Status: A: data valid, V: data NOT valid
-    if (valid_fix == true)
-        {
-            sentence_str << ",A";
-        }
-    else
-        {
-            sentence_str << ",V";
-        };
-
-    if (print_avg_pos == true)
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
-        }
-    else
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
-        }
-
-    // Speed over ground (knots)
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.precision(2);
-    sentence_str << speed_over_ground_knots;
-
-    // course over ground (degrees)
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.precision(2);
-    sentence_str << course_over_ground_deg;
-
-    // Date ddmmyy
-    boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
-    unsigned int year = sentence_date.year();
-    unsigned int day = sentence_date.day();
-    unsigned int month = sentence_date.month();
-
-    sentence_str << ",";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << day;
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << month;
-
-    std::stringstream year_strs;
-    year_strs << std::dec << year;
-    sentence_str << std::dec << year_strs.str().substr(2);
-
-    // Magnetic Variation (degrees)
-    // ToDo: Implement magnetic compass
-    sentence_str << ",";
-
-    // Magnetic Variation (E or W)
-    // ToDo: Implement magnetic compass
-    sentence_str << ",";
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
 }
 
@@ -477,82 +389,10 @@ std::string Nmea_Printer::get_GPGSA()
 {
     // $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
     // GSA-GNSS DOP and Active Satellites
-    bool valid_fix = d_PVT_data->is_valid_position();
-    int n_sats_used = d_PVT_data->get_num_valid_observations();
-    double pdop = d_PVT_data->get_pdop();
-    double hdop = d_PVT_data->get_hdop();
-    double vdop = d_PVT_data->get_vdop();
-
     std::stringstream sentence_str;
-    std::string sentence_header;
+    unsigned char buff[1024] = {0};
-    sentence_header = "$GPGSA,";
+    outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
-    sentence_str << sentence_header;
+    sentence_str << buff;
-
-    // mode1:
-    // (M) Manual-forced to operate in 2D or 3D mode
-    // (A) Automatic-allowed to automatically switch 2D/3D
-    std::string mode1 = "M";
-    sentence_str << mode1;
-
-    // mode2:
-    // 1 fix not available
-    // 2 fix 2D
-    // 3 fix 3D
-    if (valid_fix == true)
-        {
-            sentence_str << ",3";
-        }
-    else
-        {
-            sentence_str << ",1";
-        };
-
-    // Used satellites
-    for (int i = 0; i < 12; i++)
-        {
-            sentence_str << ",";
-            if (i < n_sats_used)
-                {
-                    sentence_str.width(2);
-                    sentence_str.fill('0');
-                    sentence_str << d_PVT_data->get_visible_satellites_ID(i);
-                }
-        }
-
-    // PDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << pdop;
-    // HDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << hdop;
-    // VDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << vdop;
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
 }
 
@@ -560,199 +400,22 @@ 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<double>(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<int>(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<int>(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<int>(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<int>(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[1024] = {0};
+    outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
+    sentence_str << buff;
+    return sentence_str.str();
 }
 
 
 std::string Nmea_Printer::get_GPGGA()
 {
-    // boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
-    bool valid_fix = d_PVT_data->is_valid_position();
-    int n_channels = d_PVT_data->get_num_valid_observations();  //d_nchannels
-    double hdop = d_PVT_data->get_hdop();
-    double MSL_altitude;
-
-    if (d_PVT_data->is_averaging() == true)
-        {
-            MSL_altitude = d_PVT_data->get_avg_height();
-        }
-    else
-        {
-            MSL_altitude = d_PVT_data->get_height();
-        }
-
     std::stringstream sentence_str;
-
+    unsigned char buff[1024] = {0};
-    // GPGGA (Global Positioning System Fixed Data)
+    outnmea_gga(buff, &d_PVT_data->pvt_sol);
-    std::string sentence_header;
+    sentence_str << buff;
-    sentence_header = "$GPGGA,";
-    sentence_str << sentence_header;
-
-    // UTC Time: hhmmss.sss
-    sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
-
-    if (d_PVT_data->is_averaging() == true)
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
-        }
-    else
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
-        }
-
-    // Position fix indicator
-    // 0 - Fix not available or invalid
-    // 1 - GPS SPS Mode, fix valid
-    // 2 - Differential GPS, SPS Mode, fix valid
-    // 3-5 - Not supported
-    // 6 - Dead Reckoning Mode, fix valid
-    // ToDo: Update PVT module to identify the fix mode
-
-    if (valid_fix == true)
-        {
-            sentence_str << ",1";
-        }
-    else
-        {
-            sentence_str << ",0";
-        }
-
-    // Number of satellites used in PVT
-    sentence_str << ",";
-    if (n_channels < 10)
-        {
-            sentence_str << '0' << n_channels;
-        }
-    else
-        {
-            sentence_str << n_channels;
-        }
-
-    // HDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << hdop;
-
-    // MSL Altitude
-    sentence_str << ",";
-    sentence_str.precision(1);
-    sentence_str << MSL_altitude;
-    sentence_str << ",M";
-
-    // Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
-    // ToDo: Compute this value
-    sentence_str << ",";
-    sentence_str << "0.0";
-    sentence_str << ",M";
-
-    // Age of Diff. Corr.  (Seconds) Null fields when DGPS is not used
-    // Diff. Ref. Station ID (0000)
-    // ToDo: Implement this fields for Differential GPS
-    sentence_str << ",";
-    sentence_str << "0.0,0000";
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
     // $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
 }

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"
@@ -74,7 +75,11 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
     rtk_ = rtk;
     for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0;
     pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0};
-
+    ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}};
+    for (unsigned int i = 0; i < MAXSAT; i++)
+        {
+            pvt_ssat[i] = ssat0;
+        }
     // ############# ENABLE DATA FILE LOG #################
     if (d_flag_dump_enabled == true)
         {
@@ -794,7 +799,11 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                     unsigned int used_sats = 0;
                     for (unsigned int i = 0; i < MAXSAT; i++)
                         {
-                            if (rtk_.ssat[i].vs == 1) used_sats++;
+                            if (rtk_.ssat[i].vs == 1)
+                                {
+                                    pvt_ssat[i] = rtk_.ssat[i];
+                                    used_sats++;
+                                }
                         }
 
                     std::vector<double> azel;
@@ -809,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     index_aux++;
                                 }
                         }
-                    if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
 
+                    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]

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();