Removed the inline modifier from all functions.

Added the use of the TMemModule structure to handle the master and slave interfaces.

dynamixel_base/Makefile

@@ -13,8 +13,9 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
 
 COMM_PATH = ../comm
 UTILS_PATH = ../utils
+MEMORY_PATH = ../memory
 
-INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include
+INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include
 
 DOC_DIR = ./doc
 

dynamixel_base/include/dynamixel_master.h

@@ -5,7 +5,6 @@
 #include "dynamixel.h"
 #include "dynamixel2.h"
 
-
 #ifndef MAX_DYN_MASTER_TX_BUFFER_LEN
   #define       MAX_DYN_MASTER_TX_BUFFER_LEN      1024
 #endif

dynamixel_base/include/dynamixel_slave.h

@@ -4,6 +4,8 @@
 #include "comm.h"
 #include "dynamixel.h"
 #include "dynamixel2.h"
+#include "dynamixel_slave_registers.h"
+#include "ram.h"
 
 #ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
   #define       MAX_DYN_SLAVE_TX_BUFFER_LEN      1024
@@ -118,6 +120,11 @@ typedef struct
    *
    */
   unsigned char bulk_read_pending;
+  /**
+   * \brief
+   *
+   */
+  TMemModule mem_module;
   /**
    * \brief
    *
@@ -128,6 +135,11 @@ typedef struct
    *
    */
   void (*set_rx_mode)(void);
+  /**
+   * \brief
+   *
+   */
+  void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate);
   /**
    * \brief
    *
@@ -160,7 +172,12 @@ typedef struct
  * \brief
  *
  */
