From 308bcbb8afa37c75a08a8cbb1ec1bbc0ba880800 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 23 Aug 2017 17:18:54 +0200
Subject: [PATCH] Removed the inline modifier from all functions. Added the use
of the TMemModule structure to handle the master and slave interfaces.
---
dynamixel_base/Makefile | 3 +-
dynamixel_base/include/dynamixel_master.h | 1 -
dynamixel_base/include/dynamixel_slave.h | 19 ++++-
.../include/dynamixel_slave_registers.h | 45 +++++++++++
dynamixel_base/src/dynamixel.c | 22 ++---
dynamixel_base/src/dynamixel2.c | 22 ++---
dynamixel_base/src/dynamixel_master.c | 10 +--
dynamixel_base/src/dynamixel_slave.c | 81 ++++++++++++++++---
8 files changed, 164 insertions(+), 39 deletions(-)
create mode 100644 dynamixel_base/include/dynamixel_slave_registers.h
diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile
index ad6b594..fef5dc8 100755
--- a/dynamixel_base/Makefile
+++ b/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
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 47af3de..4976784 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/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
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index dc6f65e..0860021 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/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
*
diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h
new file mode 100644
index 0000000..8a3a01b
--- /dev/null
+++ b/dynamixel_base/include/dynamixel_slave_registers.h
@@ -0,0 +1,45 @@
+#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
+
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index 2f67ebe..eb18001 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/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];
}
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index e9e134a..bc3bd0f 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/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];
}
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 48edf5d..cb37574 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/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);
}
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index 9f92c1c..69a43fa 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -1,6 +1,44 @@
#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;
--
GitLab