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



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020