-void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version);
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version);
+/**
+ * \brief
+ *
+ */
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
 /**
  * \brief
  *

dynamixel_base/include/dynamixel_slave_registers.h

+#ifndef _DYNAMIXEL_SLAVE_REGISTERS_H
+#define _DYNAMIXEL_SLAVE_REGISTERS_H
+
+#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS1
+  #define EEPROM_DYN_SLAVE_BASE_ADDRESS1      ((unsigned short int)0x0000)
+#endif
+
+#define EEPROM_DYN_SLAVE_LENGTH1              6 
+
+#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS2
+  #define EEPROM_DYN_SLAVE_BASE_ADDRESS2      ((unsigned short int)0x0000)
+#endif
+
+#define EEPROM_DYN_SLAVE_LENGTH2              1
+
+#define DEVICE_MODEL                          (EEPROM_DYN_SLAVE_BASE_ADDRESS1)
+#define FIRMWARE_VERSION                      (EEPROM_DYN_SLAVE_BASE_ADDRESS1+2)
+#define DEVICE_ID                             (EEPROM_DYN_SLAVE_BASE_ADDRESS1+3)
+#define BAUDRATE                              (EEPROM_DYN_SLAVE_BASE_ADDRESS1+4)
+#define RETURN_DELAY                          (EEPROM_DYN_SLAVE_BASE_ADDRESS1+5)
+
+#define RETURN_LEVEL                          (EEPROM_DYN_SLAVE_BASE_ADDRESS2)
+
+#ifndef DEFAULT_DEVICE_MODEL
+  #define DEFAULT_DEVICE_MODEL                0x7300
+#endif
+#ifndef DEFAULT_FIRMWARE_VERSION
+  #define DEFAULT_FIRMWARE_VERSION            0x0001
+#endif
+#ifndef DEFAULT_DEVICE_ID
+  #define DEFAULT_DEVICE_ID                   0x0001
+#endif
+#ifndef DEFAULT_BAUDRATE
+  #define DEFAULT_BAUDRATE                    0x0010
+#endif
+#ifndef DEFAULT_RETURN_DELAY
+  #define DEFAULT_RETURN_DELAY                0x0000
+#endif
+
+#ifndef DEFAULT_RETURN_LEVEL
+  #define DEFAULT_RETURN_LEVEL                0x0002
+#endif
+
+#endif
+

dynamixel_base/src/dynamixel.c

@@ -40,17 +40,17 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination)
     destination[i]=source[i];
 }
 
-inline unsigned char dyn_get_id(unsigned char *packet)
+unsigned char dyn_get_id(unsigned char *packet)
 {
   return packet[DYN_ID_OFF];
 }
 
-inline unsigned char dyn_get_length(unsigned char *packet)
+unsigned char dyn_get_length(unsigned char *packet)
 {
   return packet[DYN_LENGTH_OFF];
 }
 
-inline TDynInstruction dyn_get_instruction(unsigned char *packet)
+TDynInstruction dyn_get_instruction(unsigned char *packet)
 {
   return packet[DYN_INST_OFF];
 }
@@ -99,12 +99,12 @@ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char a
   dyn_set_checksum(packet);
 }
 
-inline unsigned char dyn_get_read_length(unsigned char *packet)
+unsigned char dyn_get_read_length(unsigned char *packet)
 {
   return packet[DYN_DATA_OFF+1];
 }
 
-inline unsigned char dyn_get_read_address(unsigned char *packet)
+unsigned char dyn_get_read_address(unsigned char *packet)
 {
   return packet[DYN_DATA_OFF];
 }
@@ -125,12 +125,12 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char
   dyn_set_checksum(packet);
 }
 
-inline unsigned char dyn_get_write_address(unsigned char *packet)
+unsigned char dyn_get_write_address(unsigned char *packet)
 {
   return packet[DYN_DATA_OFF];
 }
 
-inline unsigned char dyn_get_write_length(unsigned char *packet)
+unsigned char dyn_get_write_length(unsigned char *packet)
 {
   return packet[DYN_LENGTH_OFF]-3;
 }
@@ -161,12 +161,12 @@ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned c
   dyn_set_checksum(packet);
 }
 
-inline unsigned char dyn_get_reg_write_address(unsigned char *packet)
+unsigned char dyn_get_reg_write_address(unsigned char *packet)
 {
   return packet[DYN_DATA_OFF];
 }
 
-inline unsigned char dyn_get_reg_write_length(unsigned char *packet)
+unsigned char dyn_get_reg_write_length(unsigned char *packet)
 {
   return packet[DYN_LENGTH_OFF]-3;
 }
@@ -315,7 +315,7 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err
   dyn_set_checksum(packet); 
 }
 
-inline TDynError dyn_get_status_error(unsigned char *packet)
+TDynError dyn_get_status_error(unsigned char *packet)
 {
   return packet[DYN_ERROR_OFF];
 }
@@ -331,7 +331,7 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data
   return packet[DYN_LENGTH_OFF]-0x02;
 }
 
-inline unsigned char dyn_get_read_status_id(unsigned char *packet)
+unsigned char dyn_get_read_status_id(unsigned char *packet)
 {
   return packet[DYN_ID_OFF];
 }

dynamixel_base/src/dynamixel2.c

@@ -77,17 +77,17 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination)
     destination[i]=source[i];
 }
 
-inline unsigned char dyn2_get_id(unsigned char *packet)
+unsigned char dyn2_get_id(unsigned char *packet)
 {
   return packet[DYN2_ID_OFF];
 }
 
-inline unsigned short int dyn2_get_length(unsigned char *packet)
+unsigned short int dyn2_get_length(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_LENGTH_OFF]);
 }
 
-inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
+TDynInstruction dyn2_get_instruction(unsigned char *packet)
 {
   return packet[DYN2_INST_OFF];
 }
@@ -141,12 +141,12 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short
   dyn2_set_checksum(packet);
 }
 
-inline unsigned short int dyn2_get_read_length(unsigned char *packet)
+unsigned short int dyn2_get_read_length(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_DATA_OFF+2]);
 }
 
-inline unsigned short int dyn2_get_read_address(unsigned char *packet)
+unsigned short int dyn2_get_read_address(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
 }
@@ -169,12 +169,12 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor
   dyn2_set_checksum(packet);
 }
 
-inline unsigned short int dyn2_get_write_address(unsigned char *packet)
+unsigned short int dyn2_get_write_address(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
 }
 
-inline unsigned short int dyn2_get_write_length(unsigned char *packet)
+unsigned short int dyn2_get_write_length(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5;
 }
@@ -208,12 +208,12 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned
   dyn2_set_checksum(packet);
 }
 
-inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
+unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
 }
 
-inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
+unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
 {
   return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5;
 }
@@ -468,7 +468,7 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
   dyn2_set_checksum(packet); 
 }
 
-inline TDynError dyn2_get_status_error(unsigned char *packet)
+TDynError dyn2_get_status_error(unsigned char *packet)
 {
   return packet[DYN2_DATA_OFF];
 }
@@ -485,7 +485,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
   return length;
 }
 
-inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
+unsigned char dyn2_get_read_status_id(unsigned char *packet)
 {
   return packet[DYN2_ID_OFF];
 }

dynamixel_base/src/dynamixel_master.c

@@ -743,19 +743,19 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
   master->set_rx_mode();
 }
 
-inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
+void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
 {
   if(master!=0x00000000)
     master->rx_timeout_ms=timeout_ms;
 }
 
-inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
+void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
 {
   if(master!=0x00000000)
     master->return_level=level;
 }
 
-inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
+return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
 {
   if(master!=0x00000000)
     return master->return_level;
@@ -815,7 +815,7 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
   return error;
 }
 
-inline unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
+unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
 {
   return dyn_master_read_table(master,id,address,1,data);
 }
@@ -884,7 +884,7 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un
   return error;
 }
 
-inline unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
+unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
 {
   return dyn_master_write_table(master,id,address,1,&data);
 }

dynamixel_base/src/dynamixel_slave.c

 #include "dynamixel_slave.h"
 
+/* EEPROM data */
+unsigned short int dyn_slave_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_DEVICE_MODEL&0x00FF,DEVICE_MODEL,
+                                                                                  (DEFAULT_DEVICE_MODEL>>8)&0x00FF,DEVICE_MODEL+1,
+                                                                                  DEFAULT_FIRMWARE_VERSION,FIRMWARE_VERSION,
+                                                                                  DEFAULT_DEVICE_ID,DEVICE_ID,
+                                                                                  DEFAULT_BAUDRATE,BAUDRATE,
+                                                                                  DEFAULT_RETURN_DELAY,RETURN_DELAY,
+                                                                                  DEFAULT_RETURN_LEVEL,RETURN_LEVEL};
+
 /* private functions */
