21 #include "direct_com_message.h" 23 #include <core/exception.h> 30 const unsigned char DirectRobotinoComMessage::MSG_HEAD = 0xAA;
31 const unsigned char DirectRobotinoComMessage::MSG_DATA_ESCAPE = 0x55;
32 const unsigned char DirectRobotinoComMessage::MSG_DATA_MANGLE = 0x20;
35 const unsigned int DirectRobotinoComMessage::MSG_METADATA_SIZE = 5;
49 unsigned char byte1,
unsigned char byte2)
50 :
Exception(
"Checksum verification error for Robotino message, " 51 "expected %u, got %u (%02x %02x)", expected, received, byte1, byte2)
115 escaped_data_ = (
unsigned char *)malloc(msg_size);
116 memcpy(escaped_data_, msg, msg_size);
117 escaped_data_size_ = msg_size;
118 size_t escaped_consumed = unescape_data();
120 if (escaped_consumed < msg_size) {
121 escaped_data_ = (
unsigned char *)realloc(escaped_data_, escaped_consumed);
122 escaped_data_size_ = escaped_consumed;
129 DirectRobotinoComMessage::ctor()
134 data_ = (
unsigned char *)malloc(data_size_);
135 memset(data_, 0, data_size_);
137 cur_data_ = data_ + 3;
140 escaped_data_ = NULL;
149 data_size_ = payload_size_ = 0;
150 if (escaped_data_) ::free(escaped_data_);
160 DirectRobotinoComMessage::assert_mode(mode_t mode)
const 162 if (mode_ == WRITE && mode == READ) {
163 throw Exception(
"Message mode is writing, but requested reading operation");
164 }
else if (mode_ == READ && mode == WRITE) {
165 throw Exception(
"Message mode is reading, but requested writing operation");
174 DirectRobotinoComMessage::assert_command()
const 177 throw Exception(
"No command has been opened for reading (call next_command)");
186 DirectRobotinoComMessage::assert_command_data(uint8_t size)
const 188 if (payload_size_ < size || cur_data_ + size > cur_cmd_ + cur_cmd_[1] + 2) {
189 throw Exception(
"Cannot read beyond command length %x %x (%x + %u >= %x + %u + 2)",
190 cur_data_ + size, cur_cmd_ + cur_cmd_[1] + 2, cur_data_, size, cur_cmd_, cur_cmd_[1]);
199 DirectRobotinoComMessage::inc_payload_by(uint16_t count)
203 throw Exception(
"Must add command before values");
206 if (payload_size_ + count >= data_size_ - MSG_METADATA_SIZE) {
208 data_ = (
unsigned char *)realloc(data_, data_size_ + 128);
210 payload_size_ += count;
211 cur_cmd_[1] += count;
224 cur_cmd_ = cur_data_;
228 cur_cmd_[0] = 0xff & cmdid;
240 *(cur_data_++) = 0xFF & value;
251 *(cur_data_++) = 0xFF & value;
262 *(cur_data_++) = 0xFF & value;
263 *(cur_data_++) = value >> 8;
274 *(cur_data_++) = 0xFF & value;
275 *(cur_data_++) = value >> 8;
286 *(cur_data_++) = 0xFF & value;
287 *(cur_data_++) = 0xFF & ( value >> 8 );
288 *(cur_data_++) = 0xFF & ( value >> 16 );
289 *(cur_data_++) = 0xFF & ( value >> 24 );
300 *(cur_data_++) = 0xFF & value;
301 *(cur_data_++) = 0xFF & ( value >> 8 );
302 *(cur_data_++) = 0xFF & ( value >> 16 );
303 *(cur_data_++) = 0xFF & ( value >> 24 );
314 const char* p =
reinterpret_cast<const char*
>( &value );
316 for(
int i = 0; i < 4; ++i) {
317 *(cur_data_++) = *(p++);
328 cur_data_ = data_ + 3;
335 DirectRobotinoComMessage::command_id_t
339 if (cur_cmd_ == NULL && payload_size_ >= 2) {
341 cur_cmd_ = &data_[3];
342 cur_data_ = cur_cmd_ + 2;
343 return (command_id_t)cur_cmd_[0];
344 }
else if (cur_cmd_ && ((data_ + payload_size_ + MSG_METADATA_SIZE - 2) - (cur_cmd_ + cur_cmd_[1] + 2)) >= 2) {
346 cur_cmd_ += cur_cmd_[1] + 2;
347 cur_data_ = cur_cmd_ + 2;
348 return (command_id_t)cur_cmd_[0];
370 DirectRobotinoComMessage::command_id_t
375 return (command_id_t)cur_cmd_[0];
388 assert_command_data(1);
390 int8_t value = (int8_t)cur_data_[0];
405 assert_command_data(1);
407 uint8_t value = (uint8_t)cur_data_[0];
422 assert_command_data(2);
424 int16_t value = (uint8_t)cur_data_[0];
425 value |= ( (int16_t)cur_data_[1] << 8 );
440 assert_command_data(2);
442 uint16_t value = (uint8_t)cur_data_[0];
443 uint16_t h = (uint8_t)cur_data_[1];
457 uint16_t value = (uint8_t)buf[0];
458 uint16_t h = (uint8_t)buf[1];
473 assert_command_data(4);
475 int32_t value = (uint8_t)cur_data_[0];
476 int32_t h1 = (uint8_t)cur_data_[1];
477 int32_t h2 = (uint8_t)cur_data_[2];
478 int32_t h3 = (uint8_t)cur_data_[3];
496 assert_command_data(4);
498 uint32_t value = (uint8_t)cur_data_[0];
499 uint32_t h1 = (uint8_t)cur_data_[1];
500 uint32_t h2 = (uint8_t)cur_data_[2];
501 uint32_t h3 = (uint8_t)cur_data_[3];
519 assert_command_data(4);
522 char* p =
reinterpret_cast<char*
>( &value );
523 *(p++) = cur_data_[0];
524 *(p++) = cur_data_[1];
525 *(p++) = cur_data_[2];
526 *(p++) = cur_data_[3];
541 assert_command_data(1);
543 size_t remaining = (cur_cmd_ + cur_cmd_[1] + 2) - cur_data_;
544 std::string value((
const char *)cur_data_, remaining);
545 cur_data_ += remaining;
559 assert_command_data(1);
573 assert_command_data(1);
587 assert_command_data(2);
601 assert_command_data(2);
615 assert_command_data(4);
629 assert_command_data(4);
643 assert_command_data(4);
659 return escaped_data_size_;
669 return payload_size_;
683 DirectRobotinoComMessage::escape()
685 unsigned short to_escape = 0;
686 for (
int i = 1; i < payload_size_ + 4; ++i) {
687 if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
691 if (escaped_data_) ::free(escaped_data_);
692 escaped_data_size_ = payload_size_ + MSG_METADATA_SIZE + to_escape;
693 escaped_data_ = (
unsigned char *)malloc(escaped_data_size_);
696 escaped_data_[0] = MSG_HEAD;
697 unsigned char *p = escaped_data_;
699 for (
unsigned int i = 1; i < payload_size_ + (MSG_METADATA_SIZE-1); ++i) {
700 if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
701 *p++ = MSG_DATA_ESCAPE;
702 *p++ = data_[i] ^ MSG_DATA_MANGLE;
708 memcpy(escaped_data_, data_, escaped_data_size_);
724 const unsigned char *escaped,
size_t escaped_size)
726 if (unescaped_size == 0)
return 0;
729 for (
unsigned int i = 0; i < escaped_size; ++i) {
730 if (escaped[i] == MSG_DATA_ESCAPE) {
731 if (i >= escaped_size - 1) {
732 throw Exception(
"Read escaped byte last in message");
734 unescaped[j++] = escaped[i+1] ^ MSG_DATA_MANGLE;
737 unescaped[j++] = escaped[i];
739 if (j == unescaped_size)
return i+1;
742 throw Exception(
"Not enough escaped bytes for unescaping");
749 DirectRobotinoComMessage::unescape_data()
751 if (! escaped_data_ || escaped_data_size_ == 0) {
752 throw Exception(
"No escaped data to unescape");
755 if (data_size_ < 3) {
756 data_ = (
unsigned char *)realloc(data_, 3);
760 size_t consumed_bytes =
unescape(&data_[1], 2, &escaped_data_[1], escaped_data_size_-1) + 1;
763 if (data_size_ < unescaped_size + 3) {
764 data_ = (
unsigned char *)realloc(data_, unescaped_size + 3);
765 data_size_ = unescaped_size + 3;
767 payload_size_ = unescaped_size - 2;
771 &escaped_data_[consumed_bytes], escaped_data_size_ - consumed_bytes);
773 return consumed_bytes;
783 boost::asio::const_buffer
787 return boost::asio::buffer(escaped_data_, escaped_data_size_);
796 if (! escaped_data_) {
797 data_[1] = 0xff & payload_size_;
798 data_[2] = payload_size_ >> 8;
799 unsigned short checksum_value =
checksum();
800 data_[payload_size_ + 3] = 0xff & checksum_value;
801 data_[payload_size_ + 4] = checksum_value >> 8;
813 const unsigned char* p = &data_[1];
815 for (
unsigned int i = 0; i < payload_size_ + (MSG_METADATA_SIZE-3); ++i) {
819 return 0xffff & ( (1<<16) - rv );
827 DirectRobotinoComMessage::check_checksum()
const 830 uint16_t packet_checksum =
parse_uint16(&data_[payload_size_ + 3]);
831 if (checksum_v != packet_checksum) {
833 data_[payload_size_ + 3], data_[payload_size_ + 4]);
845 boost::asio::const_buffer b;
846 const unsigned char *bp;
850 bp = boost::asio::buffer_cast<
const unsigned char*>(b);
851 bsize = boost::asio::buffer_size(b);
854 bsize = payload_size_ + MSG_METADATA_SIZE;
865 for (
unsigned int i = 0; i < bsize; ++i) {
866 bool cmd_opened =
false;
867 bool cmd_closed =
false;
869 if (i == 3 && bsize > MSG_METADATA_SIZE) {
873 }
else if (i > 3 && cmd_l >= 0 && cmd_i == cmd_l) {
877 }
else if (i > 3 && cmd_l < 0 && bsize - i > 2) {
886 if (i > 0 && (i+1) % 16 == 0) {
887 snprintf(tmp, 8,
"%s%02x%s\n", cmd_opened ?
"(" :
" ", bp[i], cmd_closed ?
")" :
" ");
888 }
else if (i > 0 && (i+1) % 8 == 0) {
889 snprintf(tmp, 8,
"%s%02x%s ", cmd_opened ?
"(" :
" ", bp[i], cmd_closed ?
")" :
" ");
891 snprintf(tmp, 8,
"%s%02x%s", cmd_opened ?
"(" :
" ", bp[i], cmd_closed ?
")" :
" ");
895 if ((bsize-1) % 16 != 0) {
void add_uint32(uint32_t value)
Add 32-bit unsigned integer to current command.
int8_t get_int8()
Get 8-bit signed integer from current command.
void add_uint16(uint16_t value)
Add 16-bit unsigned integer to current command.
void add_command(command_id_t cmdid)
Add a command header.
ChecksumError(unsigned int expected, unsigned int received, unsigned char byte1, unsigned char byte2)
Constructor.
DirectRobotinoComMessage()
Constructor.
void add_uint8(uint8_t value)
Add 8-bit unsigned integer to current command.
void skip_uint16()
Skip 16-bit unsigned integer from current command.
void skip_int8()
Skip 8-bit signed integer from current command.
Fawkes library namespace.
std::string to_string(bool escaped=false)
Generate string representation of message.
Exception()
Constructor for subclasses.
void rewind()
Rewind to read again from start.
std::string get_string()
Get string from current command.
void skip_float()
Skip float from current command.
virtual ~DirectRobotinoComMessage()
Destructor.
float get_float()
Get float from current command.
boost::asio::const_buffer buffer()
Get access to buffer for sending.
size_t data_size()
Get internal data buffer size.
size_t payload_size()
Get payload size.
void add_int16(int16_t value)
Add 16-bit signed integer to current command.
Base class for exceptions in Fawkes.
void add_int32(int32_t value)
Add 32-bit signed integer to current command.
void skip_int32()
Skip 32-bit signed integer from current command.
void skip_int16()
Skip 16-bit signed integer from current command.
uint32_t get_uint32()
Get 32-bit unsigned integer from current command.
uint16_t get_uint16()
Get 16-bit unsigned integer from current command.
int32_t get_int32()
Get 32-bit signed integer from current command.
command_id_t next_command()
Get next available command.
static uint16_t parse_uint16(const unsigned char *buf)
Parse 16-bit unsigned integer from given buffer.
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_uint8()
Skip 8-bit unsigned integer from current command.
uint16_t checksum() const
Get checksum of message.
command_id_t command_id() const
Get ID of current command.
void skip_uint32()
Skip 32-bit unsigned integer from current command.
void add_int8(int8_t value)
Add 8-bit signed integer to current command.
int16_t get_int16()
Get 16-bit signed integer from current command.
uint8_t get_uint8()
Get 8-bit unsigned integer from current command.
size_t escaped_data_size()
Size of escaped buffer.
void add_float(float value)
Add float to current command.
uint8_t command_length() const
Get length of current command.