Debian Codegraph @

libs/hwdrivers/src/CGPSInterface_parser_NOVATEL_OEM6.cpp

raw
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 }