Main MRPT website > C++ reference for MRPT 1.9.9
CGPSInterface_parser_NOVATEL_OEM6.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/os.h>
13 #include <mrpt/system/filesystem.h>
15 #include <mrpt/io/CMemoryStream.h>
16 #include <mrpt/system/crc.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  // Short header?
44  const bool is_short_hdr =
45  peek_buffer[0] == nv_oem6_short_header_t::SYNCH0 &&
46  peek_buffer[1] == nv_oem6_short_header_t::SYNCH1 &&
47  peek_buffer[2] == nv_oem6_short_header_t::SYNCH2;
48 
49  const bool is_regular_hdr = peek_buffer[0] == nv_oem6_header_t::SYNCH0 &&
50  peek_buffer[1] == nv_oem6_header_t::SYNCH1 &&
51  peek_buffer[2] == nv_oem6_header_t::SYNCH2;
52 
53  if (!is_short_hdr && !is_regular_hdr)
54  return false; // skip 1 byte, we dont recognize this format
55 
56  if (is_short_hdr)
57  {
58  if (nBytesAval < sizeof(nv_oem6_short_header_t))
59  {
60  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
61  return true; // we must wait for more data in the buffer
62  }
64  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
65  const uint32_t expected_total_msg_len =
66  sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
67  if (nBytesAval < expected_total_msg_len)
68  {
69  out_minimum_rx_buf_to_decide = expected_total_msg_len;
70  return true; // we must wait for more data in the buffer
71  }
72 
73  std::vector<uint8_t> buf(expected_total_msg_len);
74  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
75  m_rx_buffer.pop_many(
76  reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
77  hdr.msg_len + 4 /*crc*/);
78 
79  // Check CRC:
80  const uint32_t crc_computed =
81  mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
82  const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
83  (buf[expected_total_msg_len - 2] << 16) |
84  (buf[expected_total_msg_len - 3] << 8) |
85  (buf[expected_total_msg_len - 4] << 0);
86  if (crc_read != crc_computed)
87  return false; // skip 1 byte, we dont recognize this format
88 
89  // Deserialize the message:
90  // 1st, test if we have a specific data structure for this msg_id:
91  const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
93  // ------ Serialization format:
94  // const int32_t msg_id = message_type;
95  // out << msg_id;
96  // this->internal_writeToStream(out); == > out <<
97  // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
98  // }
99  // ------
100  mrpt::io::CMemoryStream tmpStream;
101  auto arch = mrpt::serialization::archiveFrom(tmpStream);
102  const uint32_t msg_id = use_generic_container
105  arch << (uint32_t)(msg_id);
106  // This len = hdr + hdr.msg_len + 4 (crc);
107  arch << (uint32_t)(expected_total_msg_len);
108  arch.WriteBuffer(&buf[0], buf.size());
109 
110  tmpStream.Seek(0);
111  gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
112  if (!msg.get())
113  {
114  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
115  "parsing binary packet msg_id="
116  << hdr.msg_id << "\n";
117  return true;
118  }
119  m_just_parsed_messages.messages[msg->message_type] = msg;
120  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
121  if (!CObservationGPS::GPS_time_to_UTC(
122  hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
123  m_just_parsed_messages.timestamp))
124  m_just_parsed_messages.timestamp = mrpt::system::now();
125  else
126  m_just_parsed_messages.has_satellite_timestamp = true;
127 
128  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
129 
130  flushParsedMessagesNow();
131  return true;
132  } // end short hdr
133 
134  if (is_regular_hdr)
135  {
136  if (nBytesAval < sizeof(nv_oem6_header_t))
137  {
138  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t);
139  return true; // we must wait for more data in the buffer
140  }
141  nv_oem6_header_t hdr;
142  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
143  const uint32_t expected_total_msg_len =
144  sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
145  if (nBytesAval < expected_total_msg_len)
146  {
147  out_minimum_rx_buf_to_decide = expected_total_msg_len;
148  return true; // we must wait for more data in the buffer
149  }
150 
151  std::vector<uint8_t> buf(expected_total_msg_len);
152  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
153  m_rx_buffer.pop_many(
154  reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]),
155  hdr.msg_len + 4 /*crc*/);
156 
157  // Check CRC:
158  const uint32_t crc_computed =
159  mrpt::system::compute_CRC32(&buf[0], expected_total_msg_len - 4);
160  const uint32_t crc_read = (buf[expected_total_msg_len - 1] << 24) |
161  (buf[expected_total_msg_len - 2] << 16) |
162  (buf[expected_total_msg_len - 3] << 8) |
163  (buf[expected_total_msg_len - 4] << 0);
164  if (crc_read != crc_computed)
165  return false; // skip 1 byte, we dont recognize this format
166 
167  // Deserialize the message:
168  // 1st, test if we have a specific data structure for this msg_id:
169  const bool use_generic_container = !gnss_message::FactoryKnowsMsgType(
171  // ------ Serialization format:
172  // const int32_t msg_id = message_type;
173  // out << msg_id;
174  // this->internal_writeToStream(out); == > out <<
175  // static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN);
176  // }
177  // ------
178  mrpt::io::CMemoryStream tmpStream;
179  auto arch = mrpt::serialization::archiveFrom(tmpStream);
180  const int32_t msg_id = use_generic_container
183  arch << msg_id;
184  arch << (uint32_t)(expected_total_msg_len);
185  arch.WriteBuffer(&buf[0], buf.size());
186 
187  tmpStream.Seek(0);
188  gnss_message_ptr msg(gnss_message::readAndBuildFromStream(arch));
189  if (!msg.get())
190  {
191  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error "
192  "parsing binary packet msg_id="
193  << hdr.msg_id << "\n";
194  return true;
195  }
196  m_just_parsed_messages.messages[msg->message_type] = msg;
197  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
198  {
199  // Detect NV_OEM6_IONUTC msgs to learn about the current leap
200  // seconds:
201  const gnss::Message_NV_OEM6_IONUTC* ionutc =
202  dynamic_cast<const gnss::Message_NV_OEM6_IONUTC*>(msg.get());
203  if (ionutc) num_leap_seconds = ionutc->fields.deltat_ls;
204  }
205  if (!CObservationGPS::GPS_time_to_UTC(
206  hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
207  m_just_parsed_messages.timestamp))
208  m_just_parsed_messages.timestamp = mrpt::system::now();
209  else
210  m_just_parsed_messages.has_satellite_timestamp = true;
211 
212  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
213  flushParsedMessagesNow();
214  return true;
215  } // end regular hdr
216 
217  // Shouldnt arrive here, but MSVC complies anyway:
218  return false;
219 }
gnss_message_type_t
List of all known GNSS message types.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
STL namespace.
Novatel OEM6 regular header structure.
unsigned char uint8_t
Definition: rptypes.h:41
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:561
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)
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.
__int32 int32_t
Definition: rptypes.h:46
const std::string & getMessageTypeAsString() const
Returns "NMEA_GGA", etc.
unsigned __int32 uint32_t
Definition: rptypes.h:47
A smart pointer to a GNSS message.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019