gnss_messages_novatel.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers

#include <mrpt/obs/gnss_messages_novatel.h>
#include <map>

using namespace std;
using namespace mrpt::obs::gnss;

nv_oem6_header_t::nv_oem6_header_t()
        : hdr_len(0),
          msg_id(0),
          msg_type(0),
          port_addr(0),
          msg_len(0),
          seq_number(0),
          idle_percent(0),
          time_status(0),
          week(0),
          ms_in_week(0),
          receiver_status(0),
          reserved(0),
          receiver_sw_version(0)
{
        synch[0] = synch[1] = synch[2] = 0;
}

nv_oem6_short_header_t::nv_oem6_short_header_t()
        : msg_len(), msg_id(), week(), ms_in_week()
{
        synch[0] = synch[1] = synch[2] = 0;
}

// ------------
void Message_NV_OEM6_GENERIC_FRAME::dumpToStream(
        mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 GENERIC FRAME]\n");
        out.printf(" Message ID: %u\n", (unsigned)this->header.msg_id);
}
void Message_NV_OEM6_GENERIC_FRAME::internal_writeToStream(
        mrpt::utils::CStream& out) const
{
        out.WriteBuffer(&header, sizeof(header));
        out << static_cast<uint32_t>(msg_body.size());
        if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
}
void Message_NV_OEM6_GENERIC_FRAME::internal_readFromStream(
        mrpt::utils::CStream& in)
{
        in.ReadBuffer(&header, sizeof(header));
        uint32_t nBytesInStream;
        in >> nBytesInStream;
        msg_body.resize(nBytesInStream);
        if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
}
// ------------
void Message_NV_OEM6_GENERIC_SHORT_FRAME::dumpToStream(
        mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 GENERIC SHORT FRAME]\n");
        out.printf(" Message ID: %u\n", (unsigned)this->header.msg_id);
}
void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_writeToStream(
        mrpt::utils::CStream& out) const
{
        out.WriteBuffer(&header, sizeof(header));
        out << static_cast<uint32_t>(msg_body.size());
        if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
}
void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_readFromStream(
        mrpt::utils::CStream& in)
{
        in.ReadBuffer(&header, sizeof(header));
        uint32_t nBytesInStream;
        in >> nBytesInStream;
        msg_body.resize(nBytesInStream);
        if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
}

// ------------
const std::string& nv_oem6_solution_status::enum2str(int val)
{
        static bool init_map = false;
        static std::map<int, std::string> val2str;
        if (!init_map)
        {
                init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
                DEF_TYPESTR(SOL_COMPUTED)
                DEF_TYPESTR(INSUFFICIENT_OBS)
                DEF_TYPESTR(NO_CONVERGENCE)
                DEF_TYPESTR(SINGULARITY)
                DEF_TYPESTR(COV_TRACE)
                DEF_TYPESTR(TEST_DIST)
                DEF_TYPESTR(COLD_START)
                DEF_TYPESTR(V_H_LIMIT)
                DEF_TYPESTR(VARIANCE)
                DEF_TYPESTR(RESIDUALS)
                DEF_TYPESTR(DELTA_POS)
                DEF_TYPESTR(NEGATIVE_VAR)
                DEF_TYPESTR(INTEGRITY_WARNING)
                DEF_TYPESTR(INS_INACTIVE)
                DEF_TYPESTR(INS_ALIGNING)
                DEF_TYPESTR(INS_BAD)
                DEF_TYPESTR(IMU_UNPLUGGED)
                DEF_TYPESTR(PENDING)
                DEF_TYPESTR(INVALID_FIX)
#undef DEF_TYPESTR
        }
        std::map<int, std::string>::const_iterator it = val2str.find(val);
        static const std::string nullstr("???");
        return (it == val2str.end()) ? nullstr : it->second;
}

