MRPT  1.9.9
protocolhandler.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 #include "protocolhandler.h"
10 #include <xsens/xsmessage.h>
11 #include <xsens/xsresultvalue.h>
12 #include <iomanip>
13 
14 /*! \class ProtocolHandler
15  \brief Message protocol handling class
16 
17  This class' purpose is to get valid messages according to its protocol from
18  the raw
19  data that is supplied to it. The default implementation (ProtocolHandler)
20  implements
21  the Xsens message protocol. To use a different protocol, overload the
22  findMessage function.
23 
24  The class is intended to be state-less with respect to the data it handles.
25 */
26 
27 //! Default constructor
29 //! Destructor
31 /*! \brief Compute the expected message size given a possibly incomplete message
32  \details When analyzing a message with incomplete size information, the
33  function
34  will return the minimum size of the message given the information that is
35  available
36 */
37 int expectedMessageSize(const unsigned char* buffer, int sz)
38 {
39  const XsMessageHeader* hdr = (const XsMessageHeader*)buffer;
40  if (sz < 4)
41  return XS_LEN_MSGHEADERCS; // no size information available at all,
42  // return a minimum message
43 
44  // we have size information
45  if (hdr->m_length == XS_EXTLENCODE)
46  {
47  if (sz < 6)
48  return XS_EXTLENCODE + XS_LEN_MSGEXTHEADERCS; // typical minimum
49  // size at which
50  // point extended
51  // size is needed
52 
53  return XS_LEN_MSGEXTHEADERCS +
56  }
57  return XS_LEN_MSGHEADERCS + (uint32_t)(hdr->m_length);
58 }
59 
60 /*! \copydoc IProtocolHandler::findMessage
61  \todo Since the assumption is that we receive a stream of valid messages
62  without garbage, the scan
63  is implemented in a rather naive and simple way. If we can expect lots of
64  garbage in the data
65  stream, this should probably be looked into.
66 */
68  XsMessage& rcv, const XsByteArray& raw) const
69 {
70  JLTRACE(gJournal, "Entry");
71  MessageLocation rv(-1, 0);
72  rcv.clear();
73 
74  int bufferSize = (int)raw.size();
75  if (bufferSize == 0) return rv;
76 
77  const unsigned char* buffer = raw.data();
78 
79  // loop through the buffer to find a preamble
80  for (int pre = 0; pre < bufferSize; ++pre)
81  {
82  if (buffer[pre] == XS_PREAMBLE)
83  {
84  JLTRACE(gJournal, "Preamble found at " << pre);
85  // we found a preamble, see if we can read a message from here
86  if (rv.m_startPos == -1) rv.m_startPos = (int32_t)pre;
87  int remaining =
88  bufferSize -
89  pre; // remaining bytes in buffer INCLUDING preamble
90 
91  if (remaining < XS_LEN_MSGHEADERCS)
92  {
93  JLTRACE(gJournal, "Not enough header data read");
94  if (rv.m_startPos != pre) continue;
95  rv.m_size = -expectedMessageSize(&buffer[pre], remaining);
96  return rv;
97  }
98 
99  // read header
100  const uint8_t* msgStart = &(buffer[pre]);
101  const XsMessageHeader* hdr = (const XsMessageHeader*)msgStart;
102  if (hdr->m_length == XS_EXTLENCODE)
103  {
104  if (remaining < XS_LEN_MSGEXTHEADERCS)
105  {
106  JLTRACE(gJournal, "Not enough extended header data read");
107  if (rv.m_startPos != pre) continue;
108  rv.m_size = -expectedMessageSize(&buffer[pre], remaining);
109  return rv;
110  }
111  }
112  else if (hdr->m_busId == 0 && hdr->m_messageId == 0)
113  {
114  // found 'valid' message that isn't actually valid... happens
115  // inside GPS raw data
116  // skip to next preamble
117  continue;
118  }
119 
120  // check the reported size
121  int target = expectedMessageSize(&buffer[pre], remaining);
122 
123  JLTRACE(
124  gJournal, "Bytes in buffer=" << remaining
125  << ", full target = " << target);
126  if (target > (XS_LEN_MSGEXTHEADERCS + XS_MAXDATALEN))
127  {
128  // skip current preamble
129  JLALERT(gJournal, "Invalid message length: " << target);
130  rv.m_startPos = -1;
131  continue;
132  }
133 
134  if (remaining < target)
135  {
136  // not enough data read, skip current preamble
137  JLTRACE(
138  gJournal, "Not enough data read: " << remaining << " / "
139  << target);
140  if (rv.m_size == 0) rv.m_size = -target;
141  continue;
142  }
143 
144  // we have read enough data to fulfill our target so we'll try to
145  // parse the message
146  // and check the checksum
147  // if (rcv->loadFromString(msgStart, (uint16_t) target) == XRV_OK)
148  if (rcv.loadFromString(msgStart, (uint16_t)target))
149  {
150  JLTRACE(
151  gJournal,
152  "OK, size = " << (int)rcv.getTotalMessageSize() << std::hex
153  << std::setfill('0') << " First bytes "
154  << std::setw(2) << (int)msgStart[0] << " "
155  << std::setw(2) << (int)msgStart[1] << " "
156  << std::setw(2) << (int)msgStart[2] << " "
157  << std::setw(2) << (int)msgStart[3] << " "
158  << std::setw(2) << (int)msgStart[4]
159  << std::dec << std::setfill(' '));
160  rv.m_size = (int)rcv.getTotalMessageSize();
161  rv.m_startPos = pre; // we do this again here because this may
162  // not be the first preamble encountered
163  // (the check for -1 at the start of the
164  // loop is necessary)
165  return rv;
166  }
167 
168  // we could not read the message, clear message and try next
169  // preamble
170  rcv.clear();
171  if (rv.m_startPos == pre)
172  {
173  rv.m_startPos = -1;
174  JLALERT(
175  gJournal, "Invalid checksum"
176  << std::hex << std::setfill('0')
177  << " First bytes " << std::setw(2)
178  << (int)msgStart[0] << " " << std::setw(2)
179  << (int)msgStart[1] << " " << std::setw(2)
180  << (int)msgStart[2] << " " << std::setw(2)
181  << (int)msgStart[3] << " " << std::setw(2)
182  << (int)msgStart[4] << std::dec
183  << std::setfill(' '));
184  }
185  }
186  }
187  JLTRACE(gJournal, "Exit");
188  return rv;
189 }
190 /*! \brief Returns the minimum size of a valid message of this protocol
191  * including preambles and checksums */
193 {
194  return XS_LEN_MSGHEADERCS; // minimum size of xsens xbus protocol message
195 }
196 
197 /*! \brief Returns the maximum size of a valid message of this protocol
198  * including preambles and checksums */
200 {
201  return XS_LEN_MSGEXTHEADERCS +
202  XS_MAXDATALEN; // maximum size of xsens xbus protocol message
203 }
204 
205 /*! \brief Compose a message for transmission
206  \param raw The raw byte array to be constructed from the message
207  \param msg The message to translate into a raw byte array
208  \returns The size of the generated byte array
209  \todo Generalize this method -> IProtocolHandler
210 */
212 {
213  if (msg.getTotalMessageSize() <
214  5) // minimum size of an xsens message including envelope is 5 bytes
215  return -1;
216 
217  raw.assign(msg.getTotalMessageSize(), msg.getMessageStart());
218  return (int)raw.size();
219 }
220 
222 {
223  return 0; // XPT_Xbus;
224 }
A message header.
Definition: xsmessage.h:149
union XsMessageHeader::LengthData m_datlen
unsigned __int16 uint16_t
Definition: rptypes.h:44
GLuint buffer
Definition: glext.h:3917
uint8_t m_length
The length of the message.
Definition: xsmessage.h:159
struct XsByteArray XsByteArray
Definition: xsbytearray.h:25
uint8_t m_busId
The bus ID.
Definition: xsmessage.h:154
#define XS_PREAMBLE
Definition: xsmessage.h:114
unsigned char uint8_t
Definition: rptypes.h:41
virtual ~ProtocolHandler()
Destructor.
#define XS_LEN_MSGEXTHEADERCS
Definition: xsmessage.h:120
Stores the location of a message in a buffer using a start position and a size.
Structure for storing a single message.
Definition: xsmessage.h:198
int m_startPos
The offset of the first byte of the message or -1 if no message.
struct XsMessageHeader::LengthData::ExtendedLength::ExtendedParts m_length
virtual MessageLocation findMessage(XsMessage &rcv, const XsByteArray &raw) const
Find the first message in the raw byte stream.
virtual int minimumMessageSize() const
Returns the minimum size of a valid message of this protocol including preambles and checksums...
static int composeMessage(XsByteArray &raw, const XsMessage &msg)
Compose a message for transmission.
virtual int maximumMessageSize() const
Returns the maximum size of a valid message of this protocol including preambles and checksums...
#define XS_MAXDATALEN
Definition: xsmessage.h:127
struct XsMessageHeader::LengthData::ExtendedLength m_extended
__int32 int32_t
Definition: rptypes.h:46
int m_size
The size of the message, when less than 0 it indicates the expected message size. ...
uint8_t m_messageId
The message ID.
Definition: xsmessage.h:156
int expectedMessageSize(const unsigned char *buffer, int sz)
Compute the expected message size given a possibly incomplete message.
uint8_t m_high
High byte of extended length.
Definition: xsmessage.h:172
virtual int type() const
Returns the type of the protocol handler.
#define XS_LEN_MSGHEADERCS
Definition: xsmessage.h:119
ProtocolHandler()
Default constructor.
uint8_t m_low
Low byte of extended length.
Definition: xsmessage.h:174
#define JLTRACE(...)
unsigned __int32 uint32_t
Definition: rptypes.h:47
#define JLALERT(...)
#define XS_EXTLENCODE
Definition: xsmessage.h:115



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020