stdbin_decoder.cpp
Go to the documentation of this file.
1 #include <numeric>
2 #include <sstream>
3 #include <stdexcept>
4 
6 
7 /* Navigation data blocs */
41 
42 /* Extended navigation data blocs */
50 
51 /* External data blocs */
66 
67 using namespace boost::asio;
68 
69 namespace ixblue_stdbin_decoder
70 {
71 
72 StdBinDecoder::StdBinDecoder()
73  : navigationParsers(
74  {std::make_shared<Parser::AttitudeHeading>(),
75  std::make_shared<Parser::AttitudeHeadingDeviation>(),
76  std::make_shared<Parser::RealTimeHeaveSurgeSway>(),
77  std::make_shared<Parser::SmartHeave>(),
78  std::make_shared<Parser::HeadingRollPitchRate>(),
79  std::make_shared<Parser::RotationRateVesselFrame>(),
80  std::make_shared<Parser::AccelerationVesselFrame>(),
81  std::make_shared<Parser::Position>(),
82  std::make_shared<Parser::PositionDeviation>(),
83  std::make_shared<Parser::SpeedGeographicFrame>(),
84  std::make_shared<Parser::SpeedGeographicFrameDeviation>(),
85  std::make_shared<Parser::CurrentGeographicFrame>(),
86  std::make_shared<Parser::CurrentGeographicFrameDeviation>(),
87  std::make_shared<Parser::SystemDate>(),
88  std::make_shared<Parser::SensorStatus>(),
89  std::make_shared<Parser::INSAlgorithmStatus>(),
90  std::make_shared<Parser::INSSystemStatus>(),
91  std::make_shared<Parser::INSUserStatus>(),
92  std::make_shared<Parser::AHRSAlgorithmStatus>(),
93  std::make_shared<Parser::AHRSSystemStatus>(),
94  std::make_shared<Parser::AHRSUserStatus>(),
95  std::make_shared<Parser::HeaveSurgeSwaySpeed>(),
96  std::make_shared<Parser::SpeedVesselFrame>(),
97  std::make_shared<Parser::AccelerationGeographicFrame>(),
98  std::make_shared<Parser::CourseSpeedoverGround>(),
99  std::make_shared<Parser::Temperatures>(),
100  std::make_shared<Parser::AttitudeQuaternion>(),
101  std::make_shared<Parser::AttitudeQuaternionDeviation>(),
102  std::make_shared<Parser::RawAccelerationVesselFrame>(),
103  std::make_shared<Parser::AccelerationVesselFrameDeviation>(),
104  std::make_shared<Parser::RotationRateVesselFrameDeviation>()},
105  [](const MemoryBlockParserPtr& lhs, const MemoryBlockParserPtr& rhs) -> bool {
106  return lhs->getOffsetInMask() < rhs->getOffsetInMask();
107  }),
109  {std::make_shared<Parser::RotationAccelerationVesselFrame>(),
110  std::make_shared<Parser::RotationAccelerationVesselFrameDeviation>(),
111  std::make_shared<Parser::RawRotationRateVesselFrame>(),
112  std::make_shared<Parser::VehicleAttitudeHeading>(),
113  std::make_shared<Parser::VehiclePosition>(),
114  std::make_shared<Parser::VehiclePositionDeviation>()},
115  [](const MemoryBlockParserPtr& lhs, const MemoryBlockParserPtr& rhs) -> bool {
116  return lhs->getOffsetInMask() < rhs->getOffsetInMask();
117  }),
119  {std::make_shared<Parser::Utc>(),
120  std::make_shared<Parser::Gnss1>(),
121  std::make_shared<Parser::Gnss2>(),
122  std::make_shared<Parser::GnssManual>(),
123  std::make_shared<Parser::Emlog1>(),
124  std::make_shared<Parser::Emlog2>(),
125  std::make_shared<Parser::Depth>(),
126  std::make_shared<Parser::Usbl1>(),
127  std::make_shared<Parser::Usbl2>(),
128  std::make_shared<Parser::Usbl3>(),
129  std::make_shared<Parser::DvlGroundSpeed1>(),
130  std::make_shared<Parser::DvlWaterSpeed1>(),
131  std::make_shared<Parser::SoundVelocity>(),
132  std::make_shared<Parser::Dmi>(),
133  std::make_shared<Parser::Lbl1>(),
134  std::make_shared<Parser::Lbl2>(),
135  std::make_shared<Parser::Lbl3>(),
136  std::make_shared<Parser::Lbl4>(),
137  std::make_shared<Parser::EventMarkerA>(),
138  std::make_shared<Parser::EventMarkerB>(),
139  std::make_shared<Parser::EventMarkerC>(),
140  std::make_shared<Parser::DvlGroundSpeed2>(),
141  std::make_shared<Parser::DvlWaterSpeed2>(),
142  std::make_shared<Parser::TurretAngles>(),
143  std::make_shared<Parser::Vtg1>(),
144  std::make_shared<Parser::Vtg2>(),
145  std::make_shared<Parser::LogBook>()},
146  [](const MemoryBlockParserPtr& lhs, const MemoryBlockParserPtr& rhs) -> bool {
147  return lhs->getOffsetInMask() < rhs->getOffsetInMask();
148  }),
149  internalBuffer{128000}
150 
151 {}
152 
153 void StdBinDecoder::addNewData(const std::vector<uint8_t>& data)
154 {
155  addNewData(data.data(), data.size());
156 }
157 
158 void StdBinDecoder::addNewData(const uint8_t* data, std::size_t length)
159 {
160  internalBuffer.insert(internalBuffer.end(), data, data + length);
161 }
162 
164 {
165  // Now, we will look for version of the received frame and wait to have receive enough
166  // data to parse full header.
168  {
169  return false;
170  }
171 
172  // The call to linearize() can lead to a copy of the buffer content, but once in a
173  // because the buffer is quite large
174  boost::asio::const_buffer buffer(internalBuffer.linearize(), internalBuffer.size());
175  lastHeader = parseHeader(buffer);
176  // if we didn't receive the whole frame, we return false, and wait for the next
177  // memory chunck.
178  if(internalBuffer.size() < lastHeader.telegramSize)
179  {
180  return false;
181  }
182 
183  // Compare checksum before going further (will throw if bad)
184  compareChecksum();
185 
186  if(lastHeader.messageType == Data::NavHeader::MessageType::NavData)
187  {
188  for(const auto& parser : navigationParsers)
189  {
190  parser->parse(buffer, lastHeader.navigationBitMask, lastParsed);
191  }
192 
193  if(lastHeader.extendedNavigationBitMask.is_initialized())
194  {
195  for(const auto& parser : extendedNavigationParsers)
196  {
197  parser->parse(buffer, lastHeader.extendedNavigationBitMask.get(),
198  lastParsed);
199  }
200  }
201 
202  for(const auto& parser : externalDataParsers)
203  {
204  parser->parse(buffer, lastHeader.externalSensorBitMask, lastParsed);
205  }
206  }
207  else if(lastHeader.messageType == Data::NavHeader::MessageType::Answer)
208  {
209  lastAnswer.clear();
210  const size_t answerSize =
211  lastHeader.telegramSize - ANSWER_HEADER_SIZE - CHECKSUM_SIZE;
212  lastAnswer.resize(answerSize);
213  buffer_copy(boost::asio::buffer(lastAnswer), buffer, answerSize);
214  buffer = buffer + answerSize;
215  }
216 
217  // Remove the parsed telegram from the buffer
218  internalBuffer.erase_begin(lastHeader.telegramSize);
219  return true;
220 }
221 
223 {
224  while(internalBuffer.size() > 3)
225  {
226  const uint8_t protocol_version = internalBuffer[2];
227  if(internalBuffer[0] == 'I' && internalBuffer[1] == 'X')
228  {
229  switch(protocol_version)
230  {
231  case 0x02: return internalBuffer.size() >= HEADER_SIZE_V2;
232  case 0x03: return internalBuffer.size() >= HEADER_SIZE_V3;
233  case 0x04: return internalBuffer.size() >= HEADER_SIZE_V4;
234  case 0x05: return internalBuffer.size() >= HEADER_SIZE_V5;
235  default: throw std::runtime_error("Unhandled protocol version");
236  }
237  }
238  else if(internalBuffer[0] == 'A' && internalBuffer[1] == 'N')
239  {
240  if(protocol_version >= 3 && protocol_version <= 5)
241  {
242  return internalBuffer.size() >= ANSWER_HEADER_SIZE;
243  }
244  else
245  {
246  throw std::runtime_error("Unhandled protocol version for an answer");
247  }
248  }
249  else
250  {
251  // No valid header found, pop one byte at the front of the buffer and try
252  // again
253  internalBuffer.pop_front();
254  }
255  }
256  return false;
257 }
258 
260 {
261  // Create a new buffer to start from start of frame
262  // This call to linearize() is free because the buffer as been linearized just before
263  boost::asio::const_buffer buffer(internalBuffer.linearize(), internalBuffer.size());
264  const std::size_t checksumPositionInFrame = lastHeader.telegramSize - CHECKSUM_SIZE;
265  buffer = buffer + checksumPositionInFrame;
266  uint32_t receivedChecksum = 0;
267  buffer >> receivedChecksum;
268 
269  const uint32_t computedChecksum = std::accumulate(
270  internalBuffer.begin(), internalBuffer.begin() + checksumPositionInFrame, 0);
271 
272  if(receivedChecksum != computedChecksum)
273  {
274  std::ostringstream ss;
275  ss << "Bad checksum. Received: 0x" << std::hex << receivedChecksum
276  << ", computed: 0x" << computedChecksum;
277  throw std::runtime_error(ss.str());
278  }
279 }
280 
281 Data::NavHeader StdBinDecoder::parseHeader(const_buffer& buffer) const
282 {
283  // We know we have enough bytes to parse the whole header because it had been
284  // checked before.
285  static constexpr size_t HEADER_MINIMAL_SIZE = 3;
286 
287  Data::NavHeader res;
288  if(buffer_size(buffer) < HEADER_MINIMAL_SIZE)
289  {
290  throw std::runtime_error("Not enough bytes in buffer to parse header");
291  }
292 
293  res.messageType = getHeaderType(buffer);
295  {
296  throw std::runtime_error("Incorrect frame header, expected 'I', 'X' or 'A', 'N'");
297  }
298 
299  buffer >> res.protocolVersion;
300  if(res.protocolVersion < 2 || res.protocolVersion > 5)
301  {
302  throw std::runtime_error(
303  "Unknown protocol version. Supported protocol are version 2->5");
304  }
305 
307  {
308  buffer >> res.navigationBitMask;
309  if(res.protocolVersion > 2)
310  {
311  uint32_t extendedNavigationMask;
312  buffer >> extendedNavigationMask;
313  res.extendedNavigationBitMask = extendedNavigationMask;
314  }
315  buffer >> res.externalSensorBitMask;
316  uint16_t navigationSize = 0;
317  if(res.protocolVersion > 3)
318  {
319  buffer >> navigationSize;
320  }
321  buffer >> res.telegramSize;
322  buffer >> res.navigationDataValidityTime_100us;
323  uint32_t counter;
324  buffer >> counter;
325  }
326  else
327  {
328  buffer >> res.telegramSize;
329  }
330  return res;
331 }
332 
334 {
335  // We already checked the buffer size before calling this method.
336  uint8_t h1, h2;
337  buffer >> h1;
338  buffer >> h2;
339 
340  if(h1 == 'I' && h2 == 'X')
341  {
343  }
344 
345  if(h1 == 'A' && h2 == 'N')
346  {
348  }
349 
351 }
352 
353 } // namespace ixblue_stdbin_decoder
static constexpr size_t HEADER_SIZE_V2
boost::circular_buffer< uint8_t > internalBuffer
void addNewData(const uint8_t *data, std::size_t length)
Add new binary data to the parser internal buffer The new data can only be a part of a frame...
static constexpr size_t HEADER_SIZE_V5
boost::optional< uint32_t > extendedNavigationBitMask
Definition: nav_header.h:22
std::shared_ptr< MemoryBlockParser > MemoryBlockParserPtr
void compareChecksum()
Compute current frame checksum and compare with the frame checksum. If mismatch, throw std::runtime_e...
bool parseNextFrame()
Try to parse a frame from the parser internal buffer. Some binary data must have been added with the ...
static constexpr size_t ANSWER_HEADER_SIZE
static constexpr size_t HEADER_SIZE_V3
static constexpr size_t CHECKSUM_SIZE
Data::NavHeader::MessageType getHeaderType(boost::asio::const_buffer &buffer) const
static constexpr size_t HEADER_SIZE_V4
Data::NavHeader parseHeader(boost::asio::const_buffer &buffer) const


ixblue_stdbin_decoder
Author(s): Adrien BARRAL , Laure LEBROTON
autogenerated on Sat Jan 9 2021 03:13:21