Merge branch 'develop' of github.com:ROBOTIS-GIT/Dynamixel2Arduino into develop

Author: ROBOTIS-Will

src/utility/config.h

@@ -33,13 +33,15 @@
 #define ENABLE_ACTUATOR_PRO_RA          1
 #define ENABLE_ACTUATOR_PRO_PLUS        1
 
+#define DXL_BYTE_STUFF_SAFE_CNT         8
+
 
 #if defined (ARDUINO_AVR_UNO) || defined (ARDUINO_AVR_YUN) \
   || defined (ARDUINO_AVR_INDUSTRIAL101)
 #define DEFAULT_DXL_BUF_LENGTH       192
 #elif defined (ARDUINO_AVR_LEONARDO)
 #define DEFAULT_DXL_BUF_LENGTH       256
-#elif defined (OpenCR)
+#elif defined (__OPENCR__)
 #define DEFAULT_DXL_BUF_LENGTH       2048
 #else
 #define DEFAULT_DXL_BUF_LENGTH       1024
@@ -60,7 +62,7 @@
 #elif defined (ARDUINO_AVR_LEONARDO)
 #define DXL_MAX_NODE                   16
 #define DXL_MAX_NODE_BUFFER_SIZE       12
-#elif defined (OpenCR)
+#elif defined (__OPENCR__)
 #define DXL_MAX_NODE                  253 // Max number of XEL on DYNAMIXEL protocol
 #define DXL_MAX_NODE_BUFFER_SIZE       32
 #else

src/utility/slave.cpp

@@ -24,7 +24,7 @@ Slave::Slave(DXLPortHandler &port, const uint16_t model_num, float protocol_ver)
   setPortProtocolVersion(protocol_ver);
   addDefaultControlItem();
 
-  p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH];
+  p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH + DXL_BYTE_STUFF_SAFE_CNT];
   if(p_packet_buf_ != nullptr){
     packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH;
     is_buf_malloced_ = true;
@@ -42,7 +42,7 @@ Slave::Slave(const uint16_t model_num, float protocol_ver)
   setPortProtocolVersion(protocol_ver);
   addDefaultControlItem();
 
-  p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH];
+  p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH + DXL_BYTE_STUFF_SAFE_CNT];
   if(p_packet_buf_ != nullptr){
     packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH;
     is_buf_malloced_ = true;
@@ -432,7 +432,7 @@ Slave::processInstRead()
         if(addr_length+11 > packet_buf_capacity_){
           err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
         }
-        p_tx_param = &p_packet_buf_[9];
+        p_tx_param = &p_packet_buf_[9 + DXL_BYTE_STUFF_SAFE_CNT];
       }else{
         err = DXL_LIB_ERROR_WRONG_PACKET;
       }
@@ -443,7 +443,7 @@ Slave::processInstRead()
         if(addr_length+6 > packet_buf_capacity_){
           err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
         }
-        p_tx_param = &p_packet_buf_[5];
+        p_tx_param = &p_packet_buf_[5 + DXL_BYTE_STUFF_SAFE_CNT];
       }else{
         err = DXL_LIB_ERROR_WRONG_PACKET;
       }