Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
14 #include <mrpt/system/filesystem.h>
17 #include <mrpt/utils/crc.h>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::obs;
21 using namespace std;
22 
23 bool CGPSInterface::implement_parser_NOVATEL_OEM6(size_t &out_minimum_rx_buf_to_decide)
24 {
25  // to be grabbed from the last Message_NV_OEM6_IONUTC msg
26  static uint32_t num_leap_seconds = getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS")==NULL ?
27  18 : atoi(getenv("MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS"));
28 
29  using namespace mrpt::obs::gnss;
30 
31  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_short_header_t);
32 
33  const size_t nBytesAval = m_rx_buffer.size(); // Available for read
34  if (nBytesAval<out_minimum_rx_buf_to_decide)
35  return true; // no need to skip 1 byte
36 
37  // If the synch bytes do not match, it is not a valid frame:
38  uint8_t peek_buffer[3];
39  m_rx_buffer.peek_many(&peek_buffer[0],3);
40  // Short header?
41  const bool is_short_hdr =
42  peek_buffer[0]==nv_oem6_short_header_t::SYNCH0 &&
43  peek_buffer[1]==nv_oem6_short_header_t::SYNCH1 &&
44  peek_buffer[2]==nv_oem6_short_header_t::SYNCH2;
45 
46  const bool is_regular_hdr =
47  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)
52  return false; // skip 1 byte, we dont recognize this format
53 
54  if (is_short_hdr)
55  {
56  if (nBytesAval<sizeof(nv_oem6_short_header_t)) {
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  }
61  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
62  const uint32_t expected_total_msg_len = sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
63  if (nBytesAval<expected_total_msg_len) {
64  out_minimum_rx_buf_to_decide = expected_total_msg_len;
65  return true; // we must wait for more data in the buffer
66  }
67 
68  std::vector<uint8_t> buf(expected_total_msg_len);
69  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
70  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]), hdr.msg_len + 4 /*crc*/ );
71 
72  // Check CRC:
73  const uint32_t crc_computed = mrpt::utils::compute_CRC32(&buf[0], expected_total_msg_len-4);
74  const uint32_t crc_read =
75  (buf[expected_total_msg_len-1] << 24) |
76  (buf[expected_total_msg_len-2] << 16) |
77  (buf[expected_total_msg_len-3] << 8) |
78  (buf[expected_total_msg_len-4] << 0);
79  if (crc_read!=crc_computed)
80  return false; // skip 1 byte, we dont recognize this format
81 
82  // Deserialize the message:
83  // 1st, test if we have a specific data structure for this msg_id:
84  const bool use_generic_container = !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 << static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN); }
89  // ------
91  const uint32_t msg_id = use_generic_container ?
93  :
95  tmpStream << (uint32_t )(msg_id);
96  tmpStream << (uint32_t)(expected_total_msg_len); // This len = hdr + hdr.msg_len + 4 (crc);
97  tmpStream.WriteBuffer(&buf[0],buf.size());
98 
99  tmpStream.Seek(0);
100  gnss_message_ptr msg( gnss_message::readAndBuildFromStream(tmpStream) );
101  if (!msg.get()) {
102  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error parsing binary packet msg_id="<< hdr.msg_id<<"\n";
103  return true;
104  }
105  m_just_parsed_messages.messages[msg->message_type] = msg;
106  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
107  if (!CObservationGPS::GPS_time_to_UTC(hdr.week,hdr.ms_in_week*1e-3,num_leap_seconds, m_just_parsed_messages.timestamp))
108  m_just_parsed_messages.timestamp = mrpt::system::now();
109  else m_just_parsed_messages.has_satellite_timestamp = true;
110 
111  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
112 
113  flushParsedMessagesNow();
114  return true;
115  } // end short hdr
116 
117  if (is_regular_hdr)
118  {
119  if (nBytesAval<sizeof(nv_oem6_header_t)) {
120  out_minimum_rx_buf_to_decide = sizeof(nv_oem6_header_t);
121  return true; // we must wait for more data in the buffer
122  }
123  nv_oem6_header_t hdr;
124  m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr), sizeof(hdr));
125  const uint32_t expected_total_msg_len = sizeof(hdr) + hdr.msg_len + 4 /*crc*/;
126  if (nBytesAval<expected_total_msg_len)
127  {
128  out_minimum_rx_buf_to_decide = expected_total_msg_len;
129  return true; // we must wait for more data in the buffer
130  }
131 
132  std::vector<uint8_t> buf(expected_total_msg_len);
133  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[0]), sizeof(hdr));
134  m_rx_buffer.pop_many(reinterpret_cast<uint8_t*>(&buf[sizeof(hdr)]), hdr.msg_len + 4 /*crc*/ );
135 
136  // Check CRC:
137  const uint32_t crc_computed = mrpt::utils::compute_CRC32(&buf[0], expected_total_msg_len-4);
138  const uint32_t crc_read =
139  (buf[expected_total_msg_len-1] << 24) |
140  (buf[expected_total_msg_len-2] << 16) |
141  (buf[expected_total_msg_len-3] << 8) |
142  (buf[expected_total_msg_len-4] << 0);
143  if (crc_read!=crc_computed)
144  return false; // skip 1 byte, we dont recognize this format
145 
146  // Deserialize the message:
147  // 1st, test if we have a specific data structure for this msg_id:
148  const bool use_generic_container = !gnss_message::FactoryKnowsMsgType( (gnss_message_type_t)(NV_OEM6_MSG2ENUM + hdr.msg_id ) );
149  // ------ Serialization format:
150  //const int32_t msg_id = message_type;
151  //out << msg_id;
152  //this->internal_writeToStream(out); == > out << static_cast<uint32_t>(DATA_LEN); out.WriteBuffer(DATA_PTR,DATA_LEN); }
153  // ------
154  mrpt::utils::CMemoryStream tmpStream;
155  const int32_t msg_id = use_generic_container ?
157  :
159  tmpStream << msg_id;
160  tmpStream << (uint32_t)(expected_total_msg_len);
161  tmpStream.WriteBuffer(&buf[0],buf.size());
162 
163  tmpStream.Seek(0);
164  gnss_message_ptr msg( gnss_message::readAndBuildFromStream(tmpStream) );
165  if (!msg.get()) {
166  std::cerr << "[CGPSInterface::implement_parser_NOVATEL_OEM6] Error parsing binary packet msg_id="<< hdr.msg_id<<"\n";
167  return true;
168  }
169  m_just_parsed_messages.messages[msg->message_type] = msg;
170  m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
171  {
172  // Detect NV_OEM6_IONUTC msgs to learn about the current leap seconds:
173  const gnss::Message_NV_OEM6_IONUTC *ionutc = dynamic_cast<const gnss::Message_NV_OEM6_IONUTC *>(msg.get());
174  if (ionutc)
175  num_leap_seconds = ionutc->fields.deltat_ls;
176  }
177  if (!CObservationGPS::GPS_time_to_UTC(hdr.week,hdr.ms_in_week*1e-3,num_leap_seconds,m_just_parsed_messages.timestamp))
178  m_just_parsed_messages.timestamp = mrpt::system::now();
179  else m_just_parsed_messages.has_satellite_timestamp = true;
180 
181  m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
182  flushParsedMessagesNow();
183  return true;
184  } // end regular hdr
185 
186  // Shouldnt arrive here, but MSVC complies anyway:
187  return false;
188 }
gnss_message_type_t
List of all known GNSS message types.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
Contains classes for various device interfaces.
STL namespace.
Novatel OEM6 regular header structure.
unsigned char uint8_t
Definition: rptypes.h:43
This CStream derived class allow using a memory buffer as a CStream.
Definition: CMemoryStream.h:26
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:48
uint32_t BASE_IMPEXP 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:27
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) MRPT_OVERRIDE
Introduces a pure virtual method for moving to a specified position in the streamed resource...
const std::string & getMessageTypeAsString() const
Returns "NMEA_GGA", etc.
unsigned __int32 uint32_t
Definition: rptypes.h:49
A smart pointer to a GNSS message.



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019