21 #include "direct_com_message.h"
23 #include <core/exception.h>
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;
36 const unsigned int DirectRobotinoComMessage::MSG_METADATA_SIZE = 5;
50 unsigned int received,
53 :
Exception(
"Checksum verification error for Robotino message, "
54 "expected %u, got %u (%02x %02x)",
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_;
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_);
117 escaped_data_ = NULL;
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();
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_) {
154 throw Exception(
"Failed to allocate more memory");
156 escaped_data_size_ = escaped_consumed;
163 DirectRobotinoComMessage::ctor()
168 data_ = (
unsigned char *)malloc(data_size_);
169 memset(data_, 0, data_size_);
171 cur_data_ = data_ + 3;
174 escaped_data_ = NULL;
183 data_size_ = payload_size_ = 0;
185 ::free(escaped_data_);
201 ::free(escaped_data_);
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_;
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_);
218 escaped_data_ = NULL;
229 DirectRobotinoComMessage::assert_mode(mode_t mode)
const
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");
242 DirectRobotinoComMessage::assert_command()
const
245 throw Exception(
"No command has been opened for reading (call next_command)");
254 DirectRobotinoComMessage::assert_command_data(uint8_t size)
const
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)",
259 cur_cmd_ + cur_cmd_[1] + 2,
272 DirectRobotinoComMessage::inc_payload_by(uint16_t count)
276 throw Exception(
"Must add command before values");
279 if (payload_size_ + count >= data_size_ - MSG_METADATA_SIZE) {
281 unsigned char *old_data = data_;
283 data_ = (
unsigned char *)realloc(data_, data_size_);
286 throw Exception(
"Failed to allocate more memory");
289 payload_size_ += count;
290 cur_cmd_[1] += count;
302 cur_cmd_ = cur_data_;
306 cur_cmd_[0] = 0xff & cmdid;
318 *(cur_data_++) = 0xFF & value;
329 *(cur_data_++) = 0xFF & value;
340 *(cur_data_++) = 0xFF & value;
341 *(cur_data_++) = value >> 8;
352 *(cur_data_++) = 0xFF & value;
353 *(cur_data_++) = value >> 8;
364 *(cur_data_++) = 0xFF & value;
365 *(cur_data_++) = 0xFF & (value >> 8);
366 *(cur_data_++) = 0xFF & (value >> 16);
367 *(cur_data_++) = 0xFF & (value >> 24);
378 *(cur_data_++) = 0xFF & value;
379 *(cur_data_++) = 0xFF & (value >> 8);
380 *(cur_data_++) = 0xFF & (value >> 16);
381 *(cur_data_++) = 0xFF & (value >> 24);
392 const char *p =
reinterpret_cast<const char *
>(&value);
394 for (
int i = 0; i < 4; ++i) {
395 *(cur_data_++) = *(p++);
406 cur_data_ = data_ + 3;
413 DirectRobotinoComMessage::command_id_t
417 if (cur_cmd_ == NULL && payload_size_ >= 2) {
419 cur_cmd_ = &data_[3];
420 cur_data_ = cur_cmd_ + 2;
421 return (command_id_t)cur_cmd_[0];
423 && ((data_ + payload_size_ + MSG_METADATA_SIZE - 2) - (cur_cmd_ + cur_cmd_[1] + 2))
426 cur_cmd_ += cur_cmd_[1] + 2;
427 cur_data_ = cur_cmd_ + 2;
428 return (command_id_t)cur_cmd_[0];
448 DirectRobotinoComMessage::command_id_t
453 return (command_id_t)cur_cmd_[0];
466 assert_command_data(1);
468 int8_t value = (int8_t)cur_data_[0];
483 assert_command_data(1);
485 uint8_t value = (uint8_t)cur_data_[0];
500 assert_command_data(2);
502 int16_t value = (uint8_t)cur_data_[0];
503 value |= ((int16_t)cur_data_[1] << 8);
518 assert_command_data(2);
520 uint16_t value = (uint8_t)cur_data_[0];
521 uint16_t h = (uint8_t)cur_data_[1];
535 uint16_t value = (uint8_t)buf[0];
536 uint16_t h = (uint8_t)buf[1];
551 assert_command_data(4);
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];
574 assert_command_data(4);
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];
597 assert_command_data(4);
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];
619 assert_command_data(1);
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;
636 assert_command_data(1);
650 assert_command_data(1);
664 assert_command_data(2);
678 assert_command_data(2);
692 assert_command_data(4);
706 assert_command_data(4);
720 assert_command_data(4);
735 return escaped_data_size_;
744 return payload_size_;
758 DirectRobotinoComMessage::escape()
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) {
767 ::free(escaped_data_);
768 escaped_data_size_ = payload_size_ + MSG_METADATA_SIZE + to_escape;
769 escaped_data_ = (
unsigned char *)malloc(escaped_data_size_);
772 escaped_data_[0] = MSG_HEAD;
773 unsigned char *p = escaped_data_;
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;
784 memcpy(escaped_data_, data_, escaped_data_size_);
800 size_t unescaped_size,
801 const unsigned char *escaped,
804 if (unescaped_size == 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");
813 unescaped[j++] = escaped[i + 1] ^ MSG_DATA_MANGLE;
816 unescaped[j++] = escaped[i];
818 if (j == unescaped_size)
822 throw Exception(
"Not enough escaped bytes for unescaping");
829 DirectRobotinoComMessage::unescape_data()
831 if (!escaped_data_ || escaped_data_size_ == 0) {
832 throw Exception(
"No escaped data to unescape");
835 if (data_size_ < 3) {
836 unsigned char *old_data = data_;
837 data_ = (
unsigned char *)realloc(data_, 3);
840 throw Exception(
"Failed to allocate more memory");
845 size_t consumed_bytes =
unescape(&data_[1], 2, &escaped_data_[1], escaped_data_size_ - 1) + 1;
848 if (data_size_ < unescaped_size + 3) {
849 unsigned char *old_data = data_;
850 data_ = (
unsigned char *)realloc(data_, unescaped_size + 3);
853 throw Exception(
"Failed to allocate more memory");
855 data_size_ = unescaped_size + 3;
857 payload_size_ = unescaped_size - 2;
859 consumed_bytes +=
unescape(&data_[3],
861 &escaped_data_[consumed_bytes],
862 escaped_data_size_ - consumed_bytes);
864 return consumed_bytes;
873 boost::asio::const_buffer
877 return boost::asio::buffer(escaped_data_, escaped_data_size_);
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;
903 const unsigned char *p = &data_[1];
905 for (
unsigned int i = 0; i < payload_size_ + (MSG_METADATA_SIZE - 3); ++i) {
909 return 0xffff & ((1 << 16) - rv);
917 DirectRobotinoComMessage::check_checksum()
const
920 uint16_t packet_checksum =
parse_uint16(&data_[payload_size_ + 3]);
921 if (checksum_v != packet_checksum) {
922 throw ChecksumError(checksum_v,
924 data_[payload_size_ + 3],
925 data_[payload_size_ + 4]);
937 boost::asio::const_buffer b;
938 const unsigned char * bp;
942 bp = boost::asio::buffer_cast<const unsigned char *>(b);
943 bsize = boost::asio::buffer_size(b);
946 bsize = payload_size_ + MSG_METADATA_SIZE;
957 for (
unsigned int i = 0; i < bsize; ++i) {
958 bool cmd_opened =
false;
959 bool cmd_closed =
false;
961 if (i == 3 && bsize > MSG_METADATA_SIZE) {
965 }
else if (i > 3 && cmd_l >= 0 && cmd_i == cmd_l) {
969 }
else if (i > 3 && cmd_l < 0 && bsize - i > 2) {
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 ?
")" :
" ");
983 snprintf(tmp, 8,
"%s%02x%s", cmd_opened ?
"(" :
" ", bp[i], cmd_closed ?
")" :
" ");
987 if ((bsize - 1) % 16 != 0) {
ChecksumError(unsigned int expected, unsigned int received, unsigned char byte1, unsigned char byte2)
Constructor.
Robotino communication message.
uint16_t checksum() const
Get checksum of message.
int32_t get_int32()
Get 32-bit signed integer from current command.
std::string to_string(bool escaped=false)
Generate string representation of message.
size_t escaped_data_size()
Size of escaped buffer.
void add_uint16(uint16_t value)
Add 16-bit unsigned integer to current command.
void rewind()
Rewind to read again from start.
boost::asio::const_buffer buffer()
Get access to buffer for sending.
int8_t get_int8()
Get 8-bit signed integer from current command.
void add_command(command_id_t cmdid)
Add a command header.
void skip_float()
Skip float from current command.
uint8_t get_uint8()
Get 8-bit unsigned integer from current command.
void add_uint32(uint32_t value)
Add 32-bit unsigned integer to current command.
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.
void skip_int32()
Skip 32-bit signed integer from current command.
void skip_uint16()
Skip 16-bit unsigned integer from current command.
void add_uint8(uint8_t value)
Add 8-bit unsigned integer to current command.
command_id_t next_command()
Get next available command.
void add_int16(int16_t value)
Add 16-bit signed integer to current command.
void skip_int16()
Skip 16-bit signed integer from current command.
void skip_uint32()
Skip 32-bit unsigned integer from current command.
int16_t get_int16()
Get 16-bit signed integer from current command.
static uint16_t parse_uint16(const unsigned char *buf)
Parse 16-bit unsigned integer from given buffer.
size_t payload_size()
Get payload size.
size_t data_size()
Get internal data buffer size.
uint16_t get_uint16()
Get 16-bit unsigned integer from current command.
uint8_t command_length() const
Get length of current command.
void add_float(float value)
Add float to current command.
uint32_t get_uint32()
Get 32-bit unsigned integer from current command.
float get_float()
Get float from current command.
virtual ~DirectRobotinoComMessage()
Destructor.
void skip_uint8()
Skip 8-bit unsigned integer from current command.
DirectRobotinoComMessage & operator=(const DirectRobotinoComMessage &other)
Assignment operator.
void add_int8(int8_t value)
Add 8-bit signed integer to current command.
void add_int32(int32_t value)
Add 32-bit signed integer to current command.
std::string get_string()
Get string from current command.
void skip_int8()
Skip 8-bit signed integer from current command.
DirectRobotinoComMessage()
Constructor.
command_id_t command_id() const
Get ID of current command.
Base class for exceptions in Fawkes.
Fawkes library namespace.