Modified the whole RX procedure to optimize it.

dynamixel_base/include/dynamixel_slave.h

@@ -50,11 +50,6 @@ typedef struct TDynamixelSlave
    *
    */
   TDynVersion version;
-  /**
-   * \brief
-   *
-   */
-  unsigned char packet_ready;
   /**
    * \brief
    *
@@ -80,11 +75,6 @@ typedef struct TDynamixelSlave
    *
    */
   unsigned char current_rx_buffer;
-  /**
-   * \brief
-   *
-   */
-  unsigned char rx_dma_mode;
   /**
    * \brief
    *
@@ -115,21 +105,6 @@ typedef struct TDynamixelSlave
    *
    */
   unsigned char (*on_relay)(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt);
-  /**
-   * \brief
-   *
-   */
-  unsigned char (*tx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
-  /**
-   * \brief
-   *
-   */
-  unsigned char (*rx_irq)(void *hal_dev, const unsigned char *data, unsigned short int length);
-  /**
-   * \brief
-   *
-   */
-  unsigned char (*abort_irq)(void *hal_dev);
   /**
    * \brief
    *

dynamixel_base/src/dynamixel_slave.c

 #include "dynamixel_slave.h"
 
 // communication functions
-void dyn_slave_receive_cb(TDynamixelSlave *slave)
-{
-  unsigned char *buffer;
-
-  if(slave!=0x00000000)
-  {
-    if(slave->rx_dma_mode==0x01)
-    {
-      if(slave->current_rx_buffer==0)
-      {
-        buffer=slave->rx_buffer2;
-        slave->current_rx_buffer=1;
-      }
-      else
-      {
-        buffer=slave->rx_buffer1;
-        slave->current_rx_buffer=0;
-      }
-      slave->rx_dma_mode=0x00;
-      if(slave->hal_dev!=0x00000000)
-      {
-        if(slave->version==DYN_VER1)
-          slave->rx_irq(slave->hal_dev,buffer,4);
-        else
-          slave->rx_irq(slave->hal_dev,buffer,7);
-      }
-      time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
-      slave->packet_ready=0x01;
-    }
-    else
-    {
-      if(slave->current_rx_buffer==0)
-        buffer=slave->rx_buffer1;
-      else
-        buffer=slave->rx_buffer2;
-      slave->rx_dma_mode=0x01;
-      if(slave->version==DYN_VER1)
-      {
-        if(buffer[0]!=0xFF) return;
-        if(buffer[1]!=0xFF) return;
-        if(slave->hal_dev!=0x00000000)
-          slave->rx_dma(slave->hal_dev,&buffer[4],buffer[3]);
-      }
-      else
-      {
-        if(buffer[0]!=0xFF) return;
-        if(buffer[1]!=0xFF) return;
-        if(buffer[2]!=0xFD) return;
-        if(slave->hal_dev!=0x00000000)
-          slave->rx_dma(slave->hal_dev,&buffer[7],buffer[5]+(buffer[6]<<8));
-      }
-    }
-  }
-}
-
 void dyn_slave_send_cb(TDynamixelSlave *slave)
 {
   if(slave!=0x00000000)
   {
     // enable tx interrupts
     slave->set_rx_mode();  
+    time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
   }
 }
 
@@ -142,21 +88,61 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
   }
 }
 
+unsigned char dyn_slave_check_new_packet(TDynamixelSlave *slave)
+{
+  unsigned char *buffer;
+
+  if(slave!=0x00000000)
+  {
+    if(slave->current_rx_buffer==0)
+      buffer=slave->rx_buffer1;
+    else
+      buffer=slave->rx_buffer2;
+    if(slave->version==DYN_VER1)
+    {
+      if(buffer[0]!=0xFF) return 0x00;
+      if(buffer[1]!=0xFF) return 0x00;
+      if(dyn_check_checksum(buffer)!=0xFF) return 0x00;
+      return 0x01;
+    }
+    else
+    {
+      if(buffer[0]!=0xFF) return 0x00;
+      if(buffer[1]!=0xFF) return 0x00;
+      if(buffer[2]!=0xFD) return 0x00;
+      if(dyn2_check_checksum(buffer)!=0x01) return 0x00;
+      return 0x01;
+    }
+  }
+  else
+    return 0x00;
+}
+
 void dyn_slave_loop(TDynamixelSlave *slave)
 {
-  unsigned char i,send_status,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
+  unsigned char i,send_status=0xFF,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
   unsigned short int length;
   unsigned char *buffer;
 
   if(slave!=0x00000000)
   {
-    if(slave->packet_ready)// check if a new instruction packet has been received
+    if(dyn_slave_check_new_packet(slave))// check if a new instruction packet has been received
     {
-      slave->packet_ready=0x00;
+      // cancel current DMA transfer
+      slave->abort_dma(slave->hal_dev);
+      // change current_buffer
       if(slave->current_rx_buffer==0)
-        buffer=slave->rx_buffer2;
-      else
+      {
         buffer=slave->rx_buffer1;
+        slave->current_rx_buffer=1;
+        slave->rx_dma(slave->hal_dev,slave->rx_buffer2,MAX_DYN_SLAVE_RX_BUFFER_LEN);
+      }
+      else
+      {
+        buffer=slave->rx_buffer2;
+        slave->current_rx_buffer=0;
+        slave->rx_dma(slave->hal_dev,slave->rx_buffer1,MAX_DYN_SLAVE_RX_BUFFER_LEN);
+      }
       // check address
       for(i=0;i<slave->num_slave_devices;i++)
       {
@@ -179,7 +165,12 @@ void dyn_slave_loop(TDynamixelSlave *slave)
           else
             slave->tx_dma(slave->hal_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
         } 
+        else
+          dyn_slave_send_cb(slave);
       }
+      // erase header of the current buffer
+      for(i=0;i<4;i++)
+        buffer[i]=0x00;
     }
     else
     {
@@ -187,18 +178,14 @@ void dyn_slave_loop(TDynamixelSlave *slave)
       {
         if(slave->hal_dev!=0x00000000)
         {
-          /* cancel any IRQ or DMA reception */
-          slave->abort_irq(slave->hal_dev);
+          // cancel DMA reception 
           slave->abort_dma(slave->hal_dev);
-          /* enable reception by IRQ */
+          // enable reception by DMa 
           if(slave->current_rx_buffer==0)
             buffer=slave->rx_buffer1;
           else
             buffer=slave->rx_buffer2;
-          if(slave->version==DYN_VER1)
-            slave->rx_irq(slave->hal_dev,buffer,4);
-          else
-            slave->rx_irq(slave->hal_dev,buffer,7);
+          slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
         }
         time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
       }