const std::string& nv_oem6_position_type::enum2str(int val)
{
        static bool init_map = false;
        static std::map<int, std::string> val2str;
        if (!init_map)
        {
                init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
                DEF_TYPESTR(NONE)
                DEF_TYPESTR(FIXEDPOS)
                DEF_TYPESTR(FIXEDHEIGHT)
                DEF_TYPESTR(Reserved)
                DEF_TYPESTR(FLOATCONV)
                DEF_TYPESTR(WIDELANE)
                DEF_TYPESTR(NARROWLANE)
                DEF_TYPESTR(DOPPLER_VELOCITY)
                DEF_TYPESTR(SINGLE)
                DEF_TYPESTR(PSRDIFF)
                DEF_TYPESTR(WAAS)
                DEF_TYPESTR(PROPAGATED)
                DEF_TYPESTR(OMNISTAR)
                DEF_TYPESTR(L1_FLOAT)
                DEF_TYPESTR(IONOFREE_FLOAT)
                DEF_TYPESTR(NARROW_FLOAT)
                DEF_TYPESTR(L1_INT)
                DEF_TYPESTR(WIDE_INT)
                DEF_TYPESTR(NARROW_INT)
                DEF_TYPESTR(RTK_DIRECT_INS)
                DEF_TYPESTR(INS)
                DEF_TYPESTR(INS_PSRSP)
                DEF_TYPESTR(INS_PSRDIFF)
                DEF_TYPESTR(INS_RTKFLOAT)
                DEF_TYPESTR(INS_RTKFIXED)
                DEF_TYPESTR(OMNISTAR_HP)
                DEF_TYPESTR(OMNISTAR_XP)
                DEF_TYPESTR(CDGPS)
#undef DEF_TYPESTR
        }
        std::map<int, std::string>::const_iterator it = val2str.find(val);
        static const std::string nullstr("???");
        return (it == val2str.end()) ? nullstr : it->second;
}

const std::string& nv_oem6_ins_status_type::enum2str(int val)
{
        static bool init_map = false;
        static std::map<int, std::string> val2str;
        if (!init_map)
        {
                init_map = true;
#define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
                DEF_TYPESTR(INS_INACTIVE)
                DEF_TYPESTR(INS_ALIGNING)
                DEF_TYPESTR(INS_HIGH_VARIANCE)
                DEF_TYPESTR(INS_SOLUTION_GOOD)
                DEF_TYPESTR(INS_SOLUTION_FREE)
                DEF_TYPESTR(INS_ALIGNMENT_COMPLETE)
                DEF_TYPESTR(DETERMINING_ORIENTATION)
                DEF_TYPESTR(WAITING_INITIALPOS)
#undef DEF_TYPESTR
        }
        std::map<int, std::string>::const_iterator it = val2str.find(val);
        static const std::string nullstr("???");
        return (it == val2str.end()) ? nullstr : it->second;
}

