MRPT  1.9.9
CGPSInterface_parser_NOVATEL_OEM6.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
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 #include <iostream>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::obs;
21 using namespace std;
22 
24  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 =
28  getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS") == nullptr
29  ? 18
30  : atoi(getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS"));
31 
32  using namespace mrpt::obs::gnss;
33 
34  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
35 
36  const size_t nBytesAval = m_rx_buffer.size(); // Available for read
37  if (nBytesAval < out_minimum_rx_buf_to_decide)
38  return true; // no need to skip 1 byte
39 
40  // If the synch bytes do not match, it is not a valid frame:
41  uint8_t peek_buffer[3];
42  m_rx_buffer.peek_many(&peek_buffer[0], 3);
43 
44  // Short header?
45  const bool is_short_hdr =
46  peek_buffer[0] == nv_oem6_short_header_t::SYNCH0 &&
47  peek_buffer[1] == nv_oem6_short_header_t::SYNCH1 &&
48  peek_buffer[2] == nv_oem6_short_header_t::SYNCH2;
49 
50  const bool is_regular_hdr = peek_buffer[0] == nv_oem6_header_t::SYNCH0 &&
51  peek_buffer[1] == nv_oem6_header_t::SYNCH1 &&
52  peek_buffer[2] == nv_oem6_header_t::SYNCH2;
53 
54  if (!is_short_hdr && !is_regular_hdr)
55  return false; // skip 1 byte, we dont recognize this format
56 
57  if (is_short_hdr)
58  {
59  if (nBytesAval < sizeof(nv_oem6_short_header_t))
60  {
61  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
62  return true; // we must wait for more data in the buffer
63  }
65  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
66  hdr.fixEndianness();
67  const uint32_t expected_total_msg_len =
68  sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
69  if (nBytesAval < expected_total_msg_len)
70  {
71  out_minimum_rx_buf_to_decide = expected_total_msg_len;
72  return true; // we must wait for more data in the buffer
73  }
74 
75  std::vector<uint8_t> buf(expected_total_msg_len);
76  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
77  m_rx_buffer.pop_many(
78  reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
79  hdr.msg_len + 4 /*crc*/);
80 
81  // Check CRC:
82  const uint32_t crc_computed =
83  mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
84  const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
85  (buf[expected_total_msg_len - 2] << 16) |
86  (buf[expected_total_msg_len - 3] << 8) |
87  (buf[expected_total_msg_len - 4] << 0);
88  if (crc_read != crc_computed)
89  return false; // skip 1 byte, we dont recognize this format
90 
91  // Deserialize the message:
92  // 1st, test if we have a specific data structure for this msg_id:
93  const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
95  // ------ Serialization format:
96  // const int32_t msg_id = message_type;
97  // out << msg_id;
98  // this->internal_writeToStream(out); == > out <<
99  // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
100  // }
101  // ------
102  mrpt::io::CMemoryStream tmpStream;
103  auto arch = mrpt::serialization::archiveFrom(tmpStream);
104  const uint32_t msg_id = use_generic_container
105  ? (uint32_t)(NV_OEM6_GENERIC_SHORT_FRAME)
106  : (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
107  arch << (uint32_t)(msg_id);
108  // This len = hdr + hdr.msg_len + 4 (crc);
109  arch << (uint32_t)(expected_total_msg_len);
110  arch.WriteBuffer(&buf[0], buf.size());
111 
112  tmpStream.Seek(0);
113  gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
114  if (!msg.get())
115  {
116  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
117  "parsing binary packet msg_id="
118  << hdr.msg_id << "\n";
119  return true;
120  }
121  m_just_parsed_messages.messages[msg->message_type] = msg;
122  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
123  if (!CObservationGPS::GPS_time_to_UTC(
124  hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
125  m_just_parsed_messages.timestamp))
126  m_just_parsed_messages.timestamp = mrpt::system::now();
127  else
128  m_just_parsed_messages.has_satellite_timestamp = true;
129 
130  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
131 
132  flushParsedMessagesNow();
133  return true;
134  } // end short hdr
135 
136  if (is_regular_hdr)
137  {
138  if (nBytesAval < sizeof(nv_oem6_header_t))
139  {
140  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t);
141  return true; // we must wait for more data in the buffer
142  }
143  nv_oem6_header_t hdr;
144  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
145  hdr.fixEndianness();
146  const uint32_t expected_total_msg_len =
147  sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
148  if (nBytesAval < expected_total_msg_len)
149  {
150  out_minimum_rx_buf_to_decide = expected_total_msg_len;
151  return true; // we must wait for more data in the buffer
152  }
153 
154  std::vector<uint8_t> buf(expected_total_msg_len);
155  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
156  m_rx_buffer.pop_many(
157  reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
158  hdr.msg_len + 4 /*crc*/);
159 
160  // Check CRC:
161  const uint32_t crc_computed =
162  mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
163  const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
164  (buf[expected_total_msg_len - 2] << 16) |
165  (buf[expected_total_msg_len - 3] << 8) |
166  (buf[expected_total_msg_len - 4] << 0);
167  if (crc_read != crc_computed)
168  return false; // skip 1 byte, we dont recognize this format
169 
170  // Deserialize the message:
171  // 1st, test if we have a specific data structure for this msg_id:
172  const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
174  // ------ Serialization format:
175  // const int32_t msg_id = message_type;
176  // out << msg_id;
177  // this->internal_writeToStream(out); == > out <<
178  // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
179  // }
180  // ------
181  mrpt::io::CMemoryStream tmpStream;
182  auto arch = mrpt::serialization::archiveFrom(tmpStream);
183  const int32_t msg_id = use_generic_container
184  ? (uint32_t)(NV_OEM6_GENERIC_FRAME)
185  : (uint32_t)hdr.msg_id + NV_OEM6_MSG2ENUM;
186  arch << msg_id;
187  arch << (uint32_t)(expected_total_msg_len);
188  arch.WriteBuffer(&buf[0], buf.size());
189 
190  tmpStream.Seek(0);
191  gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
192  if (!msg.get())
193  {
194  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
195  "parsing binary packet msg_id="
196  << hdr.msg_id << "\n";
197  return true;
198  }
199  m_just_parsed_messages.messages[msg->message_type] = msg;
200  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
201  {
202  // Detect NV_OEM6_IONUTC msgs to learn about the current leap
203  // seconds:
204  const auto* ionutc =
205  dynamic_cast<const gnss::Message_NV_OEM6_IONUTC*>(msg.get());
206  if (ionutc) num_leap_seconds = ionutc->fields.deltat_ls;
207  }
208  if (!CObservationGPS::GPS_time_to_UTC(
209  hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
210  m_just_parsed_messages.timestamp))
211  m_just_parsed_messages.timestamp = mrpt::system::now();
212  else
213  m_just_parsed_messages.has_satellite_timestamp = true;
214 
215  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
216  flushParsedMessagesNow();
217  return true;
218  } // end regular hdr
219 
220  // Shouldnt arrive here, but MSVC complies anyway:
221  return false;
222 }
gnss_message_type_t
List of all known GNSS message types.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
STL namespace.
Novatel OEM6 regular header structure.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:591
This CStream derived class allow using a memory buffer as a CStream.
uint32_t compute_CRC32(const std::vector< uint8_t > &data, const uint32_t gen_pol=0xEDB88320L)
Computes the CRC32 checksum of a block of data.
Definition: crc.cpp:22
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource...
This namespace contains representation of robot actions and observations.
bool implement_parser_NOVATEL_OEM6(size_t &out_minimum_rx_buf_to_decide)
__int32 int32_t
Definition: glext.h:3455
gnss_message_type_t message_type
Type of GNSS message.
Novatel OEM6 short header structure.
GNSS (GPS) data structures, mainly for use within mrpt::obs::CObservationGPS.
const std::string & getMessageTypeAsString() const
Returns "NMEA_GGA", etc.
A smart pointer to a GNSS message.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019