diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile
index fef5dc81ca91d8ee5c4e03d28c6f842ab9f5af02..4f63efe978f1c1ef546c38a513cfb7ebcbe26bb3 100755
--- a/dynamixel_base/Makefile
+++ b/dynamixel_base/Makefile
@@ -14,8 +14,11 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
COMM_PATH = ../comm
UTILS_PATH = ../utils
MEMORY_PATH = ../memory
+SCHEDULER_PATH = ../scheduler
-INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include
+INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include
+
+MACROS = -DMAX_DYN_SLAVE_TX_BUFFER_LEN=256 -DMAX_DYN_SLAVE_RX_BUFFER_LEN=256 -DMAX_DYN_SLAVE_REG_BUFFER_LEN=256 -DMAX_NUM_SLAVE_DEVICES=4 -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256
DOC_DIR = ./doc
@@ -53,13 +56,13 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM
all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0) -o $@ $<
$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0plus) -o $@ $<
$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M3) -o $@ $<
mkdir_build:
mkdir -p build/m4_fpu
mkdir -p build/m0
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 7da7c90be7873723a8b9b27c5db894b792ad787e..8ee13ea4ea16273afe5b31e7ec1b20ef404d227b 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -6,11 +6,11 @@
#include "dynamixel2.h"
#ifndef MAX_DYN_MASTER_TX_BUFFER_LEN
- #define MAX_DYN_MASTER_TX_BUFFER_LEN 256
+ #error "Please, specify the maximum TX buffer length with the MAX_DYN_MASTER_TX_BUFFER_LEN macro"
#endif
#ifndef MAX_DYN_MASTER_RX_BUFFER_LEN
- #define MAX_DYN_MASTER_RX_BUFFER_LEN 256
+ #error "Please, specify the maximum RX buffer length with the MAX_DYN_MASTER_TX_BUFFER_LEN macro"
#endif
/**
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 37902b616fd26d24244cd6cfbba0adf54164e5d6..64f8d751f22e4f766651c3b764d9c2e19a78a6d8 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -4,52 +4,47 @@
#include "comm.h"
#include "dynamixel.h"
#include "dynamixel2.h"
-#include "dynamixel_slave_registers.h"
-#include "ram.h"
+#include "scheduler.h"
+#include "dynamixel_slave_device.h"
#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
- #define MAX_DYN_SLAVE_TX_BUFFER_LEN 256
+ #error "Please, specify the maximum TX buffer length with the MAX_DYN_SLAVE_TX_BUFFER_LEN macro"
#endif
#ifndef MAX_DYN_SLAVE_RX_BUFFER_LEN
- #define MAX_DYN_SLAVE_RX_BUFFER_LEN 256
+ #error "Please, specify the maximum RX buffer length with the MAX_DYN_SLAVE_RX_BUFFER_LEN macro"
#endif
-#ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN
- #define MAX_DYN_SLAVE_REG_BUFFER_LEN 256
+#ifndef MAX_NUM_SLAVE_DEVICES
+ #error "Please, specify the maximum number of slave devices with the MAX_NUM_SLAVE_DEVICES macro"
#endif
/**
* \brief
*
*/
-typedef struct
+typedef struct TDynamixelSlave
{
/**
* \brief
*
*/
- TComm *comm_dev;
- /**
- * \brief
- *
- */
- TDynVersion version;
+ TScheduler *scheduler;
/**
* \brief
*
*/
- unsigned char address;
+ sched_channel_t sch_channel;
/**
* \brief
*
*/
- unsigned char return_delay;
+ TComm *comm_dev;
/**
* \brief
*
*/
- return_level_t return_level;
+ TDynVersion version;
/**
* \brief
*
@@ -84,47 +79,12 @@ typedef struct
* \brief
*
*/
- unsigned short int reg_address;
- /**
- * \brief
- *
- */
- unsigned short int reg_length;
- /**
- * \brief
- *
- */
- unsigned char reg_buffer[MAX_DYN_SLAVE_REG_BUFFER_LEN];
- /**
- * \brief
- *
- */
- unsigned short int sync_bulk_address;
- /**
- * \brief
- *
- */
- unsigned short int sync_bulk_length;
+ unsigned char num_slave_devices;
/**
* \brief
*
*/
- unsigned char sync_bulk_prev_id;
- /**
- * \brief
- *
- */
- unsigned char sync_read_pending;
- /**
- * \brief
- *
- */
- unsigned char bulk_read_pending;
- /**
- * \brief
- *
- */
- TMemModule mem_module;
+ TDynamixelSlaveDevice *slave_devices[MAX_NUM_SLAVE_DEVICES];
/**
* \brief
*
@@ -140,26 +100,6 @@ typedef struct
*
*/
void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate);
- /**
- * \brief
- *
- */
- void (*on_ping)(void);
- /**
- * \brief
- *
- */
- unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data);
- /**
- * \brief
- *
- */
- unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data);
- /**
- * \brief
- *
- */
- unsigned char (*on_reset)(void);
/**
* \brief
*
@@ -172,57 +112,36 @@ typedef struct
* \brief
*
*/
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version);
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
/**
* \brief
*
*/
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
+unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device);
/**
* \brief
*
*/
-inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
-/**
- * \brief
- *
- */
-inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address);
-/**
- * \brief
- *
- */
-inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave);
-/**
- * \brief
- *
- */
-inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay);
-/**
- * \brief
- *
- */
-inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave);
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
/**
* \brief
*
*/
-inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level);
+inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
/**
* \brief
*
*/
-inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave);
+inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
/**
* \brief
*
*/
-inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
+void dyn_slave_start(TDynamixelSlave *slave);
/**
* \brief
*
*/
-void dyn_slave_loop(TDynamixelSlave *slave);
-
+void dyn_slave_stop(TDynamixelSlave *slave);
#endif
diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h
new file mode 100644
index 0000000000000000000000000000000000000000..c43300b02604378f4a2699ffc3c2d29a0989346c
--- /dev/null
+++ b/dynamixel_base/include/dynamixel_slave_device.h
@@ -0,0 +1,173 @@
+#ifndef _DYNAMIXEL_SLAVE_DEVICE_H
+#define _DYNAMIXEL_SLAVE_DEVICE_H
+
+#include "dynamixel_slave_registers.h"
+#include "dynamixel.h"
+#include "dynamixel2.h"
+#include "ram.h"
+
+#ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN
+ #error "Please, specify the maximum registered buffer length with the MAX_DYN_SLAVE_REG_BUFFER_LEN macro"
+#endif
+
+/**
+ * \brief
+ *
+ */
+typedef struct
+{
+ /**
+ * \brief
+ *
+ */
+ struct TDynamixelSlave *slave;
+ /**
+ * \brief
+ *
+ */
+ unsigned char address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int eeprom_base_address1;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int eeprom_base_address2;
+ /**
+ * \brief
+ *
+ */
+ unsigned char return_delay;
+ /**
+ * \brief
+ *
+ */
+ return_level_t return_level;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int reg_address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int reg_length;
+ /**
+ * \brief
+ *
+ */
+ unsigned char reg_buffer[MAX_DYN_SLAVE_REG_BUFFER_LEN];
+ /**
+ * \brief
+ *
+ */
+ unsigned short int sync_bulk_address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int sync_bulk_length;
+ /**
+ * \brief
+ *
+ */
+ unsigned char sync_bulk_prev_id;
+ /**
+ * \brief
+ *
+ */
+ unsigned char sync_read_pending;
+ /**
+ * \brief
+ *
+ */
+ unsigned char bulk_read_pending;
+ /**
+ * \brief
+ *
+ */
+ TMemory memory;
+ /**
+ * \brief
+ *
+ */
+ TMemModule mem_module;
+ /**
+ * \brief
+ *
+ */
+ void (*on_ping)(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_reset)(void);
+}TDynamixelSlaveDevice;
+
+/* public functions */
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char address,unsigned char eeprom_base_address1,unsigned char eeprom_base_address2);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address);
+/**
+ * \brief
+ *
+ */
+inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay);
+/**
+ * \brief
+ *
+ */
+inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level);
+/**
+ * \brief
+ *
+ */
+inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned char *length, unsigned char *data);
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned short int *length, unsigned char *data);
+/**
+ * \brief
+ *
+ */
+inline TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device);
+
+#endif
diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h
index 8a3a01bd147a1913bc031aba1c32c97a49960954..46cef2b79342753136e2514c7ce41c79383306a1 100644
--- a/dynamixel_base/include/dynamixel_slave_registers.h
+++ b/dynamixel_base/include/dynamixel_slave_registers.h
@@ -1,45 +1,27 @@
#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 DEVICE_MODEL 0
+#define FIRMWARE_VERSION 2
+#define DEVICE_ID 3
+#define BAUDRATE 4
+#define RETURN_DELAY 5
-#define RETURN_LEVEL (EEPROM_DYN_SLAVE_BASE_ADDRESS2)
+#define RETURN_LEVEL 0
-#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
+#define dyn_slave_control_eeprom_data(name,section_name,base_address1,base_address2,DEFAULT_DEVICE_MODEL,DEFAULT_FIRMWARE_VERSION,DEFAULT_DEVICE_ID,DEFAULT_BAUDRATE,DEFAULT_RETURN_DELAY,DEFAULT_RETURN_LEVEL) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_DEVICE_MODEL&0x00FF,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_MODEL, \
+ (DEFAULT_DEVICE_MODEL>>8)&0x00FF,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_MODEL+1, \
+ DEFAULT_FIRMWARE_VERSION,(DEFAULT_DEVICE_ID<<8) + base_address1+FIRMWARE_VERSION, \
+ DEFAULT_DEVICE_ID,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_ID, \
+ DEFAULT_BAUDRATE,(DEFAULT_DEVICE_ID<<8) + base_address1+BAUDRATE, \
+ DEFAULT_RETURN_DELAY,(DEFAULT_DEVICE_ID<<8) + base_address1+RETURN_DELAY, \
+ DEFAULT_RETURN_LEVEL,(DEFAULT_DEVICE_ID<<8) + base_address2+RETURN_LEVEL};
-#ifndef DEFAULT_RETURN_LEVEL
- #define DEFAULT_RETURN_LEVEL 0x0002
-#endif
#endif
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index e1818981fa23d44bc06a1f2ae9d8159010b2acab..6afdbe8076451d1cd205c32119123762182443af 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -1,44 +1,6 @@
#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);
-}
-
+// communication functions
unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte)
{
if(dyn_slave!=0x00000000)
@@ -152,44 +114,24 @@ void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate)
}
-void dummy_dyn_slave_on_ping(void)
-{
-
-}
-
-unsigned char dummy_dyn_slave_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_slave_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_slave_on_reset(void)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
{
return DYN_NO_DEVICE;
}
-void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned short int length, unsigned char *data)
+void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,unsigned char error,unsigned short int length, unsigned char *data,unsigned char return_delay)
{
if(slave!=0x00000000)
{
// wait until the previous transmission has ended (if any)
while(comm_is_send_done(slave->comm_dev)==COMM_BUSY);
- if(slave->return_delay>0)
+ if(return_delay>0)
if(slave->comm_dev->time!=0x00000000)
- time_delay_us(slave->comm_dev->time,slave->return_delay<<1);
+ time_delay_us(slave->comm_dev->time,return_delay<<1);
if(slave->version==DYN_VER1)
{
// create the status packet
- dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ dyn_init_status_packet(slave->tx_buffer,address,error,length,data);
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
@@ -198,7 +140,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,uns
else
{
// create the status packet
- dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ dyn2_init_status_packet(slave->tx_buffer,address,error,length,data);
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
@@ -207,272 +149,46 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,uns
}
}
-void dyn_v1_slave_loop(TDynamixelSlave *slave)
-{
- static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id;
-
- if(slave!=0x000000000)
- {
- id=dyn_get_id(slave->rx_buffer);
- if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
- {
- // check the packet checksum
- if(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
- {
- // process the packet
- switch(dyn_get_instruction(slave->rx_buffer))
- {
- case DYN_PING: slave->on_ping();
- if(id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data);
- if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data);
- error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
- slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_ACTION: if(slave->reg_address!=0xFFFF)
- {
- error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
- slave->reg_address=0xFFFF;
- }
- else
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
- error=slave->on_write(address,length,data);
- break;
- case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->bulk_read_pending=0x01;
- }
- }
- break;
- case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- default:
- break;
- }
- }
- else
- {
- // send a checksum error answer
- if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
- }
- }
- else
- {
- if(slave->bulk_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->bulk_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else// the packet is addressed to another device, so relay it
- {
- if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
- {
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
- }
- }
- }
- }
-}
-
-void dyn_v2_slave_loop(TDynamixelSlave *slave)
+void dyn_slave_loop(TDynamixelSlave *slave)
{
- static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,prev_id,id;
- static unsigned short int length,address;
+ unsigned char i,send_status,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
+ unsigned short int length;
if(slave!=0x00000000)
{
- id=dyn2_get_id(slave->rx_buffer);
- if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ if(slave->packet_ready)// check if a new instruction packet has been received
{
- // check the packet checksum
- if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay
- {
- // process the packet
- switch(dyn2_get_instruction(slave->rx_buffer))
- {
- case DYN_PING: slave->on_ping();
- if(id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data);
- if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data);
- error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
- slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_ACTION: if(slave->reg_address!=0xFFFF)
- {
- error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
- slave->reg_address=0xFFFF;
- }
- else
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->sync_read_pending=0x01;
- }
- }
- break;
- case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
- error=slave->on_write(address,length,data);
- break;
- case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->bulk_read_pending=0x01;
- }
- }
- break;
- case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))
- error=slave->on_write(address,length,data);
- break;
- default:
- break;
- }
- }
- else
+ slave->packet_ready=0x00;
+ // check address
+ for(i=0;i<slave->num_slave_devices;i++)
{
- // send a checksum error answer
- if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
+ if(slave->version==DYN_VER1)
+ send_status=dyn_v1_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,(unsigned char *)&length,data);
+ else
+ send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,&length,data);
+ if(send_status==0x01)
+ dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
}
+ if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
+ {
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ }
}
else
{
- if(slave->bulk_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->bulk_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else if(slave->sync_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->sync_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else// the packet is addressed to another device, so relay it
+ if(slave->comm_dev->time!=0x00000000)
{
- if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
- {
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+8);
+ if(time_is_timeout(slave->comm_dev->time))
+ {
+ /* cancel any IRQ or DMA reception */
+ comm_cancel_irq_receive(slave->comm_dev);
+ comm_cancel_dma_receive(slave->comm_dev);
+ slave->received_bytes=0;
+ /* enable reception by IRQ */
+ comm_receive_irq(slave->comm_dev,0);
}
}
}
@@ -480,8 +196,10 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave)
}
/* public functions */
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version)
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
{
+ unsigned char i;
+
/* assign communication functions */
dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb;
dev->dma_send_cb=(unsigned char (*)(void *))dyn_slave_dma_send_cb;
@@ -493,37 +211,20 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,u
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;
- slave->on_reset=dummy_dyn_slave_on_reset;
slave->on_relay=dummy_dyn_slave_on_relay;
/* initialize internal variables */
- slave->address=address;
- slave->return_delay=0x00;
- slave->return_level=return_all;
slave->packet_ready=0x00;
slave->rx_timeout_ms=50;
slave->received_bytes=0x00;
- slave->reg_address=0xFFFF;
- slave->reg_length=0x0000;
- slave->sync_bulk_address=0x0000;
- slave->sync_bulk_length=0x0000;
- slave->sync_bulk_prev_id=0x00;
- slave->sync_read_pending=0x00;
- slave->bulk_read_pending=0x00;
+ /* initialize slave devices */
+ slave->num_slave_devices=0;
+ for(i=0;i<MAX_NUM_SLAVE_DEVICES;i++)
+ slave->slave_devices[i]=0x00000000;
- /* 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;
+ /* assigna a scheduler channel */
+ scheduler_set_channel(scheduler,ch,(void (*)(void *))dyn_slave_loop,1,slave);
+ slave->scheduler=scheduler;
+ slave->sch_channel=ch;
slave->set_rx_mode();
/* enable reception by IRQ */
@@ -532,57 +233,33 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,u
return 0x01;
}
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
-{
- slave->set_baudrate(slave->comm_dev,baudrate);
-}
-
-void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
+unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device)
{
if(slave!=0x00000000)
- slave->rx_timeout_ms=timeout_ms;
-}
-
-void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
-{
- if(slave!=0x00000000)
- slave->address=address;
-}
-
-unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
-{
- if(slave!=0x00000000)
- return slave->address;
- else
- return 0xFF;
-}
-
-void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay)
-{
- if(slave!=0x00000000)
- slave->return_delay=delay;
-}
-
-unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
-{
- if(slave!=0x00000000)
- return slave->return_delay;
+ {
+ if(slave->num_slave_devices<=(MAX_NUM_SLAVE_DEVICES-1))
+ {
+ slave->slave_devices[slave->num_slave_devices]=device;
+ slave->num_slave_devices++;
+ device->slave=slave;
+ return 0x01;
+ }
+ else
+ return 0x00;
+ }
else
- return 0xFF;
+ return 0x00;
}
-void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level)
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
{
- if(slave!=0x00000000)
- slave->return_level=level;
+ slave->set_baudrate(slave->comm_dev,baudrate);
}
-return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
+void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
{
if(slave!=0x00000000)
- return slave->return_level;
- else
- return return_all;
+ slave->rx_timeout_ms=timeout_ms;
}
TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
@@ -593,33 +270,13 @@ TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
return DYN_VER1;
}
-void dyn_slave_loop(TDynamixelSlave *slave)
+void dyn_slave_start(TDynamixelSlave *slave)
{
- if(slave!=0x00000000)
- {
- if(slave->packet_ready)// check if a new instruction packet has been received
- {
- slave->packet_ready=0x00;
- // check address
- if(slave->version==DYN_VER1)
- dyn_v1_slave_loop(slave);
- else
- dyn_v2_slave_loop(slave);
- }
- else
- {
- if(slave->comm_dev->time!=0x00000000)
- {
- if(time_is_timeout(slave->comm_dev->time))
- {
- /* cancel any IRQ or DMA reception */
- comm_cancel_irq_receive(slave->comm_dev);
- comm_cancel_dma_receive(slave->comm_dev);
- slave->received_bytes=0;
- /* enable reception by IRQ */
- comm_receive_irq(slave->comm_dev,0);
- }
- }
- }
- }
+ scheduler_enable_channel(slave->scheduler,slave->sch_channel);
}
+
+void dyn_slave_stop(TDynamixelSlave *slave)
+{
+ scheduler_disable_channel(slave->scheduler,slave->sch_channel);
+}
+
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
new file mode 100644
index 0000000000000000000000000000000000000000..c11eecc6446d528b375a4c9dfb659730938d5642
--- /dev/null
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -0,0 +1,468 @@
+#include "dynamixel_slave_device.h"
+#include "dynamixel_slave.h"
+
+/* private functions */
+void dyn_slave_device_write_cmd(TDynamixelSlaveDevice *device,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned short int offset;
+
+ offset=address-device->eeprom_base_address1;
+ if(ram_in_window(device->eeprom_base_address1+DEVICE_MODEL,2,address,length))
+ {
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_MODEL,address,length))
+ device->memory.data[device->eeprom_base_address1+DEVICE_MODEL]=data[DEVICE_MODEL-offset];
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_MODEL+1,address,length))
+ device->memory.data[device->eeprom_base_address1+DEVICE_MODEL+1]=data[DEVICE_MODEL+1-offset];
+ }
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_ID,address,length))
+ {
+ dyn_slave_device_set_address(device,data[DEVICE_ID-offset]);
+ device->memory.data[device->eeprom_base_address1+DEVICE_ID]=data[DEVICE_ID-offset];
+ }
+ if(ram_in_range(device->eeprom_base_address1+BAUDRATE,address,length))
+ {
+ dyn_slave_set_baudrate(device->slave,2000000/(data[BAUDRATE-offset]+1));
+ device->memory.data[device->eeprom_base_address1+BAUDRATE]=data[BAUDRATE-offset];
+ }
+ if(ram_in_range(device->eeprom_base_address1+RETURN_DELAY,address,length))
+ {
+ dyn_slave_device_set_return_delay(device,data[RETURN_DELAY-offset]);
+ device->memory.data[device->eeprom_base_address1+RETURN_DELAY]=data[RETURN_DELAY-offset];
+ }
+ offset=address-device->eeprom_base_address2;
+ if(ram_in_range(device->eeprom_base_address2+RETURN_LEVEL,address,length))
+ {
+ dyn_slave_device_set_return_level(device,data[RETURN_LEVEL-offset]);
+ device->memory.data[device->eeprom_base_address2+RETURN_LEVEL]=data[RETURN_LEVEL-offset];
+ }
+}
+
+void dyn_slave_device_read_cmd(TDynamixelSlaveDevice *device,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(&device->memory,address,length,data);
+}
+
+void dummy_dyn_slave_device_on_ping(void)
+{
+
+}
+
+unsigned char dummy_dyn_slave_device_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dummy_dyn_slave_device_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dummy_dyn_slave_device_on_reset(void)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned char *length,unsigned char *data)
+{
+ static unsigned char address,prev_id,id;
+ unsigned char send_status=0x00;
+
+ if(device!=0x000000000)
+ {
+ id=dyn_get_id(rx_buffer);
+ if(id==device->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(dyn_check_checksum(rx_buffer)==0xFF)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn_get_instruction(rx_buffer))
+ {
+ case DYN_PING: device->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ *error=DYN_NO_ERROR;
+ *length=0;
+ }
+ break;
+ case DYN_READ: (*error)=device->on_read(dyn_get_read_address(rx_buffer),dyn_get_read_length(rx_buffer),data);
+ if(device->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=dyn_get_read_length(rx_buffer);
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ break;
+ case DYN_WRITE: (*length)=dyn_get_write_data(rx_buffer,data);
+ (*error)=device->on_write(dyn_get_write_address(rx_buffer),*length,data);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*length)=0;
+ if((*error)!=DYN_NO_ERROR)
+ (*error)=DYN_INST_ERROR;
+ }
+ break;
+ case DYN_REG_WRITE: device->reg_length=dyn_get_reg_write_data(rx_buffer,device->reg_buffer);
+ device->reg_address=dyn_get_reg_write_address(rx_buffer);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_ACTION: if(device->reg_address!=0xFFFF)
+ {
+ (*error)=device->on_write(device->reg_address,device->reg_length,device->reg_buffer);
+ device->reg_address=0xFFFF;
+ }
+ else
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: send_status=0x01;
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ break;
+ case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(rx_buffer,device->address,&address,length,data))// the device is addressed
+ (*error)=device->on_write(address,*length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ send_status=0x01;
+ (*error)=device->on_read(address,*length,data);
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: send_status=0x01;
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_CHECKSUM_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ else
+ {
+ if(device->bulk_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->bulk_read_pending=0x00;
+ send_status=0x01;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ }
+ }
+
+ return send_status;
+}
+
+unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned short int *length,unsigned char *data)
+{
+ static unsigned char prev_id,id;
+ static unsigned short int address;
+ unsigned char send_status=0x00;
+
+ if(device!=0x00000000)
+ {
+ id=dyn2_get_id(rx_buffer);
+ if(id==device->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(dyn2_check_checksum(rx_buffer)==0x01)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn2_get_instruction(rx_buffer))
+ {
+ case DYN_PING: device->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_READ: (*error)=device->on_read(dyn2_get_read_address(rx_buffer),dyn2_get_read_length(rx_buffer),data);
+ if(device->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=dyn2_get_read_length(rx_buffer);
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ break;
+ case DYN_WRITE: (*length)=dyn2_get_write_data(rx_buffer,data);
+ (*error)=device->on_write(dyn2_get_write_address(rx_buffer),*length,data);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*length)=0;
+ if((*error)!=DYN_NO_ERROR)
+ (*error)=DYN_INST_ERROR;
+ }
+ break;
+ case DYN_REG_WRITE: device->reg_length=dyn2_get_reg_write_data(rx_buffer,device->reg_buffer);
+ device->reg_address=dyn2_get_reg_write_address(rx_buffer);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_ACTION: if(device->reg_address!=0xFFFF)
+ {
+ (*error)=device->on_write(device->reg_address,device->reg_length,device->reg_buffer);
+ device->reg_address=0xFFFF;
+ }
+ else
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ (*error)=device->on_read(address,*length,data);
+ send_status=0x01;
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->sync_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(rx_buffer,device->address,&address,length,data))// the device is addressed
+ (*error)=device->on_write(address,*length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ send_status=0x01;
+ (*error)=device->on_read(address,*length,data);
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(rx_buffer,device->address,&address,length,data))
+ (*error)=device->on_write(address,*length,data);
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_CHECKSUM_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ else
+ {
+ if(device->bulk_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->bulk_read_pending=0x00;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ }
+ else if(device->sync_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->sync_read_pending=0x00;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ }
+ }
+ }
+
+ return send_status;
+}
+
+/* public functions */
+unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char address,unsigned char eeprom_base_address1,unsigned char eeprom_base_address2)
+{
+ /* initialize the internal callbacks */
+ device->on_ping=dummy_dyn_slave_device_on_ping;
+ device->on_read=dummy_dyn_slave_device_on_read;
+ device->on_write=dummy_dyn_slave_device_on_write;
+ device->on_reset=dummy_dyn_slave_device_on_reset;
+ /* initialize internal variables */
+ device->address=address;
+ device->return_delay=0x00;
+ device->return_level=return_all;
+ device->reg_address=0xFFFF;
+ device->reg_length=0x0000;
+ device->sync_bulk_address=0x0000;
+ device->sync_bulk_length=0x0000;
+ device->sync_bulk_prev_id=0x00;
+ device->sync_read_pending=0x00;
+ device->bulk_read_pending=0x00;
+
+ /* initialize memory module */
+ mem_init(&device->memory,address);
+ mem_module_init(&device->mem_module);
+ device->mem_module.data=device;
+ device->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_device_write_cmd;
+ device->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_device_read_cmd;
+ if(!mem_module_add_eeprom_segment(&device->mem_module,eeprom_base_address1,EEPROM_DYN_SLAVE_LENGTH1))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&device->mem_module,eeprom_base_address2,EEPROM_DYN_SLAVE_LENGTH2))
+ return 0x00;
+ if(!mem_add_module(&device->memory,&device->mem_module))
+ return 0x00;
+ device->eeprom_base_address1=eeprom_base_address1;
+ device->eeprom_base_address2=eeprom_base_address2;
+
+ return 0x01;
+}
+
+void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
+{
+ if(device!=0x00000000)
+ device->address=address;
+}
+
+unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->address;
+ else
+ return 0xFF;
+}
+
+void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
+{
+ if(device!=0x00000000)
+ device->return_delay=delay;
+}
+
+unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->return_delay;
+ else
+ return 0xFF;
+}
+
+void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
+{
+ if(device!=0x00000000)
+ device->return_level=level;
+}
+
+return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->return_level;
+ else
+ return return_all;
+}
+
+TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device)
+{
+ return &device->memory;
+}
diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile
index 4ebb37992b0d459fc28f3864365f430b99e11ee1..020c8182f704eec57c52c1efec5a4c7590ff25d3 100755
--- a/dynamixel_manager/Makefile
+++ b/dynamixel_manager/Makefile
@@ -18,6 +18,8 @@ MEMORY_PATH = ../memory
INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include
+MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8
+
TCHAIN_PREFIX=arm-none-eabi-
CC = $(TCHAIN_PREFIX)gcc
@@ -33,8 +35,8 @@ DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a
SRC_DIR=./src/
SRC=$(wildcard $(SRC_DIR)*.c)
-SRC_DIR_MODULES=./src/modules/
-SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
+#SRC_DIR_MODULES=./src/modules/
+#SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))
@@ -54,21 +56,21 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM
all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0) -o $@ $<
$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0) -o $@ $<
$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0plus) -o $@ $<
$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0plus) -o $@ $<
$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M3) -o $@ $<
$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M3) -o $@ $<
mkdir_build:
mkdir -p build/m4_fpu
mkdir -p build/m0
diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
index b9f03388a2dcf3461dcad06f3d654ff32a2c3adf..b5c8cff1085b6254a1b1b635de67f422e1b78287 100644
--- a/dynamixel_manager/include/dyn_manager.h
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -6,22 +6,22 @@
#include "memory.h"
#ifndef DYN_MANAGER_MAX_NUM_MASTERS
- #define DYN_MANAGER_MAX_NUM_MASTERS 4
+ #error "Please, specify the maximum number of masters with the DYN_MANAGER_MAX_NUM_MASTERS macro"
#endif
#ifndef DYN_MANAGER_MAX_NUM_MODULES
- #define DYN_MANAGER_MAX_NUM_MODULES 8
+ #error "Please, specify the maximum number of modules with the DYN_MANAGER_MAX_NUM_MODULES macro"
#endif
#ifndef DYN_MANAGER_MAX_NUM_DEVICES
- #define DYN_MANAGER_MAX_NUM_DEVICES 32
+ #error "Please, specify the maximum number of devices with the DYN_MANAGER_MAX_NUM_DEVICES macro"
#endif
#ifndef DYN_MANAGER_MAX_NUM_SINGLE_OP
- #define DYN_MANAGER_MAX_NUM_SINGLE_OP 16
+ #error "Please, specify the maximum number of single operations with the DYN_MANAGER_MAX_NUM_SINGLE_OP macro"
#endif
#ifndef DYN_MANAGER_MAX_NUM_SYNC_OP
- #define DYN_MANAGER_MAX_NUM_SYNC_OP 4
+ #error "Please, specify the maximum number of sync operations with the DYN_MANAGER_MAX_NUM_SYNC_OP macro"
#endif
#ifndef DYN_MANAGER_MAX_NUM_BULK_OP
- #define DYN_MANAGER_MAX_NUM_BULK_OP 4
+ #error "Please, specify the maximum number of bulk operations with the DYN_MANAGER_MAX_NUM_BULK_OP macro"
#endif
#define DYN_MANAGER_MAX_NUM_OP ((DYN_MANAGER_MAX_NUM_SINGLE_OP+DYN_MANAGER_MAX_NUM_SYNC_OP+DYN_MANAGER_MAX_NUM_BULK_OP)*DYN_MANAGER_MAX_NUM_MASTERS)
@@ -101,6 +101,8 @@ typedef struct{
struct TDynModule;
typedef struct{
+ unsigned short int eeprom_base_address;
+ unsigned short int ram_base_address;
TDynamixelMaster *masters[DYN_MANAGER_MAX_NUM_MASTERS];
TDynManagerOps operations[DYN_MANAGER_MAX_NUM_MASTERS];
unsigned char num_masters;
@@ -113,11 +115,12 @@ typedef struct{
unsigned short int period_us;
void (*init_timer)(void);
void (*set_period)(unsigned short int period_us);
+ TMemory *memory;
TMemModule mem_module;
}TDynManager;
// public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory);
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address);
void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us);
unsigned short int dyn_manager_get_period(TDynManager *manager);
void dyn_manager_scan(TDynManager *manager);
diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h
index 7099cc7d5ce6a4292d3a7b7c2c96164bbe2e0264..c7d360be6c57731f044ba572f5d6af9b17b51455 100644
--- a/dynamixel_manager/include/dyn_manager_registers.h
+++ b/dynamixel_manager/include/dyn_manager_registers.h
@@ -1,26 +1,18 @@
#ifndef _DYN_MANAGER_REGISTERS_H
#define _DYN_MANAGER_REGISTERS_H
-#ifndef RAM_DYN_MANAGER_BASE_ADDRESS
- #define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0000)
-#endif
-
-#ifndef EEPROM_DYN_MANAGER_BASE_ADDRESS
- #define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0000)
-#endif
-
#define RAM_DYN_MANAGER_LENGTH 2
#define EEPROM_DYN_MANAGER_LENGTH 2
-#define DYN_MANAGER_PERIOD EEPROM_DYN_MANAGER_BASE_ADDRESS
+#define DYN_MANAGER_PERIOD 0
-#define DYN_MANAGER_NUM_MODULES RAM_DYN_MANAGER_BASE_ADDRESS
-#define DYN_MANAGER_NUM_MASTERS (RAM_DYN_MANAGER_BASE_ADDRESS+1)
+#define DYN_MANAGER_NUM_MODULES 0
+#define DYN_MANAGER_NUM_MASTERS 1
-#ifndef DEFAULT_DYN_MANAGER_PERIOD
- #define DEFAULT_DYN_MANAGER_PERIOD 0x1E78
-#endif
+#define dyn_manager_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD, \
+ (DEFAULT_PERIOD>>8)&0x00FF,base_address+DYN_MANAGER_PERIOD+1};
#endif
diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h
index f4c6d44fad8636bfe75fb69f09f4484344900090..a79d6c6a7669d5c3a3405ac8d2546fb08e2a3ff9 100644
--- a/dynamixel_manager/include/dyn_module.h
+++ b/dynamixel_manager/include/dyn_module.h
@@ -2,7 +2,7 @@
#define _DYN_MODULE_H
#ifndef MODULE_MAX_NUM_MODELS
- #define MODULE_MAX_NUM_MODELS 32
+ #error "Please, specify the maximum number of dynamixel master modules with the MODULE_MAX_NUM_MODELS macro"
#endif
#include "dyn_manager.h"
diff --git a/dynamixel_manager/include/dyn_module_registers.h b/dynamixel_manager/include/dyn_module_registers.h
index c4c7afdf264b1289021c6f3478da61db963cfe8e..fe27dbb309ebefbe6eb880dd716b8ce44518836e 100644
--- a/dynamixel_manager/include/dyn_module_registers.h
+++ b/dynamixel_manager/include/dyn_module_registers.h
@@ -5,15 +5,14 @@
#define EEPROM_DYN_MODULE_LENGTH 1
-#define DYN_MODULE_PERIOD EEPROM_DYN_MODULE_BASE_ADDRESS
+#define DYN_MODULE_PERIOD 0
-#define DYN_MODULE_CNTRL RAM_DYN_MODULE_BASE_ADDRESS
-#define DYN_MODULE_NUM_MODELS (RAM_DYN_MODULE_BASE_ADDRESS+1)
-#define DYN_MODULE_NUM_DEVICES (RAM_DYN_MODULE_BASE_ADDRESS+2)
+#define DYN_MODULE_CNTRL 0
+#define DYN_MODULE_NUM_MODELS 1
+#define DYN_MODULE_NUM_DEVICES 2
-#ifndef DEFAULT_DYN_MODULE_PERIOD
- #define DEFAULT_DYN_MODULE_PERIOD 0x01
-#endif
+#define dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD};
#endif
diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h
index b5d3bbe2e32678e5d0c055bb3bd27d08c02fa9c8..1a2e9aa525ec2a72902713300277b41e484ca573 100644
--- a/dynamixel_manager/include/modules/motion_manager.h
+++ b/dynamixel_manager/include/modules/motion_manager.h
@@ -4,10 +4,7 @@
#include "dyn_module.h"
#ifndef MM_MAX_NUM_MOTION_MODULES
- #define MM_MAX_NUM_MOTION_MODULES 8
-#endif
-#ifdef MM_MAX_NUM_MODELS
- #define MODULE_MAX_NUM_MODELS MM_MAX_NUM_MODELS
+ #error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro"
#endif
typedef enum {MM_NONE = -1,
diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h
index 208e6b810965e8a28d47268820f151b25498137e..02fbc9ed572a7d596bada1c73b517df5bb13815a 100644
--- a/dynamixel_manager/include/modules/motion_manager_registers.h
+++ b/dynamixel_manager/include/modules/motion_manager_registers.h
@@ -3,60 +3,14 @@
#include "dyn_module_registers.h"
-#ifndef RAM_MM_BASE_ADDRESS
- #define RAM_MM_BASE_ADDRESS ((unsigned short int)0x0000)
-#endif
-
-#ifndef EEPROM_MM_BASE_ADDRESS
- #define EEPROM_MM_BASE_ADDRESS ((unsigned short int)0x0000)
-#endif
-
-#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH + )
+#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH + 0)
-#define EEPROM_MM_LENGTH (EEPROM_DYN_MODULE_LENGTH + )
+#define EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES)
-#define MM_PERIOD DYN_MODULE_PERIOD
-
-#define MM_CNTRL DYN_MODULE_CNTRL
-#define MM_NUM_MODELS DYN_MODULE_NUM_MODELS
-#define MM_NUM_DEVICES DYN_MODULE_NUM_DEVICES
-
-#ifndef DEFAULT_MM_PERIOD
- #define DEFAULT_MM_PERIOD DEFAULT_DYN_MODULE_PERIOD
-#endif
-#define MM_SERVO0_OFFSET ((unsigned short int)0x0011)
-#define MM_SERVO1_OFFSET ((unsigned short int)0x0012)
-#define MM_SERVO2_OFFSET ((unsigned short int)0x0013)
-#define MM_SERVO3_OFFSET ((unsigned short int)0x0014)
-#define MM_SERVO4_OFFSET ((unsigned short int)0x0015)
-#define MM_SERVO5_OFFSET ((unsigned short int)0x0016)
-#define MM_SERVO6_OFFSET ((unsigned short int)0x0017)
-#define MM_SERVO7_OFFSET ((unsigned short int)0x0018)
-#define MM_SERVO8_OFFSET ((unsigned short int)0x0019)
-#define MM_SERVO9_OFFSET ((unsigned short int)0x001A)
-#define MM_SERVO10_OFFSET ((unsigned short int)0x001B)
-#define MM_SERVO11_OFFSET ((unsigned short int)0x001C)
-#define MM_SERVO12_OFFSET ((unsigned short int)0x001D)
-#define MM_SERVO13_OFFSET ((unsigned short int)0x001E)
-#define MM_SERVO14_OFFSET ((unsigned short int)0x001F)
-#define MM_SERVO15_OFFSET ((unsigned short int)0x0020)
-#define MM_SERVO16_OFFSET ((unsigned short int)0x0021)
-#define MM_SERVO17_OFFSET ((unsigned short int)0x0022)
-#define MM_SERVO18_OFFSET ((unsigned short int)0x0023)
-#define MM_SERVO19_OFFSET ((unsigned short int)0x0024)
-#define MM_SERVO20_OFFSET ((unsigned short int)0x0025)
-#define MM_SERVO21_OFFSET ((unsigned short int)0x0026)
-#define MM_SERVO22_OFFSET ((unsigned short int)0x0027)
-#define MM_SERVO23_OFFSET ((unsigned short int)0x0028)
-#define MM_SERVO24_OFFSET ((unsigned short int)0x0029)
-#define MM_SERVO25_OFFSET ((unsigned short int)0x002A)
-#define MM_SERVO26_OFFSET ((unsigned short int)0x002B)
-#define MM_SERVO27_OFFSET ((unsigned short int)0x002C)
-#define MM_SERVO28_OFFSET ((unsigned short int)0x002D)
-#define MM_SERVO29_OFFSET ((unsigned short int)0x002E)
-#define MM_SERVO30_OFFSET ((unsigned short int)0x002F)
-#define MM_SERVO31_OFFSET ((unsigned short int)0x0030)
+#define MM_SERVO_OFFSET 0
+#define dyn_mm_eeprom_data(name,section_name,base_address) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={0,base_address+MM_SERVO_OFFSET};
#endif
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 8499d2c1a5130a45251b9d02167c47bba4e6338d..73db6b175604463e9a7c863b62610ea24b11530d 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -37,7 +37,7 @@ void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsign
void dyn_manager_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
- ram_read_table(address,length,data);
+ ram_read_table(module->memory,address,length,data);
}
unsigned char dyn_manager_check_id(TDynManager *manager,unsigned char id)
@@ -211,7 +211,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
}
// public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory)
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address)
{
unsigned char i,j,k;
@@ -284,19 +284,20 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory)
/* initialize timer */
if(manager->init_timer!=0x00000000)
manager->init_timer();
- manager->set_period(DEFAULT_DYN_MANAGER_PERIOD);
/* initialize memory module */
mem_module_init(&manager->mem_module);
manager->mem_module.data=manager;
manager->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_manager_write_cmd;
manager->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_manager_read_cmd;
- if(!mem_module_add_ram_segment(&manager->mem_module,RAM_DYN_MANAGER_BASE_ADDRESS,RAM_DYN_MANAGER_LENGTH))
+ if(!mem_module_add_ram_segment(&manager->mem_module,ram_base_address,RAM_DYN_MANAGER_LENGTH))
return 0x00;
- if(!mem_module_add_eeprom_segment(&manager->mem_module,EEPROM_DYN_MANAGER_BASE_ADDRESS,EEPROM_DYN_MANAGER_LENGTH))
+ if(!mem_module_add_eeprom_segment(&manager->mem_module,eeprom_base_address,EEPROM_DYN_MANAGER_LENGTH))
return 0x00;
if(!mem_add_module(memory,&manager->mem_module))
return 0x00;
+ manager->eeprom_base_address=eeprom_base_address;
+ manager->ram_base_address=ram_base_address;
return 0x01;
}
@@ -312,8 +313,8 @@ void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us)
if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
manager->modules[i]->set_period(manager->modules[i]->data,period_us);
manager->period_us=period_us;
- ram_data[DYN_MANAGER_PERIOD]=period_us%256;
- ram_data[DYN_MANAGER_PERIOD+1]=period_us/256;
+ manager->memory->data[DYN_MANAGER_PERIOD]=period_us%256;
+ manager->memory->data[DYN_MANAGER_PERIOD+1]=period_us/256;
}
}
@@ -368,7 +369,7 @@ void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master)
{
manager->masters[i]=master;
manager->num_masters++;
- ram_data[DYN_MANAGER_NUM_MASTERS]++;
+ manager->memory->data[DYN_MANAGER_NUM_MASTERS]++;
}
}
@@ -402,7 +403,7 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module)
manager->modules[i]=module;
manager->num_modules++;
module->manager=manager;
- ram_data[DYN_MANAGER_NUM_MODULES]++;
+ manager->memory->data[DYN_MANAGER_NUM_MODULES]++;
}
}
diff --git a/f1/usart/src/usart1_remap.c b/f1/usart/src/usart1_remap.c
index 9d17fb30df8ca20040f79212950894c20e0ff05f..5c5427c1687f71d9767471219143a15dfd84d1b6 100644
--- a/f1/usart/src/usart1_remap.c
+++ b/f1/usart/src/usart1_remap.c
@@ -275,6 +275,12 @@ void usart1_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities)
}
}
+void usart1_set_baudrate(TComm *comm_dev,unsigned int baudrate)
+{
+ Uart1Handle.Init.BaudRate = baudrate;
+ HAL_UART_Init(&Uart1Handle);
+}
+
/* IRQ functions */
unsigned char usart1_send_irq(unsigned char first_byte)
{
diff --git a/memory/include/memory.h b/memory/include/memory.h
index 1829fee81dbbb353df3cc2791584e24bde51e00a..e01445f9abe68e5c69b74d720e65f5daf1ab78bf 100644
--- a/memory/include/memory.h
+++ b/memory/include/memory.h
@@ -15,11 +15,15 @@ typedef struct
{
unsigned char num_mem_modules;
TMemModule *mem_modules[MAX_NUM_MEM_MODULES];
+ unsigned char device_id;
unsigned short int (*eeprom_write_data)(unsigned short int address,unsigned short int data);
unsigned short int (*eeprom_read_data)(unsigned short int address,unsigned short int *data);
+ unsigned char data[RAM_SIZE];
+ unsigned char total_eeprom;
+ unsigned char total_ram;
}TMemory;
-void mem_init(TMemory *memory);
+void mem_init(TMemory *memory,unsigned char device_id);
void mem_initialize_data(TMemory *memory);
unsigned char mem_add_module(TMemory *memory, TMemModule *module);
void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned short int length,unsigned char *data);
diff --git a/memory/include/ram.h b/memory/include/ram.h
index ecc663695a1d6fa81089cbc8ee53c41889b9e683..1aa611f1bf238e8b1779bccfdccfcd54236d5579 100644
--- a/memory/include/ram.h
+++ b/memory/include/ram.h
@@ -13,17 +13,15 @@ extern "C" {
#define RAM_BAD_BIT -3
#define RAM_BAD_ACCESS -4
-extern unsigned char ram_data[RAM_SIZE];
-
unsigned char ram_init(TMemory *memory);
-inline void ram_read_byte(unsigned short int address, unsigned char *data);
-inline void ram_read_word(unsigned short int address, unsigned short int *data);
-unsigned char ram_read_table(unsigned short int address, unsigned short int length,unsigned char *data);
-unsigned char ram_set_bit(unsigned short int address, unsigned char bit);
-unsigned char ram_clear_bit(unsigned short int address, unsigned char bit);
-unsigned char ram_write_byte(unsigned short int address, unsigned char data);
-unsigned char ram_write_word(unsigned short int address, unsigned short int data);
-unsigned char ram_write_table(unsigned short int address, unsigned short int length,unsigned char *data);
+inline void ram_read_byte(TMemory *memory,unsigned short int address, unsigned char *data);
+inline void ram_read_word(TMemory *memory,unsigned short int address, unsigned short int *data);
+unsigned char ram_read_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data);
+unsigned char ram_set_bit(TMemory *memory,unsigned short int address, unsigned char bit);
+unsigned char ram_clear_bit(TMemory *memory,unsigned short int address, unsigned char bit);
+unsigned char ram_write_byte(TMemory *memory,unsigned short int address, unsigned char data);
+unsigned char ram_write_word(TMemory *memory,unsigned short int address, unsigned short int data);
+unsigned char ram_write_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data);
inline unsigned char ram_in_range(unsigned short int reg,unsigned short int address,unsigned short int length);
unsigned char ram_in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length);
diff --git a/memory/src/memory.c b/memory/src/memory.c
index 68fbc806be5d15e471023bd8665b03b89caab7fb..af30fbf36f18d9c31e08b50fa672aa12e2b4cf57 100644
--- a/memory/src/memory.c
+++ b/memory/src/memory.c
@@ -1,10 +1,6 @@
#include "memory.h"
#include "mem_module.h"
-/* private variables */
-unsigned char memory_total_eeprom;
-unsigned char memory_total_ram;
-
/* private functions */
unsigned char mem_in_window(unsigned short int mem_start_address,unsigned short int mem_length,unsigned short int start_address,unsigned short int length)
{
@@ -21,7 +17,7 @@ unsigned char mem_in_window(unsigned short int mem_start_address,unsigned short
}
/* public functions */
-void mem_init(TMemory *memory)
+void mem_init(TMemory *memory,unsigned char device_id)
{
unsigned char i;
@@ -30,10 +26,11 @@ void mem_init(TMemory *memory)
memory->mem_modules[i]=0x00000000;
memory->eeprom_write_data=0x00000000;
memory->eeprom_read_data=0x00000000;
+ memory->device_id=device_id;
/* initialize internal variables */
- memory_total_eeprom=0;
- memory_total_ram=0;
+ memory->total_eeprom=0;
+ memory->total_ram=0;
}
void mem_initialize_data(TMemory *memory)
@@ -48,7 +45,7 @@ void mem_initialize_data(TMemory *memory)
{
for(i=0;i<EEPROM_SIZE;i++)
{
- if(memory->eeprom_read_data(i,&data))
+ if(memory->eeprom_read_data((memory->device_id<<8)+i,&data))
eeprom_data[i]=0x00;
else
eeprom_data[i]=data&0x00FF;
@@ -122,17 +119,17 @@ unsigned char mem_add_module(TMemory *memory, TMemModule *module)
// check available memory
for(new_seg=0;new_seg<module->num_ram_segments;new_seg++)
{
- memory_total_ram+=module->ram_segments[new_seg].length;
- if(memory_total_ram>RAM_SIZE)
+ memory->total_ram+=module->ram_segments[new_seg].length;
+ if(memory->total_ram>RAM_SIZE)
return 0x00;
}
for(new_seg=0;new_seg<module->num_eeprom_segments;new_seg++)
{
- memory_total_eeprom+=module->eeprom_segments[new_seg].length;
- if(memory_total_eeprom>EEPROM_SIZE)
+ memory->total_eeprom+=module->eeprom_segments[new_seg].length;
+ if(memory->total_eeprom>EEPROM_SIZE)
return 0x00;
- memory_total_ram+=module->eeprom_segments[new_seg].length;
- if(memory_total_ram>RAM_SIZE)
+ memory->total_ram+=module->eeprom_segments[new_seg].length;
+ if(memory->total_ram>RAM_SIZE)
return 0x00;
}
@@ -195,7 +192,7 @@ void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned shor
mem_module->write_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]);
if(memory->eeprom_write_data!=0x00000000)
for(k=actual_address;k<(actual_address+actual_length);k++)
- memory->eeprom_write_data(k,data[k-start_address]);
+ memory->eeprom_write_data((memory->device_id<<8)+k,data[k-start_address]);
}
}
}
diff --git a/memory/src/ram.c b/memory/src/ram.c
index 444c73cb126562ff90a9daecb34deadf900cf33a..326ab7fd02830c2506f144983ebadf33558fd864 100644
--- a/memory/src/ram.c
+++ b/memory/src/ram.c
@@ -1,7 +1,5 @@
#include "ram.h"
-unsigned char ram_data[RAM_SIZE]={0};
-
unsigned char ram_init(TMemory *memory)
{
mem_initialize_data(memory);
@@ -9,66 +7,66 @@ unsigned char ram_init(TMemory *memory)
return 0x01;
}
-inline void ram_read_byte(unsigned short int address,unsigned char *data)
+inline void ram_read_byte(TMemory *memory,unsigned short int address,unsigned char *data)
{
- (*data)=ram_data[address];
+ (*data)=memory->data[address];
}
-inline void ram_read_word(unsigned short int address, unsigned short int *data)
+inline void ram_read_word(TMemory *memory,unsigned short int address, unsigned short int *data)
{
- (*data)=ram_data[address];
- (*data)+=ram_data[address+1]*256;
+ (*data)=memory->data[address];
+ (*data)+=memory->data[address+1]*256;
}
-unsigned char ram_read_table(unsigned short int address, unsigned short int length,unsigned char *data)
+unsigned char ram_read_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data)
{
unsigned short int i;
if((address+length)<=(RAM_SIZE-1))
{
for(i=0;i<length;i++)
- data[i]=ram_data[address+i];
+ data[i]=memory->data[address+i];
return RAM_SUCCESS;
}
else
return RAM_BAD_ADDRESS;
}
-unsigned char ram_set_bit(unsigned short int address, unsigned char bit)
+unsigned char ram_set_bit(TMemory *memory,unsigned short int address, unsigned char bit)
{
if(bit>=0 && bit<8)
{
- ram_data[address]|=(0x01<<bit);
+ memory->data[address]|=(0x01<<bit);
return RAM_SUCCESS;
}
else
return RAM_BAD_BIT;
}
-unsigned char ram_clear_bit(unsigned short int address, unsigned char bit)
+unsigned char ram_clear_bit(TMemory *memory,unsigned short int address, unsigned char bit)
{
if(bit>=0 && bit<8)
{
- ram_data[address]&=(~(0x01<<bit));
+ memory->data[address]&=(~(0x01<<bit));
return RAM_SUCCESS;
}
else
return RAM_BAD_BIT;
}
-unsigned char ram_write_byte(unsigned short int address, unsigned char data)
+unsigned char ram_write_byte(TMemory *memory,unsigned short int address, unsigned char data)
{
- ram_data[address]=data;
+ memory->data[address]=data;
return RAM_SUCCESS;
}
-unsigned char ram_write_word(unsigned short int address, unsigned short int data)
+unsigned char ram_write_word(TMemory *memory,unsigned short int address, unsigned short int data)
{
if(address < (RAM_SIZE-1))
{
- ram_data[address]=data%256;
- ram_data[address+1]=data/256;
+ memory->data[address]=data%256;
+ memory->data[address+1]=data/256;
return RAM_SUCCESS;
}
@@ -76,14 +74,14 @@ unsigned char ram_write_word(unsigned short int address, unsigned short int data
return RAM_BAD_ADDRESS;
}
-unsigned char ram_write_table(unsigned short int address, unsigned short int length,unsigned char *data)
+unsigned char ram_write_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data)
{
unsigned short int i;
if((address+length)<RAM_SIZE)
{
for(i=0;i<length;i++)
- ram_data[address+i]=data[i];
+ memory->data[address+i]=data[i];
return RAM_SUCCESS;
}
else
diff --git a/pattern_frame_buffer/include/frame_buffer.h b/pattern_frame_buffer/include/frame_buffer.h
index 6b48631151c97075bff569fdc8c376095b4c3f50..aca02b27815ec1ae5a128b4eb32a522c67dcde83 100644
--- a/pattern_frame_buffer/include/frame_buffer.h
+++ b/pattern_frame_buffer/include/frame_buffer.h
@@ -5,6 +5,8 @@
extern "C" {
#endif
+#include "memory.h"
+
#ifndef FB_MAX_BUFFER_LEN
#error "Please, specify the maximum frame buffer length with the FB_MAX_BUFFER_LEN macro"
#endif
@@ -13,6 +15,7 @@ extern "C" {
typedef struct
{
+ TMemory *memory;
unsigned short int ram_base_address;
unsigned short int eeprom_base_address;
unsigned short int num_rows;
@@ -23,7 +26,7 @@ typedef struct
unsigned char pixel_buffer[FB_MAX_BUFFER_LEN];
}TFrameBuffer;
-void frame_buffer_init(TFrameBuffer *fb,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+void frame_buffer_init(TFrameBuffer *fb,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
void frame_buffer_set_num_rows(TFrameBuffer *fb,unsigned short int rows);
unsigned short int frame_buffer_get_num_rows(TFrameBuffer *fb);
void frame_buffer_set_pixels_per_row(TFrameBuffer *fb,unsigned short int pixels);
diff --git a/pattern_frame_buffer/include/frame_buffer_control.h b/pattern_frame_buffer/include/frame_buffer_control.h
index ed4deeed49e34676641ff1bf7423c07a28a3f337..934b906a7c40b29c483c62a814d9406a3a883466 100644
--- a/pattern_frame_buffer/include/frame_buffer_control.h
+++ b/pattern_frame_buffer/include/frame_buffer_control.h
@@ -32,6 +32,7 @@ typedef struct TFBControl
TScheduler *scheduler;
sched_channel_t sch_channel;
unsigned short int period_ms;
+ TMemory *memory;
TMemModule mem_module;
unsigned short int ram_base_address;
unsigned short int eeprom_base_address;
diff --git a/pattern_frame_buffer/include/frame_buffer_control_registers.h b/pattern_frame_buffer/include/frame_buffer_control_registers.h
index f44d32dd4e0a7187e457a68f0d1ac9294f072b83..1abce5667ef6cf0caf32b392cdcabc755bfbe948 100644
--- a/pattern_frame_buffer/include/frame_buffer_control_registers.h
+++ b/pattern_frame_buffer/include/frame_buffer_control_registers.h
@@ -48,15 +48,15 @@
#define DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS 0x0001
#endif
-#define frame_buffer_control_eeprom_data(name,section_name,base_address) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_FRAME_BUFFER_CONTROL_PERIOD&0x00FF,base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET, \
- (DEFAULT_FRAME_BUFFER_CONTROL_PERIOD>>8)&0x00FF,base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET+1, \
- DEFAULT_FRAME_BUFFER_CONTROL_NUM_ROWS&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET, \
- (DEFAULT_FRAME_BUFFER_CONTROL_NUM_ROWS>>8)&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET+1, \
- DEFAULT_FRAME_BUFFER_CONTROL_NUM_PIXELS&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET, \
- (DEFAULT_FRAME_BUFFER_CONTROL_NUM_PIXELS>>8)&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET+1, \
- DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET, \
- (DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS>>8)&0x00FF,base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET+1};
+#define frame_buffer_control_eeprom_data(name,section_name,base_address,device_id) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_FRAME_BUFFER_CONTROL_PERIOD&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET, \
+ (DEFAULT_FRAME_BUFFER_CONTROL_PERIOD>>8)&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET+1, \
+ DEFAULT_FRAME_BUFFER_CONTROL_NUM_ROWS&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET, \
+ (DEFAULT_FRAME_BUFFER_CONTROL_NUM_ROWS>>8)&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET+1, \
+ DEFAULT_FRAME_BUFFER_CONTROL_NUM_PIXELS&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET, \
+ (DEFAULT_FRAME_BUFFER_CONTROL_NUM_PIXELS>>8)&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET+1, \
+ DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET, \
+ (DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS>>8)&0x00FF,(device_id<<8)+base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET+1};
#endif
diff --git a/pattern_frame_buffer/include/image_patterns.h b/pattern_frame_buffer/include/image_patterns.h
index 69696474c272258aeb2b0e0c7b105b83a7b626bc..d61001fc0ae404aeb17a6403ecb97e37213825af 100644
--- a/pattern_frame_buffer/include/image_patterns.h
+++ b/pattern_frame_buffer/include/image_patterns.h
@@ -3,6 +3,7 @@
#include "patterns.h"
#include "frame_buffer_control.h"
+#include "memory.h"
#ifndef IMG_MAX_NUM_IMAGES
#error "Please, specify the maximum number of images with the IMG_MAX_NUM_IMAGES macro"
@@ -20,7 +21,7 @@
extern unsigned char img_pattern_data[IMG_MAX_NUM_IMAGES][IMG_MAX_IMAGE_WIDTH][IMG_MAX_IMAGE_HEIGHT][3];
-void img_patterns_init(unsigned short int ram_base_address);
+void img_patterns_init(TMemory *memory,unsigned short int ram_base_address);
#pragma pack (push, 1)
typedef struct
diff --git a/pattern_frame_buffer/src/frame_buffer.c b/pattern_frame_buffer/src/frame_buffer.c
index 4e35ec0c62296d8b1c294fde8d6e280acb18deac..ec208cadcc8e039f16d594f3f1677f5d7e1545ab 100644
--- a/pattern_frame_buffer/src/frame_buffer.c
+++ b/pattern_frame_buffer/src/frame_buffer.c
@@ -7,12 +7,12 @@ void frame_buffer_compute_free_memory(TFrameBuffer *fb)
{
fb->buffer_size=fb->num_rows*fb->num_pixels_per_row*FB_BYTES_PER_PIXEL;
fb->free_mem=FB_MAX_BUFFER_LEN-fb->buffer_size*fb->num_buffers;
- ram_data[fb->ram_base_address+FRAME_BUFFER_CONTROL_FREE_MEMORY_OFFSET]=fb->free_mem&0x00FF;
- ram_data[fb->ram_base_address+FRAME_BUFFER_CONTROL_FREE_MEMORY_OFFSET+1]=(fb->free_mem&0xFF00)>>8;
+ fb->memory->data[fb->ram_base_address+FRAME_BUFFER_CONTROL_FREE_MEMORY_OFFSET]=fb->free_mem&0x00FF;
+ fb->memory->data[fb->ram_base_address+FRAME_BUFFER_CONTROL_FREE_MEMORY_OFFSET+1]=(fb->free_mem&0xFF00)>>8;
}
// public functions
-void frame_buffer_init(TFrameBuffer *fb,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
+void frame_buffer_init(TFrameBuffer *fb,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
unsigned int i;
@@ -22,6 +22,7 @@ void frame_buffer_init(TFrameBuffer *fb,unsigned short int ram_base_address,unsi
frame_buffer_set_num_buffers(fb,DEFAULT_FRAME_BUFFER_CONTROL_NUM_BUFFERS);
fb->ram_base_address=ram_base_address;
fb->eeprom_base_address=eeprom_base_address;
+ fb->memory=memory;
for(i=0;i<FB_MAX_BUFFER_LEN;i++)
fb->pixel_buffer[i]=0x00;
@@ -30,8 +31,8 @@ void frame_buffer_init(TFrameBuffer *fb,unsigned short int ram_base_address,unsi
void frame_buffer_set_num_rows(TFrameBuffer *fb,unsigned short int rows)
{
fb->num_rows=rows;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET]=rows&0x00FF;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET+1]=(rows&0xFF00)>>8;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET]=rows&0x00FF;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_ROWS_OFFSET+1]=(rows&0xFF00)>>8;
frame_buffer_compute_free_memory(fb);
}
@@ -43,8 +44,8 @@ unsigned short int frame_buffer_get_num_rows(TFrameBuffer *fb)
void frame_buffer_set_pixels_per_row(TFrameBuffer *fb,unsigned short int pixels)
{
fb->num_pixels_per_row=pixels;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET]=pixels&0x00FF;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET+1]=(pixels&0xFF00)>>8;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET]=pixels&0x00FF;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_PIXELS_OFFSET+1]=(pixels&0xFF00)>>8;
frame_buffer_compute_free_memory(fb);
}
@@ -56,8 +57,8 @@ unsigned short int frame_buffer_get_pixels_per_row(TFrameBuffer *fb)
void frame_buffer_set_num_buffers(TFrameBuffer *fb,unsigned char num)
{
fb->num_buffers=num;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET]=num&0x00FF;
- ram_data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET+1]=(num&0xFF00)>>8;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET]=num&0x00FF;
+ fb->memory->data[fb->eeprom_base_address+FRAME_BUFFER_CONTROL_NUM_BUFFERS_OFFSET+1]=(num&0xFF00)>>8;
frame_buffer_compute_free_memory(fb);
}
diff --git a/pattern_frame_buffer/src/frame_buffer_control.c b/pattern_frame_buffer/src/frame_buffer_control.c
index 12efbd809630cd1fe262fc46e5c4f34b7271bd68..f1e2277e793566d65bee38a062a5ce23e7487827 100644
--- a/pattern_frame_buffer/src/frame_buffer_control.c
+++ b/pattern_frame_buffer/src/frame_buffer_control.c
@@ -161,7 +161,9 @@ void frame_buffer_control_write_cmd(void *module,unsigned short int address,unsi
void frame_buffer_control_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
- ram_read_table(address,length,data);
+ TFBControl *control=(TFBControl *)module;
+
+ ram_read_table(control->memory,address,length,data);
}
void frame_buffer_control_scheduler(void *data)
@@ -180,7 +182,7 @@ unsigned char frame_buffer_control_init(TFBControl *control,TMemory *memory,TSch
// initialize internal variables
frame_buffer_control_clear_patterns(control);
frame_buffer_control_clear_copy_functions(control);
- frame_buffer_init(&control->frame_buffer,ram_base_address,eeprom_base_address);
+ frame_buffer_init(&control->frame_buffer,memory,ram_base_address,eeprom_base_address);
control->ram_base_address=ram_base_address;
control->eeprom_base_address=eeprom_base_address;
@@ -189,13 +191,14 @@ unsigned char frame_buffer_control_init(TFBControl *control,TMemory *memory,TSch
control->scheduler=scheduler;
control->sch_channel=ch;
- img_patterns_init(ram_base_address);
+ img_patterns_init(memory,ram_base_address);
/* initialize memory module */
mem_module_init(&control->mem_module);
control->mem_module.write_cmd=frame_buffer_control_write_cmd;
control->mem_module.read_cmd=frame_buffer_control_read_cmd;
control->mem_module.data=control;
+ control->memory=memory;
if(!mem_module_add_ram_segment(&control->mem_module,ram_base_address,RAM_FRAME_BUFFER_CONTROL_LENGTH))
return 0x00;
if(!mem_module_add_eeprom_segment(&control->mem_module,eeprom_base_address,EEPROM_FRAME_BUFFER_CONTROL_LENGTH))
@@ -209,20 +212,20 @@ unsigned char frame_buffer_control_init(TFBControl *control,TMemory *memory,TSch
void frame_buffer_control_start(TFBControl *control)
{
scheduler_enable_channel(control->scheduler,control->sch_channel);
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_OFFSET]|=FRAME_BUFFER_CONTROL_RUNNING;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_OFFSET]|=FRAME_BUFFER_CONTROL_RUNNING;
}
void frame_buffer_control_stop(TFBControl *control)
{
scheduler_disable_channel(control->scheduler,control->sch_channel);
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_OFFSET]&=(~FRAME_BUFFER_CONTROL_RUNNING);
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_OFFSET]&=(~FRAME_BUFFER_CONTROL_RUNNING);
}
void frame_buffer_control_set_period(TFBControl *control,unsigned short int period_ms)
{
scheduler_change_period(control->scheduler,control->sch_channel,period_ms);
- ram_data[control->eeprom_base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET]=period_ms&0x00FF;
- ram_data[control->eeprom_base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET+1]=(period_ms&0xFF00)>>8;
+ control->memory->data[control->eeprom_base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET]=period_ms&0x00FF;
+ control->memory->data[control->eeprom_base_address+FRAME_BUFFER_CONTROL_PERIOD_OFFSET+1]=(period_ms&0xFF00)>>8;
control->period_ms=period_ms;
}
@@ -241,22 +244,22 @@ unsigned char frame_buffer_control_add_pattern(TFBControl *control,pattern_func_
num_cols=frame_buffer_get_pixels_per_row(&control->frame_buffer);
if(area->min_row>num_rows || area->max_row>num_rows || area->min_row>area->max_row)
{
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
return 0xFF;
}
if(area->min_col>num_cols || area->max_col>num_cols || area->min_col>area->max_col)
{
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
return 0xFF;
}
if(data_length>FBC_MAX_DATA_PATTERN_LEN)
{
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
return 0xFF;
}
if(buffer_id>=frame_buffer_get_num_buffers(&control->frame_buffer))
{
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
return 0xFF;
}
@@ -274,13 +277,13 @@ unsigned char frame_buffer_control_add_pattern(TFBControl *control,pattern_func_
control->pat_parameters[j][i]=((unsigned char *)data)[i];
control->pat_functions[j]=function;
control->num_patterns++;
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_NUM_PATTERN_OFFSET]=control->num_patterns;
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=j;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_NUM_PATTERN_OFFSET]=control->num_patterns;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=j;
return j;
}
}
// the pattern could not be added
- ram_data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
+ control->memory->data[control->ram_base_address+FRAME_BUFFER_CONTROL_PATTERN_INDEX_OFFSET]=0xFF;
return 0xFF;
}
diff --git a/pattern_frame_buffer/src/image_patterns.c b/pattern_frame_buffer/src/image_patterns.c
index f49a883511e192516bc59d9508efcc551d841141..85f7aadce9cb2cc2f5aeb8924d5fe203065bbc54 100644
--- a/pattern_frame_buffer/src/image_patterns.c
+++ b/pattern_frame_buffer/src/image_patterns.c
@@ -43,12 +43,12 @@ void img_patterns_compute_range(unsigned short int area_min,unsigned short int a
}
// public functions
-void img_patterns_init(unsigned short int ram_base_address)
+void img_patterns_init(TMemory *memory,unsigned short int ram_base_address)
{
//initialize internal variables (read only variables)
- ram_data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_MAX_WIDTH_OFFSET]=IMG_MAX_IMAGE_WIDTH;
- ram_data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_MAX_HEIGHT_OFFSET]=IMG_MAX_IMAGE_HEIGHT;
- ram_data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_NUM_IMGS_OFFSET]=IMG_MAX_NUM_IMAGES;
+ memory->data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_MAX_WIDTH_OFFSET]=IMG_MAX_IMAGE_WIDTH;
+ memory->data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_MAX_HEIGHT_OFFSET]=IMG_MAX_IMAGE_HEIGHT;
+ memory->data[ram_base_address+FRAME_BUFFER_CONTROL_IMG_NUM_IMGS_OFFSET]=IMG_MAX_NUM_IMAGES;
}
void img_general(TLEDArea *area,TIMGDisplayData *pattern_data,unsigned char buffer_id,unsigned short int period,TFBControl *control)