CGPSInterface_parser_NOVATEL_OEM6.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/system/os.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/system/crc.h>
#include <iostream>

using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace std;

bool CGPSInterface::implement_parser_NOVATEL_OEM6(
    size_t& out_minimum_rx_buf_to_decide)
{
    // to be grabbed from the last Message_NV_OEM6_IONUTC msg
    static uint32_t num_leap_seconds =
        getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS") == nullptr
            ? 18
            : atoi(getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS"));

    using namespace mrpt::obs::gnss;

    out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);

    const size_t nBytesAval = m_rx_buffer.size();  // Available for read
    if (nBytesAval < out_minimum_rx_buf_to_decide)
        return true;  // no need to skip 1 byte

    // If the synch bytes do not match, it is not a valid frame:
    uint8_t peek_buffer[3];
    m_rx_buffer.peek_many(&peek_buffer[0], 3);
    // Short header?
    const bool is_short_hdr =
        peek_buffer[0] == nv_oem6_short_header_t::SYNCH0 &&
        peek_buffer[1] == nv_oem6_short_header_t::SYNCH1 &&
        peek_buffer[2] == nv_oem6_short_header_t::SYNCH2;

    const bool is_regular_hdr = peek_buffer[0] == nv_oem6_header_t::SYNCH0 &&
                                peek_buffer[1] == nv_oem6_header_t::SYNCH1 &&
                                peek_buffer[2] == nv_oem6_header_t::SYNCH2;

    if (!is_short_hdr && !is_regular_hdr)
        return false;  // skip 1 byte, we dont recognize this format

    if (is_short_hdr)
    {
        if (nBytesAval < sizeof(nv_oem6_short_header_t))
        {
            out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
            return true;  // we must wait for more data in the buffer
        }
        nv_oem6_short_header_t hdr;
        m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
        const uint32_t expected_total_msg_len =
            sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
        if (nBytesAval < expected_total_msg_len)
        {
            out_minimum_rx_buf_to_decide = expected_total_msg_len;
            return true;  // we must wait for more data in the buffer
        }

        std::vector<uint8_t> buf(expected_total_msg_len);
        m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
        m_rx_buffer.pop_many(
            reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
            hdr.msg_len + 4 /*crc*/);

        // Check CRC:
        const uint32_t crc_computed =
            mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
        const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
                                  (buf[expected_total_msg_len - 2] << 16) |
                                  (buf[expected_total_msg_len - 3] << 8) |
                                  (buf[expected_total_msg_len - 4] << 0);
        if (crc_read != crc_computed)
            return false;  // skip 1 byte, we dont recognize this format

        // Deserialize the message:
        // 1st, test if we have a specific data structure for this msg_id:
        const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
            (gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id));
        // ------ Serialization format:
        // const int32_t msg_id = message_type;
        // out << msg_id;
        // this->internal_writeToStream(out);  == >  out <<
        // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
        // }
        // ------
        mrpt::io::CMemoryStream tmpStream;
        auto arch = mrpt::serialization::archiveFrom(tmpStream);
        const uint32_t msg_id = use_generic_container
                                    ? (uint32_t)(NV_OEM6_GENERIC_SHORT_FRAME)
                                    : (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
        arch << (uint32_t)(msg_id);
        // This len = hdr + hdr.msg_len + 4 (crc);
        arch << (uint32_t)(expected_total_msg_len);
        arch.WriteBuffer(&buf[0], buf.size());

        tmpStream.Seek(0);
        gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
        if (!msg.get())
        {
            std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
                         "parsing binary packet msg_id="
                      << hdr.msg_id << "\n";
            return true;
        }
        m_just_parsed_messages.messages[msg->message_type] = msg;
        m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
        if (!CObservationGPS::GPS_time_to_UTC(
                hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
                m_just_parsed_messages.timestamp))
            m_just_parsed_messages.timestamp = mrpt::system::now();
        else
            m_just_parsed_messages.has_satellite_timestamp = true;

        m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();

        flushParsedMessagesNow();
        return true;
    }  // end short hdr

    if (is_regular_hdr)
    {
        if (nBytesAval < sizeof(nv_oem6_header_t))
        {
            out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t);
            return true;  // we must wait for more data in the buffer
        }
        nv_oem6_header_t hdr;
        m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
        const uint32_t expected_total_msg_len =
            sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
        if (nBytesAval < expected_total_msg_len)
        {
            out_minimum_rx_buf_to_decide = expected_total_msg_len;
            return true;  // we must wait for more data in the buffer
        }

        std::vector<uint8_t> buf(expected_total_msg_len);
        m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
        m_rx_buffer.pop_many(
            reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
            hdr.msg_len + 4 /*crc*/);

        // Check CRC:
        const uint32_t crc_computed =
            mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
        const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
                                  (buf[expected_total_msg_len - 2] << 16) |
                                  (buf[expected_total_msg_len - 3] << 8) |
                                  (buf[expected_total_msg_len - 4] << 0);
        if (crc_read != crc_computed)
            return false;  // skip 1 byte, we dont recognize this format

        // Deserialize the message:
        // 1st, test if we have a specific data structure for this msg_id:
        const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
            (gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id));
        // ------ Serialization format:
        // const int32_t msg_id = message_type;
        // out << msg_id;
        // this->internal_writeToStream(out);  == >  out <<
        // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
        // }
        // ------
        mrpt::io::CMemoryStream tmpStream;
        auto arch = mrpt::serialization::archiveFrom(tmpStream);
        const int32_t msg_id = use_generic_container
                                   ? (uint32_t)(NV_OEM6_GENERIC_FRAME)
                                   : (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
        arch << msg_id;
        arch << (uint32_t)(expected_total_msg_len);
        arch.WriteBuffer(&buf[0], buf.size());

        tmpStream.Seek(0);
        gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
        if (!msg.get())
        {
            std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
                         "parsing binary packet msg_id="
                      << hdr.msg_id << "\n";
            return true;
        }
        m_just_parsed_messages.messages[msg->message_type] = msg;
        m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
        {
            // Detect NV_OEM6_IONUTC msgs to learn about the current leap
            // seconds:
            const gnss::Message_NV_OEM6_IONUTC* ionutc =
                dynamic_cast<const gnss::Message_NV_OEM6_IONUTC*>(msg.get());
            if (ionutc) num_leap_seconds = ionutc->fields.deltat_ls;
        }
        if (!CObservationGPS::GPS_time_to_UTC(
                hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
                m_just_parsed_messages.timestamp))
            m_just_parsed_messages.timestamp = mrpt::system::now();
        else
            m_just_parsed_messages.has_satellite_timestamp = true;

        m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
        flushParsedMessagesNow();
        return true;
    }  // end regular hdr

    // Shouldnt arrive here, but MSVC complies anyway:
    return false;
}