26 static uint32_t num_leap_seconds = getenv(
"MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS")==NULL ?
27 18 : atoi(getenv(
"MRPT_HWDRIVERS_DEFAULT_LEAP_SECONDS"));
33 const size_t nBytesAval = m_rx_buffer.size();
34 if (nBytesAval<out_minimum_rx_buf_to_decide)
39 m_rx_buffer.peek_many(&peek_buffer[0],3);
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;
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;
51 if (!is_short_hdr && !is_regular_hdr)
61 m_rx_buffer.peek_many(reinterpret_cast<uint8_t*>(&hdr),
sizeof(hdr));
63 if (nBytesAval<expected_total_msg_len) {
64 out_minimum_rx_buf_to_decide = expected_total_msg_len;
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 );
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)
91 const uint32_t msg_id = use_generic_container ?
96 tmpStream << (
uint32_t)(expected_total_msg_len);
102 std::cerr <<
"[CGPSInterface::implement_parser_NOVATEL_OEM6] Error parsing binary packet msg_id="<< hdr.
msg_id<<
"\n";
105 m_just_parsed_messages.messages[msg->
message_type] = msg;
107 if (!CObservationGPS::GPS_time_to_UTC(hdr.
week,hdr.
ms_in_week*1e-3,num_leap_seconds, m_just_parsed_messages.timestamp))
109 else m_just_parsed_messages.has_satellite_timestamp =
true;
113 flushParsedMessagesNow();
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 ;
126 if (nBytesAval<expected_total_msg_len)
128 out_minimum_rx_buf_to_decide = expected_total_msg_len;
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 );
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)
155 const int32_t msg_id = use_generic_container ?
160 tmpStream << (
uint32_t)(expected_total_msg_len);
166 std::cerr <<
"[CGPSInterface::implement_parser_NOVATEL_OEM6] Error parsing binary packet msg_id="<< hdr.
msg_id<<
"\n";
169 m_just_parsed_messages.messages[msg->
message_type] = msg;
173 const gnss::Message_NV_OEM6_IONUTC *ionutc =
dynamic_cast<const gnss::Message_NV_OEM6_IONUTC *
>(msg.
get());
175 num_leap_seconds = ionutc->fields.deltat_ls;
177 if (!CObservationGPS::GPS_time_to_UTC(hdr.
week,hdr.
ms_in_week*1e-3,num_leap_seconds,m_just_parsed_messages.timestamp))
179 else m_just_parsed_messages.has_satellite_timestamp =
true;
182 flushParsedMessagesNow();
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.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
This CStream derived class allow using a memory buffer as a CStream.
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.
GNSS (GPS) data structures, mainly for use within mrpt::obs::CObservationGPS.
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.
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
A smart pointer to a GNSS message.