void generic_dump_BESTPOS(
        const Message_NV_OEM6_BESTPOS::content_t& fields, mrpt::utils::CStream& out)
{
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
        out.printf(
                " Solution status: `%s`\n",
                nv_oem6_solution_status::enum2str(fields.solution_stat).c_str());
        out.printf(
                " Position type  : `%s`\n",
                nv_oem6_position_type::enum2str(fields.position_type).c_str());
        out.printf(
                " Longitude: %.09f deg (std dev: %.06f m)  Latitude: %.09f deg (std "
                "dev: %.06f m)\n",
                fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
        out.printf(
                " Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m   "
                "(Sum: %.04f m)\n",
                fields.hgt, fields.hgt_sigma, fields.undulation,
                fields.hgt + fields.undulation);
        out.printf(
                " Diff age: %.03f  Solution age: %.03f\n", fields.diff_age,
                fields.sol_age);
        out.printf(" Base station ID: `%.*s`\n", 4, fields.base_station_id);
        out.printf(
                " Num sat tracked: %u  Num sat in solution: %u\n",
                (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
}

void Message_NV_OEM6_BESTPOS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 BESTPOS]\n");
        generic_dump_BESTPOS(fields, out);
}

bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m "
                 "undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age "
                 "num_sats_tracked num_sats_sol";
        return true;
}
bool Message_NV_OEM6_BESTPOS::getAllFieldValues(std::ostream& o) const
{
        o << mrpt::format(
                "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f "
                "%u %u",
                (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
                (unsigned)fields.solution_stat, (unsigned)fields.position_type,
                fields.lon, fields.lat, fields.hgt, fields.undulation, fields.lon_sigma,
                fields.lat_sigma, fields.hgt_sigma, fields.diff_age, fields.sol_age,
                (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
        return true;
}

// ------------
void Message_NV_OEM6_INSPVAS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 INSPVAS]\n");
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
        out.printf(
                " INS status: `%s`\n",
                nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
        out.printf(
                " Longitude: %.09f deg  Latitude: %.09f deg  Height: %.03f m\n",
                fields.lon, fields.lat, fields.hgt);
        out.printf(
                " Velocities: North: %.05f  East: %.05f  Up: %.05f\n", fields.vel_north,
                fields.vel_east, fields.vel_up);
        out.printf(
                " Attitude: Roll: %.05f  Pitch: %.05f  Azimuth: %.05f\n", fields.roll,
                fields.pitch, fields.azimuth);
}

bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 "
                 "vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
        return true;
}
bool Message_NV_OEM6_INSPVAS::getAllFieldValues(std::ostream& o) const
{
        o << mrpt::format(
                "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
                (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
                (unsigned)fields.ins_status, fields.lon, fields.lat, fields.hgt,
                fields.vel_north, fields.vel_east, fields.vel_up, fields.roll,
                fields.pitch, fields.azimuth);
        return true;
}

// ------------
void Message_NV_OEM6_INSCOVS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 INSCOVS]\n");
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
        out.printf(
                " Position cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
                "%9.03f %9.03f\n",
                fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
                fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
                fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8]);
        out.printf(
                " Attitude cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
                "%9.03f %9.03f\n",
                fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
                fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
                fields.att_cov[6], fields.att_cov[7], fields.att_cov[8]);
        out.printf(
                " Velocity cov: %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f "
                "%9.03f %9.03f\n",
                fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
                fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
                fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
}

bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
        return true;
}
bool Message_NV_OEM6_INSCOVS::getAllFieldValues(std::ostream& o) const
{
        o << mrpt::format(
                "%u.%08u "
                "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f "
                "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f "
                "%9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f  %9.03f %9.03f %9.03f ",
                (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
                fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
                fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
                fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8],
                fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
                fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
                fields.att_cov[6], fields.att_cov[7], fields.att_cov[8],
                fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
                fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
                fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
        return true;
}

// ------------
void Message_NV_OEM6_RANGECMP::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 RANGECMP]\n");
        out.printf(
                " Number of SAT observations: %u\n",
                static_cast<unsigned int>(this->num_obs));
}

void Message_NV_OEM6_RANGECMP::internal_writeToStream(
        mrpt::utils::CStream& out) const
{
        const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
        out << msg_len;
        out.WriteBuffer(&header, sizeof(header));
        out << num_obs;
        ASSERT_EQUAL_(num_obs, obs_data.size());
        if (num_obs)
                out.WriteBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
}

void Message_NV_OEM6_RANGECMP::internal_readFromStream(mrpt::utils::CStream& in)
{
        uint32_t expected_msg_len;
        in >> expected_msg_len;
        in.ReadBuffer(&header, sizeof(header));
        in >> num_obs;
        ASSERT_BELOW_(num_obs, 2000);
        obs_data.resize(num_obs);
        if (num_obs)
                in.ReadBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
}

// ------------
void Message_NV_OEM6_RXSTATUS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 RXSTATUS]\n");
        out.printf(
                " Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
}

// ------------
void Message_NV_OEM6_RAWEPHEM::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 RAWEPHEM]\n");
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
}

// ------------
void Message_NV_OEM6_VERSION::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 VERSION]\n");
        out.printf(
                " Number of components: %u\n",
                static_cast<unsigned int>(this->num_comps));
        for (unsigned i = 0; i < components.size(); i++)
        {
                out.printf(
                        " Component #%u:\n  Model: `%.*s`\n  Serial: `%.*s`\n  SW "
                        "version:`%.*s`\n",
                        i, (int)sizeof(components[i].model), components[i].model,
                        (int)sizeof(components[i].serial), components[i].serial,
                        (int)sizeof(components[i].swversion), components[i].swversion);
        }
}