@@ -214,20 +201,15 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,void *hal_dev,TScheduler *sc
   slave->hal_dev=hal_dev;
   slave->version=version;
   slave->current_rx_buffer=0;
-  slave->rx_dma_mode=0x00;
   /* initialize the internal callbacks */
   slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
   slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
   slave->set_baudrate=dummy_dyn_slave_set_baudrate;
   slave->on_relay=dummy_dyn_slave_on_relay;
-  slave->tx_irq=dummy_dyn_slave_tx_irq;
-  slave->rx_irq=dummy_dyn_slave_rx_irq;
-  slave->abort_irq=dummy_dyn_slave_abort_irq;
   slave->tx_dma=dummy_dyn_slave_tx_dma;
   slave->rx_dma=dummy_dyn_slave_rx_dma;
   slave->abort_dma=dummy_dyn_slave_abort_dma;
   /* initialize internal variables */
-  slave->packet_ready=0x00;
   slave->rx_timeout_ms=50;
   /* initialize slave devices */
   slave->num_slave_devices=0;
@@ -259,10 +241,7 @@ void dyn_slave_start(TDynamixelSlave *slave)
       buffer=slave->rx_buffer1;
     else
       buffer=slave->rx_buffer2;
-    if(slave->version==DYN_VER1)
-      slave->rx_irq(slave->hal_dev,buffer,4);
-    else
-      slave->rx_irq(slave->hal_dev,buffer,7);
+    slave->rx_dma(slave->hal_dev,buffer,MAX_DYN_SLAVE_RX_BUFFER_LEN);
   }
   /* start timeout */
   time_set_timeout(&slave->time,slave->rx_timeout_ms*1000);
@@ -273,10 +252,7 @@ void dyn_slave_stop(TDynamixelSlave *slave)
   if(slave->scheduler!=0x00000000)
     scheduler_disable_channel(slave->scheduler,slave->sch_channel);
   if(slave->hal_dev!=0x00000000)
-  {
-    slave->abort_irq(slave->hal_dev);
     slave->abort_dma(slave->hal_dev);
-  }
   /* cancel timeout */
   time_cancel_timeout(&slave->time);
 }