diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
index db8f3390ec70e38f5a488c2551fa357d3053c67a..3fb16778cc9fb565b29d476ed72e241754166ddc 100644
--- a/src/dynamixel_slave_uart_dma.c
+++ b/src/dynamixel_slave_uart_dma.c
@@ -48,7 +48,8 @@
 // private variables
 uint8_t dyn_slave_address;// this module slave address
 uint8_t dyn_slave_return_level;// type of response
-uint8_t dyn_slave_return_delay;// delay in th response
+uint8_t dyn_slave_return_delay;// delay in the response
+TDynVersion dyn_slave_version;// version of the dynamixel packet being received
 // input buffer
 uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
 // output buffer
@@ -66,6 +67,7 @@ DMA_InitTypeDef  DYN_SLAVE_DMA_RX_InitStructure;
 // interrupt handlers
 void DYN_SLAVE_IRQHandler(void)
 {
+  static uint16_t v2_length=0;
   static uint8_t num_bytes=0;
   uint8_t data;
 
@@ -88,18 +90,50 @@ void DYN_SLAVE_IRQHandler(void)
               }
               else num_bytes--;
               break;
-      case 2: if(data!=0xFF)
+      case 2: if(data==0xFD)// version 2 packet
               {
                 dyn_slave_rx_buffer[num_bytes]=data;
+                dyn_slave_version=DYN_VER2;
+                num_bytes++;
+              }
+              else if(data!=0xFF)// version 1 packet
+              {
+                dyn_slave_rx_buffer[num_bytes]=data;
+                dyn_slave_version=DYN_VER1;
                 num_bytes++;
               }
               break;
       case 3: dyn_slave_rx_buffer[num_bytes]=data;
+              if(dyn_slave_version==DYN_VER1)
+              {
+                num_bytes=0;
+                /* disable USART RX interrupts */
+                USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,DISABLE);
+                /* enable DMA RX */
+                DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
+                DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = data;
+                DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
+                DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
+                USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+              }
+              else
+                num_bytes++;
+              break;
+      case 4: dyn_slave_rx_buffer[num_bytes]=data;
+              num_bytes++;
+              break;
+      case 5: dyn_slave_rx_buffer[num_bytes]=data;
+              v2_length=data;
+              num_bytes++;
+              break;
+      case 6: dyn_slave_rx_buffer[num_bytes]=data;
+              v2_length+=(data<<8);
               num_bytes=0;
               /* disable USART RX interrupts */
               USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,DISABLE);
               /* enable DMA RX */
-              DMA_SetCurrDataCounter(DYN_SLAVE_RX_DMA_STREAM, data);
+              DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[7];
+              DMA_SetCurrDataCounter(DYN_SLAVE_RX_DMA_STREAM,v2_length);
               DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
               USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
               break;
@@ -170,6 +204,10 @@ void dyn_slave_init(void)
   // initialize the flags
   dyn_slave_packet_ready=0x00;
   dyn_slave_sending_packet=0x00;
+  dyn_slave_address=0x01;// this module slave address
+  dyn_slave_return_level=0x00;// type of response
+  dyn_slave_return_delay=0x00;// delay in the response
+  dyn_slave_version=DYN_VER1;// version 1
 
   USART_DeInit(DYN_SLAVE);
   USART_StructInit(&USART_InitStructure);
@@ -307,23 +345,43 @@ uint8_t dyn_slave_is_packet_ready(void)
   return dyn_slave_packet_ready;
 }
 
-void dyn_slave_get_inst_packet(uint8_t *packet)
+TDynVersion dyn_slave_get_inst_packet(uint8_t *packet)
 {
   uint8_t i;
 
-  for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
-    packet[i]=dyn_slave_rx_buffer[i];
+  if(dyn_slave_version==DYN_VER1)
+  {
+    for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
+      packet[i]=dyn_slave_rx_buffer[i];
+  }
+  else
+  {
+    for(i=0;i<dyn2_get_length(dyn_slave_rx_buffer)+7;i++)
+      packet[i]=dyn_slave_rx_buffer[i];
+  }
   dyn_slave_packet_ready=0x00;
+
+  return dyn_slave_version;
 }
 
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
+void dyn_slave_send_status_packet(uint8_t error,uint16_t length, uint8_t *data)
 {
   // wait until the previous transmission has ended (if any)
   while(dyn_slave_sending_packet==0x01);
-  // create the status packet
-  dyn_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
-  // set the DMA transfer
-  DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
+  if(dyn_slave_version==DYN_VER1)
+  {
+    // create the status packet
+    dyn_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
+    // set the DMA transfer
+    DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
+  }
+  else
+  {
+    // create the status packet
+    dyn2_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
+    // set the DMA transfer
+    DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+8);
+  }
   DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,ENABLE);
   USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
   USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);