void Message_NV_OEM6_VERSION::internal_writeToStream(
        mrpt::utils::CStream& out) const
{
        const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
        out << msg_len;
        out.WriteBuffer(&header, sizeof(header));
        out << num_comps;
        ASSERT_EQUAL_(num_comps, components.size());
        if (num_comps)
                out.WriteBuffer(
                        &components[0], sizeof(components[0]) * components.size());
}

void Message_NV_OEM6_VERSION::internal_readFromStream(mrpt::utils::CStream& in)
{
        uint32_t expected_msg_len;
        in >> expected_msg_len;
        in.ReadBuffer(&header, sizeof(header));
        in >> num_comps;
        ASSERT_BELOW_(num_comps, 2000);
        components.resize(num_comps);
        if (num_comps)
                in.ReadBuffer(
                        &components[0], sizeof(components[0]) * components.size());
}

// ------------
void Message_NV_OEM6_RAWIMUS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 RAWIMUS]\n");
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
        out.printf(" Status: 0x%08lu\n", (long)fields.imu_status);
        out.printf(
                " Acel: X=%li Y=%li Z=%li\n", (long)fields.accel_x,
                -(long)fields.accel_y_neg, (long)fields.accel_z);
        out.printf(
                " Gyro: X=%li Y=%li Z=%li\n", (long)fields.gyro_x,
                -(long)fields.gyro_y_neg, (long)fields.gyro_z);
}

bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
        return true;
}
bool Message_NV_OEM6_RAWIMUS::getAllFieldValues(std::ostream& o) const
{
        o << mrpt::format(
                "%u.%08u %u %li %li %li %li %li %li", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week), (unsigned)fields.imu_status,
                (long)fields.accel_x, -(long)fields.accel_y_neg, (long)fields.accel_z,
                (long)fields.gyro_x, -(long)fields.gyro_y_neg, (long)fields.gyro_z);
        return true;
}

// ------------
void generic_dump_MARKTIME(
        const Message_NV_OEM6_MARKTIME::content_t& fields,
        mrpt::utils::CStream& out)
{
        out.printf(" Clock status: 0x%08lu\n", (long)fields.clock_status);
        out.printf(
                " GPS week: %lu Seconds: %f\n", (long)fields.week, fields.week_seconds);
        out.printf(
                " Clock offset: %f  (std dev = %e)\n", fields.clock_offset,
                fields.clock_offset_std);
        out.printf(" UTC offset: %f\n", fields.utc_offset);
}

void Message_NV_OEM6_MARKTIME::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 MARKTIME]\n");
        generic_dump_MARKTIME(fields, out);
}

bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
        return true;
}
void generic_getFieldValues_MARKTIME(
        const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& o)
{
        o << mrpt::format(
                "%u.%08u %u %lu %f %f", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week), (unsigned)fields.clock_status,
                (long unsigned)fields.week, fields.week_seconds, fields.utc_offset);
}
bool Message_NV_OEM6_MARKTIME::getAllFieldValues(std::ostream& o) const
{
        generic_getFieldValues_MARKTIME(fields, o);
        return true;
}
// ------------
void Message_NV_OEM6_MARK2TIME::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 MARK2TIME]\n");
        generic_dump_MARKTIME(
                *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
                out);
}
bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions(std::ostream& o) const
{
        o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
        return true;
}
bool Message_NV_OEM6_MARK2TIME::getAllFieldValues(std::ostream& o) const
{
        generic_getFieldValues_MARKTIME(
                *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
                o);
        return true;
}

// ------------
void Message_NV_OEM6_MARKPOS::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel OEM6 MARKPOSE]\n");
        generic_dump_BESTPOS(
                *reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields),
                out);
}

// ------------
void Message_NV_OEM6_IONUTC::dumpToStream(mrpt::utils::CStream& out) const
{
        out.printf("[Novatel NV_OEM6_IONUTC]\n");
        out.printf(
                " GPS week: %u  ms in week: %u\n", (unsigned)fields.header.week,
                (unsigned)(fields.header.ms_in_week));
        out.printf(
                " UTC ref week: %u  Tot: %u\n", (unsigned)fields.utc_wn,
                (unsigned)fields.tot);
        out.printf(
                " Leap seconds delta_t: %u  future: %u\n", (unsigned)fields.deltat_ls,
                (unsigned)fields.deltat_lsf);
}