diff --git a/Makefile b/Makefile
index cfd15cae9095c36a4c81a15e28b974cf548f1305..a114da239b4073b6e28c21912f0e2168804977de 100755
--- a/Makefile
+++ b/Makefile
@@ -13,7 +13,7 @@ TARGET_FILES+=src/gpio.c
#TARGET_FILES+=src/darwin_time.c
TARGET_FILES+=src/eeprom.c
TARGET_FILES+=src/adc_dma.c
-#TARGET_FILES+=src/imu.c
+TARGET_FILES+=src/imu.c
#TARGET_FILES+=src/darwin_dyn_slave.c
#TARGET_FILES+=src/darwin_dyn_master.c
#TARGET_FILES+=src/darwin_dyn_master_v2.c
diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index e0a776b79e749b4a58693ad7995c277ac9721c7a..97ee25acf58ba5d7c20cfad94ef02b2d9a344569 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -10,4 +10,7 @@
/* ADC configuration */
#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115)
+/* ADC configuration */
+#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F)
+
#endif
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 7ae353435438f78741e04dccc02e5c46428f4597..59ae35e783a50db52191099b27c1a841efa4ef59 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -254,23 +254,6 @@ typedef enum {
DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
//RAM
- DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // accel_det | gyro_det | calibrating | running | | start_cal | stop | start
- DARWIN_IMU_CAL_SAMPLES_L = 0x013C,
- DARWIN_IMU_CAL_SAMPLES_H = 0x013D,
- DARWIN_IMU_GYRO_X_L = 0x013E,
- DARWIN_IMU_GYRO_X_H = 0x013F,
- DARWIN_IMU_GYRO_Y_L = 0x0140,
- DARWIN_IMU_GYRO_Y_H = 0x0141,
- DARWIN_IMU_GYRO_Z_L = 0x0142,
- DARWIN_IMU_GYRO_Z_H = 0x0143,
- DARWIN_IMU_ACCEL_X_L = 0x0144,
- DARWIN_IMU_ACCEL_X_H = 0x0145,
- DARWIN_IMU_ACCEL_Y_L = 0x0146,
- DARWIN_IMU_ACCEL_Y_H = 0x0147,
- DARWIN_IMU_ACCEL_Z_L = 0x0148,
- DARWIN_IMU_ACCEL_Z_H = 0x0149,
-
DARWIN_MM_NUM_SERVOS = 0x014A,
DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager
@@ -588,13 +571,6 @@ typedef enum {
#define IMU_BASE_ADDRESS 0x013B
#define IMU_MEM_LENGTH 15
-#define IMU_START 0x01
-#define IMU_STOP 0x02
-#define IMU_START_CAL 0x04
-#define IMU_RUNNING 0x10
-#define IMU_CALIBRATING 0x20
-#define IMU_ACCEL_DET 0x40
-#define IMU_GYRO_DET 0x80
#define MANAGER_BASE_ADDRESS 0x014A
#define MANAGER_MEM_LENGTH 86
diff --git a/include/imu.h b/include/imu.h
index f3e1c3d203ca0ebe27e7a82ba3c569d4d5cddb11..b74bf3e94cd5d75508f8985eb1e912338007e5fa 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -6,9 +6,11 @@ extern "C" {
#endif
#include "stm32f1xx.h"
+#include "imu_registers.h"
+#include "memory.h"
//public functions
-void imu_init(void);
+uint8_t imu_init(TMemory *memory);
void imu_start(void);
void imu_stop(void);
void imu_set_calibration_samples(uint16_t num_samples);
@@ -16,9 +18,6 @@ void imu_start_gyro_cal(void);
void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
uint8_t imu_is_gyro_calibrated(void);
-// operation functions
-uint8_t imu_in_range(unsigned short int address,unsigned short int length);
-void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
#ifdef __cplusplus
}
diff --git a/include/imu_registers.h b/include/imu_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..92c38a8ef391a2938faf1ff01a515ea64b459685
--- /dev/null
+++ b/include/imu_registers.h
@@ -0,0 +1,28 @@
+#ifndef _IMU_REGISTERS_H
+#define _IMU_REGISTERS_H
+
+#ifndef RAM_IMU_BASE_ADDRESS
+ #define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_IMU_LENGTH 15
+
+#define IMU_CNTRL RAM_IMU_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // accel_det | gyro_det | calibrating | running | | start_cal | stop | start
+#define IMU_CAL_SAMPLES (RAM_IMU_BASE_ADDRESS+1)
+#define IMU_GYRO_X (RAM_IMU_BASE_ADDRESS+3)
+#define IMU_GYRO_Y (RAM_IMU_BASE_ADDRESS+5)
+#define IMU_GYRO_Z (RAM_IMU_BASE_ADDRESS+7)
+#define IMU_ACCEL_X (RAM_IMU_BASE_ADDRESS+9)
+#define IMU_ACCEL_Y (RAM_IMU_BASE_ADDRESS+11)
+#define IMU_ACCEL_Z (RAM_IMU_BASE_ADDRESS+13)
+
+#define IMU_START 0x01
+#define IMU_STOP 0x02
+#define IMU_START_CAL 0x04
+#define IMU_RUNNING 0x10
+#define IMU_CALIBRATING 0x20
+#define IMU_ACCEL_DET 0x40
+#define IMU_GYRO_DET 0x80
+
+#endif
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index f8ba5f2e2bf2fad8187d199bf312c3de7162e5f9..859d26a6bf135af392808c3fa6e01c928759f95c 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -32,7 +32,7 @@ int main(void)
// initialize adc
adc_init(&darwin_memory);
// initialize imu
-// imu_init();
+ imu_init(&darwin_memory);
// initialize time module
// darwin_time_init();
/* initialize the dynamixel slave interface */
diff --git a/src/imu.c b/src/imu.c
index bbaed728d98168aec6e571a9473b1edc91b65dbc..95c834c3d2f46a2f3635fa28444a228a788c7fc5 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -117,6 +117,8 @@ const uint8_t accel_data_len=6;
SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim;
+TMemModule imu_mem_module;
+
// private functions
inline void imu_wait_op_done(void)
{
@@ -208,12 +210,12 @@ uint8_t imu_accel_detect(void)
imu_spi_get_data(&data);
if(data==ACCEL_ID)
{
- ram_data[DARWIN_IMU_CNTRL]|=IMU_ACCEL_DET;
+ ram_data[IMU_CNTRL]|=IMU_ACCEL_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_ACCEL_DET);
+ ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00;
}
}
@@ -275,12 +277,12 @@ uint8_t imu_gyro_detect(void)
imu_spi_get_data(&data);
if(data==GYRO_ID)
{
- ram_data[DARWIN_IMU_CNTRL]|=IMU_GYRO_DET;
+ ram_data[IMU_CNTRL]|=IMU_GYRO_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_GYRO_DET);
+ ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00;
}
}
@@ -331,6 +333,32 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
}
}
+void imu_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t num_samples;
+
+ if(ram_in_range(IMU_CNTRL,address,length))
+ {
+ if(data[IMU_CNTRL-address]&IMU_START)
+ imu_start();
+ else if(data[IMU_CNTRL-address]&IMU_STOP)
+ imu_stop();
+ else if(data[IMU_CNTRL-address]&IMU_START_CAL)
+ imu_start_gyro_cal();
+ }
+ ram_read_word(IMU_CAL_SAMPLES,&num_samples);
+ if(ram_in_range(IMU_CAL_SAMPLES,address,length))
+ num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES-address];
+ if(ram_in_range(IMU_CAL_SAMPLES+1,address,length))
+ num_samples=(num_samples&0x00FF)+(data[IMU_CAL_SAMPLES+1-address]<<8);
+ imu_set_calibration_samples(num_samples);
+}
+
+void imu_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(address,length,data);
+}
+
// interrupt handlers
void IMU_SPI_IRQHANDLER(void)
{
@@ -390,10 +418,10 @@ void IMU_TIMER_IRQHandler(void)
{
switch(state)
{
- case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
+ case IMU_GET_ACCEL: if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
{
imu_spi_get_data(data);
- imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]);
+ imu_gyro_convert_data(data,&ram_data[IMU_GYRO_X]);
if(gyro_calibration)
{
gyro_accum[0]+=gyro_data[0];
@@ -409,23 +437,23 @@ void IMU_TIMER_IRQHandler(void)
gyro_accum[0]=0;
gyro_accum[1]=0;
gyro_accum[2]=0;
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_CALIBRATING);
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)
+ ram_data[IMU_CNTRL]&=(~IMU_CALIBRATING);
+ if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)
imu_stop_flag=0x01;
gyro_calibration=0x00;
}
}
}
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
+ if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
imu_accel_get_data();
state=IMU_GET_GYRO;
break;
- case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
+ case IMU_GET_GYRO: if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
{
imu_spi_get_data(data);
- imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]);
+ imu_accel_convert_data(data,&ram_data[IMU_ACCEL_X]);
}
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
+ if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
imu_gyro_get_data();
state=IMU_GET_ACCEL;
break;
@@ -438,7 +466,7 @@ void IMU_TIMER_IRQHandler(void)
// public functions
-void imu_init(void)
+uint8_t imu_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
@@ -534,11 +562,22 @@ void imu_init(void)
imu_accel_get_config();
imu_accel_set_config();
}
+
+ /* initialize memory module */
+ mem_module_init(&imu_mem_module);
+ imu_mem_module.write_cmd=imu_write_cmd;
+ imu_mem_module.read_cmd=imu_read_cmd;
+ if(!mem_module_add_ram_segment(&imu_mem_module,RAM_IMU_BASE_ADDRESS,RAM_IMU_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&imu_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void imu_start(void)
{
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
+ if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -546,33 +585,33 @@ void imu_start(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
- ram_data[DARWIN_IMU_CNTRL]|=IMU_RUNNING;
+ ram_data[IMU_CNTRL]|=IMU_RUNNING;
}
}
void imu_stop(void)
{
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)
+ if(ram_data[IMU_CNTRL]&IMU_RUNNING)
{
imu_stop_flag=0x01;
while(!imu_stopped);
imu_stopped=0x00;
imu_accel_stop();
imu_gyro_stop();
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_RUNNING);
+ ram_data[IMU_CNTRL]&=(~IMU_RUNNING);
}
}
void imu_set_calibration_samples(uint16_t num_samples)
{
gyro_calibration_num_samples=num_samples;
- ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256;
- ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256;
+ ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256;
+ ram_data[IMU_CAL_SAMPLES+1]=gyro_calibration_num_samples/256;
}
void imu_start_gyro_cal(void)
{
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
+ if((ram_data[IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -581,7 +620,7 @@ void imu_start_gyro_cal(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
}
- ram_data[DARWIN_IMU_CNTRL]|=IMU_CALIBRATING;
+ ram_data[IMU_CNTRL]|=IMU_CALIBRATING;
gyro_center[0]=0;
gyro_center[1]=0;
gyro_center[2]=0;
@@ -590,7 +629,7 @@ void imu_start_gyro_cal(void)
uint8_t imu_is_gyro_calibrated(void)
{
- if(ram_data[DARWIN_IMU_CNTRL])
+ if(ram_data[IMU_CNTRL])
return 0x00;
else
return 0x01;
@@ -610,30 +649,3 @@ void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
*z=accel_data[2];
}
-// operation functions
-uint8_t imu_in_range(unsigned short int address,unsigned short int length)
-{
- return ram_in_window(IMU_BASE_ADDRESS,IMU_MEM_LENGTH,address,length);
-}
-
-void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- uint16_t num_samples;
-
- if(ram_in_range(DARWIN_IMU_CNTRL,address,length))
- {
- if(data[DARWIN_IMU_CNTRL-address]&IMU_START)
- imu_start();
- else if(data[DARWIN_IMU_CNTRL-address]&IMU_STOP)
- imu_stop();
- else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL)
- imu_start_gyro_cal();
- }
- ram_read_word(DARWIN_IMU_CAL_SAMPLES_L,&num_samples);
- if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_L,address,length))
- num_samples=(num_samples&0xFF00)+data[DARWIN_IMU_CAL_SAMPLES_L-address];
- if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_H,address,length))
- num_samples=(num_samples&0x00FF)+(data[DARWIN_IMU_CAL_SAMPLES_H-address]<<8);
- imu_set_calibration_samples(num_samples);
-}
-