Fawkes API  Fawkes Development Version
direct_com_message.cpp
1 /***************************************************************************
2  * direct_com_message.cpp - Message for RobotinoDirectThread
3  *
4  * Created: Mon Apr 04 15:48:52 2016
5  * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "direct_com_message.h"
22 
23 #include <core/exception.h>
24 
25 #include <cstdlib>
26 #include <iomanip>
27 
28 using namespace fawkes;
29 
30 /// @cond INTERNAL
31 const unsigned char DirectRobotinoComMessage::MSG_HEAD = 0xAA;
32 const unsigned char DirectRobotinoComMessage::MSG_DATA_ESCAPE = 0x55;
33 const unsigned char DirectRobotinoComMessage::MSG_DATA_MANGLE = 0x20;
34 
35 // 5: 0xAA + payload_size/2 ... + checksum/2
36 const unsigned int DirectRobotinoComMessage::MSG_METADATA_SIZE = 5;
37 /// @endcond INTERNAL
38 
39 /** @class DirectRobotinoComMessage::ChecksumError "direct_com_message.h"
40  * Excpetion thrown on checksum errors.
41  */
42 
43 /** Constructor.
44  * @param expected expected checksum
45  * @param received actually received checksm
46  * @param byte1 First byte of actually received checksum
47  * @param byte2 Second byte of actually received checksum
48  */
50  unsigned int received,
51  unsigned char byte1,
52  unsigned char byte2)
53 : Exception("Checksum verification error for Robotino message, "
54  "expected %u, got %u (%02x %02x)",
55  expected,
56  received,
57  byte1,
58  byte2)
59 {
60 }
61 
62 /** @class DirectRobotinoComMessage "direct_com_message.h"
63  * Robotino communication message.
64  *
65  * This object is used to create messages to send and parse messages
66  * to read. It is designed to be generic, i.e., it provides methods to
67  * add messages and its fields and to iterate over commands and read
68  * fields. These methods must be called in proper sequence, no command
69  * type specific processing is performed within the class. This
70  * approach was chosen since we do not strive for a user-facing
71  * generic transport, but rather for a minimal method to interact with
72  * Robotino's microcontroller.
73  *
74  * A message strictly differentiates between a reading and a writing
75  * mode, depending on the constructor with which it was created. The
76  * reading mode is used to parse received messages and provide access
77  * to its commands, the writing mode to construct messages to send.
78  * Furthermore, the message assumes quick run-throughs, i.e., after
79  * requesting the buffer of a writing message once, it is fixed and
80  * will not be re-generated after further additions.
81  *
82  * Terminology is a mix in part due to the naming in OpenRobotino:
83  * - Message: our representation of a message to send
84  * - Command: a command field within a message, this is called tag
85  * and also command in OpenRobotino. We chose the latter.
86  *
87  * @author Tim Niemueller
88  */
89 
90 /** Constructor.
91  * Create empty message for writing.
92  */
94 {
95  ctor();
96 }
97 
98 /** Copy Constructor.
99  * @param other instance to copy from
100  */
102 {
103  mode_ = other.mode_;
104 
105  payload_size_ = other.payload_size_;
106  data_size_ = other.data_size_;
107  data_ = (unsigned char *)malloc(data_size_);
108  memcpy(data_, other.data_, data_size_);
109  cur_data_ = other.cur_data_;
110  cur_cmd_ = other.cur_cmd_;
111 
112  if (other.escaped_data_) {
113  escaped_data_size_ = other.escaped_data_size_;
114  escaped_data_ = (unsigned char *)malloc(escaped_data_size_);
115  memcpy(escaped_data_, other.escaped_data_, escaped_data_size_);
116  } else {
117  escaped_data_ = NULL;
118  }
119 }
120 
121 /** Constructor for initial command.
122  * Create message for writing and add command for given message ID.
123  * @param cmdid message ID of command to add
124  */
126 {
127  ctor();
128 
129  add_command(cmdid);
130 }
131 
132 /** Constructor for incoming message.
133  * Create message for reading from incoming buffer.
134  * @param msg the message of \p msg_size is expected to be escaped and to range from
135  * the including 0xAA head byte to the checksum.
136  * @param msg_size size of \p msg buffer
137  */
138 DirectRobotinoComMessage::DirectRobotinoComMessage(const unsigned char *msg, size_t msg_size)
139 {
140  ctor();
141 
142  mode_ = READ;
143 
144  escaped_data_ = (unsigned char *)malloc(msg_size);
145  memcpy(escaped_data_, msg, msg_size);
146  escaped_data_size_ = msg_size;
147  size_t escaped_consumed = unescape_data();
148 
149  if (escaped_consumed < msg_size) {
150  unsigned char *old_data = escaped_data_;
151  escaped_data_ = (unsigned char *)realloc(escaped_data_, escaped_consumed);
152  if (!escaped_data_) {
153  free(old_data);
154  throw Exception("Failed to allocate more memory");
155  }
156  escaped_data_size_ = escaped_consumed;
157  }
158 
159  check_checksum();
160 }
161 
162 void
163 DirectRobotinoComMessage::ctor()
164 {
165  payload_size_ = 0;
166  // always allocate 128 bytes, increase if necessary
167  data_size_ = 128;
168  data_ = (unsigned char *)malloc(data_size_);
169  memset(data_, 0, data_size_);
170  data_[0] = MSG_HEAD;
171  cur_data_ = data_ + 3;
172  cur_cmd_ = NULL;
173 
174  escaped_data_ = NULL;
175 
176  mode_ = WRITE;
177 }
178 
179 /** Destructor. */
181 {
182  ::free(data_);
183  data_size_ = payload_size_ = 0;
184  if (escaped_data_)
185  ::free(escaped_data_);
186  cur_data_ = NULL;
187 }
188 
189 /** Assignment operator.
190  * @param other instance to copy from
191  * @return reference to this instance
192  */
195 {
196  if (&other == this)
197  return *this;
198 
199  ::free(data_);
200  if (escaped_data_) {
201  ::free(escaped_data_);
202  }
203 
204  mode_ = other.mode_;
205 
206  payload_size_ = other.payload_size_;
207  data_size_ = other.data_size_;
208  data_ = (unsigned char *)malloc(data_size_);
209  memcpy(data_, other.data_, data_size_);
210  cur_data_ = other.cur_data_;
211  cur_cmd_ = other.cur_cmd_;
212 
213  if (other.escaped_data_) {
214  escaped_data_size_ = other.escaped_data_size_;
215  escaped_data_ = (unsigned char *)malloc(escaped_data_size_);
216  memcpy(escaped_data_, other.escaped_data_, escaped_data_size_);
217  } else {
218  escaped_data_ = NULL;
219  }
220 
221  return *this;
222 }
223 
224 /** Assert a given message mode.
225  * @param mode mode
226  * @throw Exception on mode mismatch
227  */
228 void
229 DirectRobotinoComMessage::assert_mode(mode_t mode) const
230 {
231  if (mode_ == WRITE && mode == READ) {
232  throw Exception("Message mode is writing, but requested reading operation");
233  } else if (mode_ == READ && mode == WRITE) {
234  throw Exception("Message mode is reading, but requested writing operation");
235  }
236 }
237 
238 /** Assert that a command has been selected.
239  * @throw Exception if no command opened
240  */
241 void
242 DirectRobotinoComMessage::assert_command() const
243 {
244  if (!cur_cmd_) {
245  throw Exception("No command has been opened for reading (call next_command)");
246  }
247 }
248 
249 /** Assert minimum available data size.
250  * @param size number of bytes yet to be read
251  * @throw Exception if less than \p size bytes are available
252  */
253 void
254 DirectRobotinoComMessage::assert_command_data(uint8_t size) const
255 {
256  if (payload_size_ < size || cur_data_ + size > cur_cmd_ + cur_cmd_[1] + 2) {
257  throw Exception("Cannot read beyond command length %x %x (%x + %u >= %x + %u + 2)",
258  cur_data_ + size,
259  cur_cmd_ + cur_cmd_[1] + 2,
260  cur_data_,
261  size,
262  cur_cmd_,
263  cur_cmd_[1]);
264  }
265 }
266 
267 /** Increase payload by a number of bytes.
268  * This may reallocate the memory to hold the data if it exceeds the current size.
269  * @param count number of bytes to extend payload by
270  */
271 void
272 DirectRobotinoComMessage::inc_payload_by(uint16_t count)
273 {
274  assert_mode(WRITE);
275  if (!cur_cmd_) {
276  throw Exception("Must add command before values");
277  }
278 
279  if (payload_size_ + count >= data_size_ - MSG_METADATA_SIZE) {
280  // need to realloc for more data
281  unsigned char *old_data = data_;
282  data_size_ += 128;
283  data_ = (unsigned char *)realloc(data_, data_size_);
284  if (!data_) {
285  free(old_data);
286  throw Exception("Failed to allocate more memory");
287  }
288  }
289  payload_size_ += count;
290  cur_cmd_[1] += count;
291 }
292 
293 /** Add a command header.
294  * This only allocates the header. You must call the appropriate methods to
295  * add the required data fields afterwards or the message will be
296  * rejected/ignored by the Robotino.
297  * @param cmdid command ID to add.
298  */
299 void
301 {
302  cur_cmd_ = cur_data_;
303  cur_data_ += 2;
304  inc_payload_by(2);
305 
306  cur_cmd_[0] = 0xff & cmdid;
307  cur_cmd_[1] = 0;
308 }
309 
310 /** Add 8-bit signed integer to current command.
311  * @param value value to add
312  * @throw Exception thrown if no command has been added, yet
313  */
314 void
316 {
317  inc_payload_by(1);
318  *(cur_data_++) = 0xFF & value;
319 }
320 
321 /** Add 8-bit unsigned integer to current command.
322  * @param value value to add
323  * @throw Exception thrown if no command has been added, yet
324  */
325 void
327 {
328  inc_payload_by(1);
329  *(cur_data_++) = 0xFF & value;
330 }
331 
332 /** Add 16-bit signed integer to current command.
333  * @param value value to add
334  * @throw Exception thrown if no command has been added, yet
335  */
336 void
338 {
339  inc_payload_by(2);
340  *(cur_data_++) = 0xFF & value;
341  *(cur_data_++) = value >> 8;
342 }
343 
344 /** Add 16-bit unsigned integer to current command.
345  * @param value value to add
346  * @throw Exception thrown if no command has been added, yet
347  */
348 void
350 {
351  inc_payload_by(2);
352  *(cur_data_++) = 0xFF & value;
353  *(cur_data_++) = value >> 8;
354 }
355 
356 /** Add 32-bit signed integer to current command.
357  * @param value value to add
358  * @throw Exception thrown if no command has been added, yet
359  */
360 void
362 {
363  inc_payload_by(4);
364  *(cur_data_++) = 0xFF & value;
365  *(cur_data_++) = 0xFF & (value >> 8);
366  *(cur_data_++) = 0xFF & (value >> 16);
367  *(cur_data_++) = 0xFF & (value >> 24);
368 }
369 
370 /** Add 32-bit unsigned integer to current command.
371  * @param value value to add
372  * @throw Exception thrown if no command has been added, yet
373  */
374 void
376 {
377  inc_payload_by(4);
378  *(cur_data_++) = 0xFF & value;
379  *(cur_data_++) = 0xFF & (value >> 8);
380  *(cur_data_++) = 0xFF & (value >> 16);
381  *(cur_data_++) = 0xFF & (value >> 24);
382 }
383 
384 /** Add float to current command.
385  * @param value value to add
386  * @throw Exception thrown if no command has been added, yet
387  */
388 void
390 {
391  inc_payload_by(4);
392  const char *p = reinterpret_cast<const char *>(&value);
393 
394  for (int i = 0; i < 4; ++i) {
395  *(cur_data_++) = *(p++);
396  }
397 }
398 
399 /** Rewind to read again from start.
400  * @throw Exception thrown if no not a reading message
401  */
402 void
404 {
405  assert_mode(READ);
406  cur_data_ = data_ + 3;
407  cur_cmd_ = NULL;
408 }
409 
410 /** Get next available command.
411  * @return ID of next command, or CMDID_NONE if no more commands available
412  */
413 DirectRobotinoComMessage::command_id_t
415 {
416  assert_mode(READ);
417  if (cur_cmd_ == NULL && payload_size_ >= 2) {
418  // no command set but payload that could hold one
419  cur_cmd_ = &data_[3];
420  cur_data_ = cur_cmd_ + 2;
421  return (command_id_t)cur_cmd_[0];
422  } else if (cur_cmd_
423  && ((data_ + payload_size_ + MSG_METADATA_SIZE - 2) - (cur_cmd_ + cur_cmd_[1] + 2))
424  >= 2) {
425  // we have a command and it does not extend beyond the payload, -2: subtract length of checksum
426  cur_cmd_ += cur_cmd_[1] + 2;
427  cur_data_ = cur_cmd_ + 2;
428  return (command_id_t)cur_cmd_[0];
429  } else {
430  return CMDID_NONE;
431  }
432 }
433 
434 /** Get length of current command.
435  * @return length in bytes
436  */
437 uint8_t
439 {
440  assert_mode(READ);
441  assert_command();
442  return cur_cmd_[1];
443 }
444 
445 /** Get ID of current command.
446  * @return command ID
447  */
448 DirectRobotinoComMessage::command_id_t
450 {
451  assert_mode(READ);
452  assert_command();
453  return (command_id_t)cur_cmd_[0];
454 }
455 
456 /** Get 8-bit signed integer from current command.
457  * This also forwards the command-internal pointer appropriately.
458  * @return value
459  * @throw Exception thrown if not enough data remains to be read
460  */
461 int8_t
463 {
464  assert_mode(READ);
465  assert_command();
466  assert_command_data(1);
467 
468  int8_t value = (int8_t)cur_data_[0];
469  cur_data_ += 1;
470  return value;
471 }
472 
473 /** Get 8-bit unsigned integer from current command.
474  * This also forwards the command-internal pointer appropriately.
475  * @return value
476  * @throw Exception thrown if not enough data remains to be read
477  */
478 uint8_t
480 {
481  assert_mode(READ);
482  assert_command();
483  assert_command_data(1);
484 
485  uint8_t value = (uint8_t)cur_data_[0];
486  cur_data_ += 1;
487  return value;
488 }
489 
490 /** Get 16-bit signed integer from current command.
491  * This also forwards the command-internal pointer appropriately.
492  * @return value
493  * @throw Exception thrown if not enough data remains to be read
494  */
495 int16_t
497 {
498  assert_mode(READ);
499  assert_command();
500  assert_command_data(2);
501 
502  int16_t value = (uint8_t)cur_data_[0];
503  value |= ((int16_t)cur_data_[1] << 8);
504  cur_data_ += 2;
505  return value;
506 }
507 
508 /** Get 16-bit unsigned integer from current command.
509  * This also forwards the command-internal pointer appropriately.
510  * @return value
511  * @throw Exception thrown if not enough data remains to be read
512  */
513 uint16_t
515 {
516  assert_mode(READ);
517  assert_command();
518  assert_command_data(2);
519 
520  uint16_t value = (uint8_t)cur_data_[0];
521  uint16_t h = (uint8_t)cur_data_[1];
522  value |= (h << 8);
523  cur_data_ += 2;
524  return value;
525 }
526 
527 /** Parse 16-bit unsigned integer from given buffer.
528  * @param buf buffer at least of size 2 bytes
529  * @return value
530  * @throw Exception thrown if not enough data remains to be read
531  */
532 uint16_t
533 DirectRobotinoComMessage::parse_uint16(const unsigned char *buf)
534 {
535  uint16_t value = (uint8_t)buf[0];
536  uint16_t h = (uint8_t)buf[1];
537  value |= (h << 8);
538  return value;
539 }
540 
541 /** Get 32-bit signed integer from current command.
542  * This also forwards the command-internal pointer appropriately.
543  * @return value
544  * @throw Exception thrown if not enough data remains to be read
545  */
546 int32_t
548 {
549  assert_mode(READ);
550  assert_command();
551  assert_command_data(4);
552 
553  int32_t value = (uint8_t)cur_data_[0];
554  int32_t h1 = (uint8_t)cur_data_[1];
555  int32_t h2 = (uint8_t)cur_data_[2];
556  int32_t h3 = (uint8_t)cur_data_[3];
557  value |= (h1 << 8);
558  value |= (h2 << 16);
559  value |= (h3 << 24);
560  cur_data_ += 4;
561  return value;
562 }
563 
564 /** Get 32-bit unsigned integer from current command.
565  * This also forwards the command-internal pointer appropriately.
566  * @return value
567  * @throw Exception thrown if not enough data remains to be read
568  */
569 uint32_t
571 {
572  assert_mode(READ);
573  assert_command();
574  assert_command_data(4);
575 
576  uint32_t value = (uint8_t)cur_data_[0];
577  uint32_t h1 = (uint8_t)cur_data_[1];
578  uint32_t h2 = (uint8_t)cur_data_[2];
579  uint32_t h3 = (uint8_t)cur_data_[3];
580  value |= (h1 << 8);
581  value |= (h2 << 16);
582  value |= (h3 << 24);
583  cur_data_ += 4;
584  return value;
585 }
586 
587 /** Get float from current command.
588  * This also forwards the command-internal pointer appropriately.
589  * @return value
590  * @throw Exception thrown if not enough data remains to be read
591  */
592 float
594 {
595  assert_mode(READ);
596  assert_command();
597  assert_command_data(4);
598 
599  float value;
600  char *p = reinterpret_cast<char *>(&value);
601  *(p++) = cur_data_[0];
602  *(p++) = cur_data_[1];
603  *(p++) = cur_data_[2];
604  *(p++) = cur_data_[3];
605  cur_data_ += 4;
606  return value;
607 }
608 
609 /** Get string from current command.
610  * This consumes all remaining data in the current command.
611  * @return value
612  * @throw Exception thrown if no data remains to be read
613  */
614 std::string
616 {
617  assert_mode(READ);
618  assert_command();
619  assert_command_data(1);
620 
621  size_t remaining = (cur_cmd_ + cur_cmd_[1] + 2) - cur_data_;
622  std::string value((const char *)cur_data_, remaining);
623  cur_data_ += remaining;
624  return value;
625 }
626 
627 /** Skip 8-bit signed integer from current command.
628  * This also forwards the command-internal pointer appropriately.
629  * @throw Exception thrown if not enough data remains to be read
630  */
631 void
633 {
634  assert_mode(READ);
635  assert_command();
636  assert_command_data(1);
637 
638  cur_data_ += 1;
639 }
640 
641 /** Skip 8-bit unsigned integer from current command.
642  * This also forwards the command-internal pointer appropriately.
643  * @throw Exception thrown if not enough data remains to be read
644  */
645 void
647 {
648  assert_mode(READ);
649  assert_command();
650  assert_command_data(1);
651 
652  cur_data_ += 1;
653 }
654 
655 /** Skip 16-bit signed integer from current command.
656  * This also forwards the command-internal pointer appropriately.
657  * @throw Exception thrown if not enough data remains to be read
658  */
659 void
661 {
662  assert_mode(READ);
663  assert_command();
664  assert_command_data(2);
665 
666  cur_data_ += 2;
667 }
668 
669 /** Skip 16-bit unsigned integer from current command.
670  * This also forwards the command-internal pointer appropriately.
671  * @throw Exception thrown if not enough data remains to be read
672  */
673 void
675 {
676  assert_mode(READ);
677  assert_command();
678  assert_command_data(2);
679 
680  cur_data_ += 2;
681 }
682 
683 /** Skip 32-bit signed integer from current command.
684  * This also forwards the command-internal pointer appropriately.
685  * @throw Exception thrown if not enough data remains to be read
686  */
687 void
689 {
690  assert_mode(READ);
691  assert_command();
692  assert_command_data(4);
693 
694  cur_data_ += 4;
695 }
696 
697 /** Skip 32-bit unsigned integer from current command.
698  * This also forwards the command-internal pointer appropriately.
699  * @throw Exception thrown if not enough data remains to be read
700  */
701 void
703 {
704  assert_mode(READ);
705  assert_command();
706  assert_command_data(4);
707 
708  cur_data_ += 4;
709 }
710 
711 /** Skip float from current command.
712  * This also forwards the command-internal pointer appropriately.
713  * @throw Exception thrown if not enough data remains to be read
714  */
715 void
717 {
718  assert_mode(READ);
719  assert_command();
720  assert_command_data(4);
721 
722  cur_data_ += 4;
723 }
724 
725 /** Size of escaped buffer.
726  * In particular, after calling the parsing ctor this denotes how many
727  * bytes of the input buffer have been consumed.
728  * On message creation, only provides meaningful values after calling
729  * pack() or buffer().
730  * @return size of escaped buffer
731  */
732 size_t
734 {
735  return escaped_data_size_;
736 }
737 
738 /** Get payload size.
739  * @return payload size
740  */
741 size_t
743 {
744  return payload_size_;
745 }
746 
747 /** Get internal data buffer size.
748  * @return data buffer size
749  */
750 size_t
752 {
753  return data_size_;
754 }
755 
756 /** Perform message escaping. */
757 void
758 DirectRobotinoComMessage::escape()
759 {
760  unsigned short to_escape = 0;
761  for (unsigned int i = 1; i < payload_size_ + MSG_METADATA_SIZE; ++i) {
762  if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
763  ++to_escape;
764  }
765  }
766  if (escaped_data_)
767  ::free(escaped_data_);
768  escaped_data_size_ = payload_size_ + MSG_METADATA_SIZE + to_escape;
769  escaped_data_ = (unsigned char *)malloc(escaped_data_size_);
770 
771  if (to_escape > 0) {
772  escaped_data_[0] = MSG_HEAD;
773  unsigned char *p = escaped_data_;
774  *p++ = MSG_HEAD;
775  for (unsigned int i = 1; i < payload_size_ + MSG_METADATA_SIZE; ++i) {
776  if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
777  *p++ = MSG_DATA_ESCAPE;
778  *p++ = data_[i] ^ MSG_DATA_MANGLE;
779  } else {
780  *p++ = data_[i];
781  }
782  }
783  } else {
784  memcpy(escaped_data_, data_, escaped_data_size_);
785  }
786 }
787 
788 /** Unescape a number of unescaped bytes.
789  * @param unescaped buffer to contain the unescaped data on return,
790  * must be at least of \p unescaped_size number of bytes
791  * @param unescaped_size expected number of bytes to unescape
792  * from input buffer
793  * @param escaped escaped buffer to process
794  * @param escaped_size size of escaped_buffer
795  * @return number of bytes consumed from escaped buffer
796  * @throw Exception if not enough bytes could be unescaped
797  */
798 size_t
799 DirectRobotinoComMessage::unescape(unsigned char * unescaped,
800  size_t unescaped_size,
801  const unsigned char *escaped,
802  size_t escaped_size)
803 {
804  if (unescaped_size == 0)
805  return 0;
806 
807  unsigned int j = 0;
808  for (unsigned int i = 0; i < escaped_size; ++i) {
809  if (escaped[i] == MSG_DATA_ESCAPE) {
810  if (i >= escaped_size - 1) {
811  throw Exception("Read escaped byte last in message");
812  }
813  unescaped[j++] = escaped[i + 1] ^ MSG_DATA_MANGLE;
814  i += 1;
815  } else {
816  unescaped[j++] = escaped[i];
817  }
818  if (j == unescaped_size)
819  return i + 1;
820  }
821 
822  throw Exception("Not enough escaped bytes for unescaping");
823 }
824 
825 /** Unescape all data.
826  * @return number of bytes consumed from escaped buffer
827  */
828 size_t
829 DirectRobotinoComMessage::unescape_data()
830 {
831  if (!escaped_data_ || escaped_data_size_ == 0) {
832  throw Exception("No escaped data to unescape");
833  }
834 
835  if (data_size_ < 3) {
836  unsigned char *old_data = data_;
837  data_ = (unsigned char *)realloc(data_, 3);
838  if (!data_) {
839  free(old_data);
840  throw Exception("Failed to allocate more memory");
841  }
842  data_[0] = MSG_HEAD;
843  }
844  // +1: HEAD
845  size_t consumed_bytes = unescape(&data_[1], 2, &escaped_data_[1], escaped_data_size_ - 1) + 1;
846  size_t unescaped_size = parse_uint16(&data_[1]) + 2; // +2: checksum
847 
848  if (data_size_ < unescaped_size + 3) {
849  unsigned char *old_data = data_;
850  data_ = (unsigned char *)realloc(data_, unescaped_size + 3); // +3: HEAD, LENGTH
851  if (!data_) {
852  free(old_data);
853  throw Exception("Failed to allocate more memory");
854  }
855  data_size_ = unescaped_size + 3;
856  }
857  payload_size_ = unescaped_size - 2; // -2: no checksum
858 
859  consumed_bytes += unescape(&data_[3],
860  unescaped_size,
861  &escaped_data_[consumed_bytes],
862  escaped_data_size_ - consumed_bytes);
863 
864  return consumed_bytes;
865 }
866 
867 /** Get access to buffer for sending.
868  * This implies packing. Note that after calling this methods later
869  * modifications to the message will be ignored. The buffer is invalidated
870  * on the destruction of the message.
871  * @return buffer of escaped data.
872  */
873 boost::asio::const_buffer
875 {
876  pack();
877  return boost::asio::buffer(escaped_data_, escaped_data_size_);
878 }
879 
880 /** Pack message.
881  * This escapes the data to be sent.
882  */
883 void
885 {
886  if (!escaped_data_) {
887  data_[1] = 0xff & payload_size_;
888  data_[2] = payload_size_ >> 8;
889  unsigned short checksum_value = checksum();
890  data_[payload_size_ + 3] = 0xff & checksum_value;
891  data_[payload_size_ + 4] = checksum_value >> 8;
892  escape();
893  }
894 }
895 
896 /** Get checksum of message.
897  * @return checksum
898  */
899 uint16_t
901 {
902  uint16_t rv = 0;
903  const unsigned char *p = &data_[1];
904  // -3: do not include the 0xAA header and checksum
905  for (unsigned int i = 0; i < payload_size_ + (MSG_METADATA_SIZE - 3); ++i) {
906  rv += (uint8_t)*p;
907  ++p;
908  }
909  return 0xffff & ((1 << 16) - rv);
910 }
911 
912 /** Check the checksum.
913  * This is for a parsed message to verify that the checksum in the received
914  * messages matches the self-calculated one.
915  */
916 void
917 DirectRobotinoComMessage::check_checksum() const
918 {
919  uint16_t checksum_v = checksum();
920  uint16_t packet_checksum = parse_uint16(&data_[payload_size_ + 3]);
921  if (checksum_v != packet_checksum) {
922  throw ChecksumError(checksum_v,
923  packet_checksum,
924  data_[payload_size_ + 3],
925  data_[payload_size_ + 4]);
926  }
927 }
928 
929 /** Generate string representation of message.
930  * @param escaped true to serialize escaped buffer, false to use unescaped buffer
931  * @return string representation of message in hexadecimal view with parantheses
932  * to indicate command boundaries.
933  */
934 std::string
936 {
937  boost::asio::const_buffer b;
938  const unsigned char * bp;
939  size_t bsize;
940  if (escaped) {
941  b = buffer();
942  bp = boost::asio::buffer_cast<const unsigned char *>(b);
943  bsize = boost::asio::buffer_size(b);
944  } else {
945  bp = data_;
946  bsize = payload_size_ + MSG_METADATA_SIZE;
947  }
948 
949  std::string rv;
950  // this buffer must hold and hex representation of a byte with whitespace
951  char tmp[8];
952 
953  // used for adding braces to indicate commands
954  int16_t cmd_l = -1;
955  int16_t cmd_i = 0;
956 
957  for (unsigned int i = 0; i < bsize; ++i) {
958  bool cmd_opened = false;
959  bool cmd_closed = false;
960  if (!escaped) {
961  if (i == 3 && bsize > MSG_METADATA_SIZE) {
962  cmd_l = bp[i + 1];
963  cmd_i = 0;
964  cmd_opened = true;
965  } else if (i > 3 && cmd_l >= 0 && cmd_i == cmd_l) {
966  cmd_closed = true;
967  cmd_l = -1;
968  cmd_i = 0;
969  } else if (i > 3 && cmd_l < 0 && bsize - i > 2) {
970  cmd_opened = true;
971  cmd_l = bp[i + 1];
972  cmd_i = 0;
973  } else {
974  ++cmd_i;
975  }
976  }
977 
978  if (i > 0 && (i + 1) % 16 == 0) {
979  snprintf(tmp, 8, "%s%02x%s\n", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
980  } else if (i > 0 && (i + 1) % 8 == 0) {
981  snprintf(tmp, 8, "%s%02x%s ", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
982  } else {
983  snprintf(tmp, 8, "%s%02x%s", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
984  }
985  rv += tmp;
986  }
987  if ((bsize - 1) % 16 != 0) {
988  rv += "\n";
989  }
990 
991  return rv;
992 }
DirectRobotinoComMessage::skip_float
void skip_float()
Skip float from current command.
Definition: direct_com_message.cpp:716
DirectRobotinoComMessage::skip_int8
void skip_int8()
Skip 8-bit signed integer from current command.
Definition: direct_com_message.cpp:632
DirectRobotinoComMessage::command_length
uint8_t command_length() const
Get length of current command.
Definition: direct_com_message.cpp:438
DirectRobotinoComMessage::ChecksumError::ChecksumError
ChecksumError(unsigned int expected, unsigned int received, unsigned char byte1, unsigned char byte2)
Constructor.
Definition: direct_com_message.cpp:49
DirectRobotinoComMessage::get_int32
int32_t get_int32()
Get 32-bit signed integer from current command.
Definition: direct_com_message.cpp:547
DirectRobotinoComMessage::get_uint32
uint32_t get_uint32()
Get 32-bit unsigned integer from current command.
Definition: direct_com_message.cpp:570
DirectRobotinoComMessage
Robotino communication message.
Definition: direct_com_message.h:30
DirectRobotinoComMessage::DirectRobotinoComMessage
DirectRobotinoComMessage()
Constructor.
Definition: direct_com_message.cpp:93
DirectRobotinoComMessage::skip_uint16
void skip_uint16()
Skip 16-bit unsigned integer from current command.
Definition: direct_com_message.cpp:674
DirectRobotinoComMessage::pack
void pack()
Pack message.
Definition: direct_com_message.cpp:884
DirectRobotinoComMessage::buffer
boost::asio::const_buffer buffer()
Get access to buffer for sending.
Definition: direct_com_message.cpp:874
DirectRobotinoComMessage::~DirectRobotinoComMessage
virtual ~DirectRobotinoComMessage()
Destructor.
Definition: direct_com_message.cpp:180
DirectRobotinoComMessage::get_float
float get_float()
Get float from current command.
Definition: direct_com_message.cpp:593
DirectRobotinoComMessage::next_command
command_id_t next_command()
Get next available command.
Definition: direct_com_message.cpp:414
DirectRobotinoComMessage::parse_uint16
static uint16_t parse_uint16(const unsigned char *buf)
Parse 16-bit unsigned integer from given buffer.
Definition: direct_com_message.cpp:533
DirectRobotinoComMessage::get_string
std::string get_string()
Get string from current command.
Definition: direct_com_message.cpp:615
DirectRobotinoComMessage::get_int8
int8_t get_int8()
Get 8-bit signed integer from current command.
Definition: direct_com_message.cpp:462
DirectRobotinoComMessage::rewind
void rewind()
Rewind to read again from start.
Definition: direct_com_message.cpp:403
DirectRobotinoComMessage::checksum
uint16_t checksum() const
Get checksum of message.
Definition: direct_com_message.cpp:900
DirectRobotinoComMessage::get_int16
int16_t get_int16()
Get 16-bit signed integer from current command.
Definition: direct_com_message.cpp:496
fawkes
Fawkes library namespace.
DirectRobotinoComMessage::skip_int32
void skip_int32()
Skip 32-bit signed integer from current command.
Definition: direct_com_message.cpp:688
DirectRobotinoComMessage::add_int8
void add_int8(int8_t value)
Add 8-bit signed integer to current command.
Definition: direct_com_message.cpp:315
DirectRobotinoComMessage::data_size
size_t data_size()
Get internal data buffer size.
Definition: direct_com_message.cpp:751
DirectRobotinoComMessage::operator=
DirectRobotinoComMessage & operator=(const DirectRobotinoComMessage &other)
Assignment operator.
Definition: direct_com_message.cpp:194
DirectRobotinoComMessage::to_string
std::string to_string(bool escaped=false)
Generate string representation of message.
Definition: direct_com_message.cpp:935
DirectRobotinoComMessage::skip_uint8
void skip_uint8()
Skip 8-bit unsigned integer from current command.
Definition: direct_com_message.cpp:646
DirectRobotinoComMessage::add_command
void add_command(command_id_t cmdid)
Add a command header.
Definition: direct_com_message.cpp:300
DirectRobotinoComMessage::escaped_data_size
size_t escaped_data_size()
Size of escaped buffer.
Definition: direct_com_message.cpp:733
DirectRobotinoComMessage::get_uint8
uint8_t get_uint8()
Get 8-bit unsigned integer from current command.
Definition: direct_com_message.cpp:479
DirectRobotinoComMessage::skip_int16
void skip_int16()
Skip 16-bit signed integer from current command.
Definition: direct_com_message.cpp:660
DirectRobotinoComMessage::unescape
static size_t unescape(unsigned char *unescaped, size_t unescaped_size, const unsigned char *escaped, size_t escaped_size)
Unescape a number of unescaped bytes.
Definition: direct_com_message.cpp:799
DirectRobotinoComMessage::add_float
void add_float(float value)
Add float to current command.
Definition: direct_com_message.cpp:389
DirectRobotinoComMessage::get_uint16
uint16_t get_uint16()
Get 16-bit unsigned integer from current command.
Definition: direct_com_message.cpp:514
DirectRobotinoComMessage::payload_size
size_t payload_size()
Get payload size.
Definition: direct_com_message.cpp:742
DirectRobotinoComMessage::add_int32
void add_int32(int32_t value)
Add 32-bit signed integer to current command.
Definition: direct_com_message.cpp:361
DirectRobotinoComMessage::command_id
command_id_t command_id() const
Get ID of current command.
Definition: direct_com_message.cpp:449
DirectRobotinoComMessage::add_uint8
void add_uint8(uint8_t value)
Add 8-bit unsigned integer to current command.
Definition: direct_com_message.cpp:326
DirectRobotinoComMessage::add_uint32
void add_uint32(uint32_t value)
Add 32-bit unsigned integer to current command.
Definition: direct_com_message.cpp:375
DirectRobotinoComMessage::skip_uint32
void skip_uint32()
Skip 32-bit unsigned integer from current command.
Definition: direct_com_message.cpp:702
DirectRobotinoComMessage::add_int16
void add_int16(int16_t value)
Add 16-bit signed integer to current command.
Definition: direct_com_message.cpp:337
DirectRobotinoComMessage::add_uint16
void add_uint16(uint16_t value)
Add 16-bit unsigned integer to current command.
Definition: direct_com_message.cpp:349
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36