+void dyn_slave_write_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  if(ram_in_range(DEVICE_ID,address,length))
+  {
+    dyn_slave_set_address(dyn_slave,data[DEVICE_ID-address]);
+    ram_data[DEVICE_ID]=data[DEVICE_ID-address];
+  }
+  if(ram_in_range(BAUDRATE,address,length))
+  {
+    dyn_slave_set_baudrate(dyn_slave,2000000/(data[BAUDRATE-address]+1));
+    ram_data[BAUDRATE]=data[BAUDRATE-address];
+  }
+  if(ram_in_range(RETURN_DELAY,address,length))
+  {
+    dyn_slave_set_return_delay(dyn_slave,data[RETURN_DELAY-address]);
+    ram_data[RETURN_DELAY]=data[RETURN_DELAY-address];
+  }
+  if(ram_in_range(RETURN_LEVEL,address,length))
+  {
+    dyn_slave_set_return_level(dyn_slave,data[RETURN_LEVEL-address]);
+    ram_data[RETURN_LEVEL]=data[RETURN_LEVEL-address];
+  }
+}
+
+void dyn_slave_read_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  ram_read_table(address,length,data);
+}
+
 unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte)
 {
   if(dyn_slave!=0x00000000)
@@ -109,6 +147,11 @@ void dummy_dyn_slave_set_rx_mode(void)
 
 }
 
+void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate)
+{
+
+}
+
 void dummy_dyn_slave_on_ping(void)
 {
 
@@ -438,7 +481,7 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave)
 }
 
 /* public functions */
-void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version)
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version)
 {
   /* assign communication functions */
   dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb;
@@ -450,6 +493,7 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn
   /* 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_ping=dummy_dyn_slave_on_ping;
   slave->on_read=dummy_dyn_slave_on_read;
   slave->on_write=dummy_dyn_slave_on_write;
@@ -470,24 +514,43 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn
   slave->sync_read_pending=0x00;
   slave->bulk_read_pending=0x00;
 
+  /* initialize memory module */
+  mem_module_init(&slave->mem_module);
+  slave->mem_module.data=slave;
+  slave->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_write_cmd;
+  slave->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_read_cmd;
+  if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_LENGTH1))
+    return 0x00;
+  if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS2,EEPROM_DYN_SLAVE_LENGTH2))
+    return 0x00;
+  if(!mem_add_module(memory,&slave->mem_module))
+    return 0x00;
+
   slave->set_rx_mode();
   /* enable reception by IRQ */
   comm_receive_irq(slave->comm_dev,0);
+
+  return 0x01;
+}
+
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
+{
+  slave->set_baudrate(slave->comm_dev,baudrate);
 }
 
-inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
+void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
 {
   if(slave!=0x00000000)
     slave->rx_timeout_ms=timeout_ms;
 }
 
-inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
+void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
 {
   if(slave!=0x00000000)
     slave->address=address;
 }
 
-inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
+unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
 {
   if(slave!=0x00000000)
     return slave->address;
@@ -495,13 +558,13 @@ inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
     return 0xFF;
 }
 
-inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay)
+void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay)
 {
   if(slave!=0x00000000)
     slave->return_delay=delay;
 }
 
-inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
+unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
 {
   if(slave!=0x00000000)
     return slave->return_delay;
@@ -509,13 +572,13 @@ inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
     return 0xFF;
 }
 
-inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level)
+void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level)
 {
   if(slave!=0x00000000)
     slave->return_level=level;
 }
 
-inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
+return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
 {
   if(slave!=0x00000000)
     return slave->return_level;
@@ -523,7 +586,7 @@ inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
     return return_all;
 }
 
-inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
+TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
 {
   if(slave!=0x00000000)
     return slave->version;