| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "hwdrivers-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/hwdrivers/CGPSInterface.h> |
| 13 |
#include <mrpt/io/CMemoryStream.h> |
| 14 |
#include <mrpt/system/crc.h> |
| 15 |
#include <mrpt/system/filesystem.h> |
| 16 |
#include <mrpt/system/os.h> |
| 17 |
|
| 18 |
#include <iostream> |
| 19 |
|
| 20 |
using namespace mrpt::hwdrivers; |
| 21 |
using namespace mrpt::obs; |
| 22 |
using namespace std; |
| 23 |
|
| 24 |
bool CGPSInterface::implement_parser_NOVATEL_OEM6(size_t& out_minimum_rx_buf_to_decide) |
| 25 |
{ |
| 26 |
// to be grabbed from the last Message_NV_OEM6_IONUTC msg |
| 27 |
static uint32_t num_leap_seconds = getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS") == nullptr |
| 28 |
? 18 |
| 29 |
: atoi(getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS")); |
| 30 |
|
| 31 |
using namespace mrpt::obs::gnss; |
| 32 |
|
| 33 |
out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t); |
| 34 |
|
| 35 |
const size_t nBytesAval = m_rx_buffer.size(); // Available for read |
| 36 |
if (nBytesAval < out_minimum_rx_buf_to_decide) return true; // no need to skip 1 byte |
| 37 |
|
| 38 |
// If the synch bytes do not match, it is not a valid frame: |
| 39 |
uint8_t peek_buffer[3]; |
| 40 |
m_rx_buffer.peek_many(&peek_buffer[0], 3); |
| 41 |
|
| 42 |
// Short header? |
| 43 |
const bool is_short_hdr = peek_buffer[0] == nv_oem6_short_header_t::SYNCH0 && |
| 44 |
peek_buffer[1] == nv_oem6_short_header_t::SYNCH1 && |
| 45 |
peek_buffer[2] == nv_oem6_short_header_t::SYNCH2; |
| 46 |
|
| 47 |
const bool is_regular_hdr = peek_buffer[0] == nv_oem6_header_t::SYNCH0 && |
| 48 |
peek_buffer[1] == nv_oem6_header_t::SYNCH1 && |
| 49 |
peek_buffer[2] == nv_oem6_header_t::SYNCH2; |
| 50 |
|
| 51 |
if (!is_short_hdr && !is_regular_hdr) return false; // skip 1 byte, we dont recognize this format |
| 52 |
|
| 53 |
if (is_short_hdr) |
| 54 |
{ |
| 55 |
if (nBytesAval < sizeof(nv_oem6_short_header_t)) |
| 56 |
{ |
| 57 |
out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t); |
| 58 |
return true; // we must wait for more data in the buffer |
| 59 |
} |
| 60 |
nv_oem6_short_header_t hdr; |
| 61 |
m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr)); |
| 62 |
hdr.fixEndianness(); |
| 63 |
const uint32_t expected_total_msg_len = sizeof(hdr) + hdr.msg_len + 4 /*crc*/; |
| 64 |
if (nBytesAval < expected_total_msg_len) |
| 65 |
{ |
| 66 |
out_minimum_rx_buf_to_decide = expected_total_msg_len; |
| 67 |
return true; // we must wait for more data in the buffer |
| 68 |
} |
| 69 |
|
| 70 |
std::vector<uint8_t> buf(expected_total_msg_len); |
| 71 |
m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr)); |
| 72 |
m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]), hdr.msg_len + 4 /*crc*/); |
| 73 |
|
| 74 |
// Check CRC: |
| 75 |
const uint32_t crc_computed = mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4); |
| 76 |
const uint32_t crc_read = |
| 77 |
(buf[expected_total_msg_len - 1] << 24) | (buf[expected_total_msg_len - 2] << 16) | |
| 78 |
(buf[expected_total_msg_len - 3] << 8) | (buf[expected_total_msg_len - 4] << 0); |
| 79 |
if (crc_read != crc_computed) return false; // skip 1 byte, we dont recognize this format |
| 80 |
|
| 81 |
// Deserialize the message: |
| 82 |
// 1st, test if we have a specific data structure for this msg_id: |
| 83 |
const bool use_generic_container = |
| 84 |
!gnss_message::FactoryKnowsMsgType((gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id)); |
| 85 |
// ------ Serialization format: |
| 86 |
// const int32_t msg_id = message_type; |
| 87 |
// out << msg_id; |
| 88 |
// this->internal_writeToStream(out); == > out << |
| 89 |
// static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN); |
| 90 |
// } |
| 91 |
// ------ |
| 92 |
mrpt::io::CMemoryStream tmpStream; |
| 93 |
auto arch = mrpt::serialization::archiveFrom(tmpStream); |
| 94 |
const uint32_t msg_id = use_generic_container ? (uint32_t)(NV_OEM6_GENERIC_SHORT_FRAME) |
| 95 |
: (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM; |
| 96 |
arch << (uint32_t)(msg_id); |
| 97 |
// This len = hdr + hdr.msg_len + 4 (crc); |
| 98 |
arch << (uint32_t)(expected_total_msg_len); |
| 99 |
arch.WriteBuffer(&buf[0], buf.size()); |
| 100 |
|
| 101 |
tmpStream.Seek(0); |
| 102 |
gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch)); |
| 103 |
if (!msg.get()) |
| 104 |
{ |
| 105 |
std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error " |
| 106 |
"parsing binary packet msg_id=" |
| 107 |
<< hdr.msg_id << "\n"; |
| 108 |
return true; |
| 109 |
} |
| 110 |
m_parsed_messages->messages[msg->message_type] = msg; |
| 111 |
m_parsed_messages->originalReceivedTimestamp = mrpt::Clock::now(); |
| 112 |
if (!CObservationGPS::GPS_time_to_UTC( |
| 113 |
hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds, m_parsed_messages->timestamp)) |
| 114 |
m_parsed_messages->timestamp = mrpt::Clock::now(); |
| 115 |
else |
| 116 |
m_parsed_messages->has_satellite_timestamp = true; |
| 117 |
|
| 118 |
m_parsed_messages->sensorLabel = msg->getMessageTypeAsString(); |
| 119 |
|
| 120 |
flushParsedMessagesNow(); |
| 121 |
return true; |
| 122 |
} // end short hdr |
| 123 |
|
| 124 |
if (is_regular_hdr) |
| 125 |
{ |
| 126 |
if (nBytesAval < sizeof(nv_oem6_header_t)) |
| 127 |
{ |
| 128 |
out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t); |
| 129 |
return true; // we must wait for more data in the buffer |
| 130 |
} |
| 131 |
nv_oem6_header_t hdr; |
| 132 |
m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr)); |
| 133 |
hdr.fixEndianness(); |
| 134 |
const uint32_t expected_total_msg_len = sizeof(hdr) + hdr.msg_len + 4 /*crc*/; |
| 135 |
if (nBytesAval < expected_total_msg_len) |
| 136 |
{ |
| 137 |
out_minimum_rx_buf_to_decide = expected_total_msg_len; |
| 138 |
return true; // we must wait for more data in the buffer |
| 139 |
} |
| 140 |
|
| 141 |
std::vector<uint8_t> buf(expected_total_msg_len); |
| 142 |
m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr)); |
| 143 |
m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]), hdr.msg_len + 4 /*crc*/); |
| 144 |
|
| 145 |
// Check CRC: |
| 146 |
const uint32_t crc_computed = mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4); |
| 147 |
const uint32_t crc_read = |
| 148 |
(buf[expected_total_msg_len - 1] << 24) | (buf[expected_total_msg_len - 2] << 16) | |
| 149 |
(buf[expected_total_msg_len - 3] << 8) | (buf[expected_total_msg_len - 4] << 0); |
| 150 |
if (crc_read != crc_computed) return false; // skip 1 byte, we dont recognize this format |
| 151 |
|
| 152 |
// Deserialize the message: |
| 153 |
// 1st, test if we have a specific data structure for this msg_id: |
| 154 |
const bool use_generic_container = |
| 155 |
!gnss_message::FactoryKnowsMsgType((gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id)); |
| 156 |
// ------ Serialization format: |
| 157 |
// const int32_t msg_id = message_type; |
| 158 |
// out << msg_id; |
| 159 |
// this->internal_writeToStream(out); == > out << |
| 160 |
// static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN); |
| 161 |
// } |
| 162 |
// ------ |
| 163 |
mrpt::io::CMemoryStream tmpStream; |
| 164 |
auto arch = mrpt::serialization::archiveFrom(tmpStream); |
| 165 |
const int32_t msg_id = use_generic_container ? (uint32_t)(NV_OEM6_GENERIC_FRAME) |
| 166 |
: (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM; |
| 167 |
arch << msg_id; |
| 168 |
arch << (uint32_t)(expected_total_msg_len); |
| 169 |
arch.WriteBuffer(&buf[0], buf.size()); |
| 170 |
|
| 171 |
tmpStream.Seek(0); |
| 172 |
gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch)); |
| 173 |
if (!msg.get()) |
| 174 |
{ |
| 175 |
std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error " |
| 176 |
"parsing binary packet msg_id=" |
| 177 |
<< hdr.msg_id << "\n"; |
| 178 |
return true; |
| 179 |
} |
| 180 |
m_parsed_messages->messages[msg->message_type] = msg; |
| 181 |
m_parsed_messages->originalReceivedTimestamp = mrpt::Clock::now(); |
| 182 |
{ |
| 183 |
// Detect NV_OEM6_IONUTC msgs to learn about the current leap |
| 184 |
// seconds: |
| 185 |
const auto* ionutc = dynamic_cast<const gnss::Message_NV_OEM6_IONUTC*>(msg.get()); |
| 186 |
if (ionutc) num_leap_seconds = ionutc->fields.deltat_ls; |
| 187 |
} |
| 188 |
if (!CObservationGPS::GPS_time_to_UTC( |
| 189 |
hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds, m_parsed_messages->timestamp)) |
| 190 |
m_parsed_messages->timestamp = mrpt::Clock::now(); |
| 191 |
else |
| 192 |
m_parsed_messages->has_satellite_timestamp = true; |
| 193 |
|
| 194 |
m_parsed_messages->sensorLabel = msg->getMessageTypeAsString(); |
| 195 |
flushParsedMessagesNow(); |
| 196 |
return true; |
| 197 |
} // end regular hdr |
| 198 |
|
| 199 |
// Shouldnt arrive here, but MSVC complies anyway: |
| 200 |
return false; |
| 201 |
} |