Skip to content
Snippets Groups Projects
Commit 896a83c8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the IMU module to use the TMemory data structures.

parent 68269179
No related branches found
No related tags found
1 merge request!5Dynamixel manager
This commit is part of merge request !5. Comments created here will be created in the context of that merge request.
...@@ -13,7 +13,7 @@ TARGET_FILES+=src/gpio.c ...@@ -13,7 +13,7 @@ TARGET_FILES+=src/gpio.c
#TARGET_FILES+=src/darwin_time.c #TARGET_FILES+=src/darwin_time.c
TARGET_FILES+=src/eeprom.c TARGET_FILES+=src/eeprom.c
TARGET_FILES+=src/adc_dma.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_slave.c
#TARGET_FILES+=src/darwin_dyn_master.c #TARGET_FILES+=src/darwin_dyn_master.c
#TARGET_FILES+=src/darwin_dyn_master_v2.c #TARGET_FILES+=src/darwin_dyn_master_v2.c
......
...@@ -10,4 +10,7 @@ ...@@ -10,4 +10,7 @@
/* ADC configuration */ /* ADC configuration */
#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115) #define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115)
/* ADC configuration */
#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F)
#endif #endif
...@@ -254,23 +254,6 @@ typedef enum { ...@@ -254,23 +254,6 @@ typedef enum {
DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY, DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
//RAM //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_NUM_SERVOS = 0x014A,
DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 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 // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager
...@@ -588,13 +571,6 @@ typedef enum { ...@@ -588,13 +571,6 @@ typedef enum {
#define IMU_BASE_ADDRESS 0x013B #define IMU_BASE_ADDRESS 0x013B
#define IMU_MEM_LENGTH 15 #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_BASE_ADDRESS 0x014A
#define MANAGER_MEM_LENGTH 86 #define MANAGER_MEM_LENGTH 86
......
...@@ -6,9 +6,11 @@ extern "C" { ...@@ -6,9 +6,11 @@ extern "C" {
#endif #endif
#include "stm32f1xx.h" #include "stm32f1xx.h"
#include "imu_registers.h"
#include "memory.h"
//public functions //public functions
void imu_init(void); uint8_t imu_init(TMemory *memory);
void imu_start(void); void imu_start(void);
void imu_stop(void); void imu_stop(void);
void imu_set_calibration_samples(uint16_t num_samples); void imu_set_calibration_samples(uint16_t num_samples);
...@@ -16,9 +18,6 @@ void imu_start_gyro_cal(void); ...@@ -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_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); void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
uint8_t imu_is_gyro_calibrated(void); 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 #ifdef __cplusplus
} }
......
#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
...@@ -32,7 +32,7 @@ int main(void) ...@@ -32,7 +32,7 @@ int main(void)
// initialize adc // initialize adc
adc_init(&darwin_memory); adc_init(&darwin_memory);
// initialize imu // initialize imu
// imu_init(); imu_init(&darwin_memory);
// initialize time module // initialize time module
// darwin_time_init(); // darwin_time_init();
/* initialize the dynamixel slave interface */ /* initialize the dynamixel slave interface */
......
...@@ -117,6 +117,8 @@ const uint8_t accel_data_len=6; ...@@ -117,6 +117,8 @@ const uint8_t accel_data_len=6;
SPI_HandleTypeDef hspi2; SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim; TIM_HandleTypeDef htim;
TMemModule imu_mem_module;
// private functions // private functions
inline void imu_wait_op_done(void) inline void imu_wait_op_done(void)
{ {
...@@ -208,12 +210,12 @@ uint8_t imu_accel_detect(void) ...@@ -208,12 +210,12 @@ uint8_t imu_accel_detect(void)
imu_spi_get_data(&data); imu_spi_get_data(&data);
if(data==ACCEL_ID) if(data==ACCEL_ID)
{ {
ram_data[DARWIN_IMU_CNTRL]|=IMU_ACCEL_DET; ram_data[IMU_CNTRL]|=IMU_ACCEL_DET;
return 0x01; return 0x01;
} }
else else
{ {
ram_data[DARWIN_IMU_CNTRL]&=(~IMU_ACCEL_DET); ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00; return 0x00;
} }
} }
...@@ -275,12 +277,12 @@ uint8_t imu_gyro_detect(void) ...@@ -275,12 +277,12 @@ uint8_t imu_gyro_detect(void)
imu_spi_get_data(&data); imu_spi_get_data(&data);
if(data==GYRO_ID) if(data==GYRO_ID)
{ {
ram_data[DARWIN_IMU_CNTRL]|=IMU_GYRO_DET; ram_data[IMU_CNTRL]|=IMU_GYRO_DET;
return 0x01; return 0x01;
} }
else else
{ {
ram_data[DARWIN_IMU_CNTRL]&=(~IMU_GYRO_DET); ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00; return 0x00;
} }
} }
...@@ -331,6 +333,32 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out) ...@@ -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 // interrupt handlers
void IMU_SPI_IRQHANDLER(void) void IMU_SPI_IRQHANDLER(void)
{ {
...@@ -390,10 +418,10 @@ void IMU_TIMER_IRQHandler(void) ...@@ -390,10 +418,10 @@ void IMU_TIMER_IRQHandler(void)
{ {
switch(state) 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_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) if(gyro_calibration)
{ {
gyro_accum[0]+=gyro_data[0]; gyro_accum[0]+=gyro_data[0];
...@@ -409,23 +437,23 @@ void IMU_TIMER_IRQHandler(void) ...@@ -409,23 +437,23 @@ void IMU_TIMER_IRQHandler(void)
gyro_accum[0]=0; gyro_accum[0]=0;
gyro_accum[1]=0; gyro_accum[1]=0;
gyro_accum[2]=0; gyro_accum[2]=0;
ram_data[DARWIN_IMU_CNTRL]&=(~IMU_CALIBRATING); ram_data[IMU_CNTRL]&=(~IMU_CALIBRATING);
if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00) if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)
imu_stop_flag=0x01; imu_stop_flag=0x01;
gyro_calibration=0x00; gyro_calibration=0x00;
} }
} }
} }
if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET) if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
imu_accel_get_data(); imu_accel_get_data();
state=IMU_GET_GYRO; state=IMU_GET_GYRO;
break; 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_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(); imu_gyro_get_data();
state=IMU_GET_ACCEL; state=IMU_GET_ACCEL;
break; break;
...@@ -438,7 +466,7 @@ void IMU_TIMER_IRQHandler(void) ...@@ -438,7 +466,7 @@ void IMU_TIMER_IRQHandler(void)
// public functions // public functions
void imu_init(void) uint8_t imu_init(TMemory *memory)
{ {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i; uint8_t i;
...@@ -534,11 +562,22 @@ void imu_init(void) ...@@ -534,11 +562,22 @@ void imu_init(void)
imu_accel_get_config(); imu_accel_get_config();
imu_accel_set_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) 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 // start the accelerometer
imu_accel_start(); imu_accel_start();
...@@ -546,33 +585,33 @@ void imu_start(void) ...@@ -546,33 +585,33 @@ void imu_start(void)
// start the timer to get the imu data // start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim); HAL_TIM_Base_Start_IT(&htim);
ram_data[DARWIN_IMU_CNTRL]|=IMU_RUNNING; ram_data[IMU_CNTRL]|=IMU_RUNNING;
} }
} }
void imu_stop(void) void imu_stop(void)
{ {
if(ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING) if(ram_data[IMU_CNTRL]&IMU_RUNNING)
{ {
imu_stop_flag=0x01; imu_stop_flag=0x01;
while(!imu_stopped); while(!imu_stopped);
imu_stopped=0x00; imu_stopped=0x00;
imu_accel_stop(); imu_accel_stop();
imu_gyro_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) void imu_set_calibration_samples(uint16_t num_samples)
{ {
gyro_calibration_num_samples=num_samples; gyro_calibration_num_samples=num_samples;
ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256; ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256;
ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256; ram_data[IMU_CAL_SAMPLES+1]=gyro_calibration_num_samples/256;
} }
void imu_start_gyro_cal(void) 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 // start the accelerometer
imu_accel_start(); imu_accel_start();
...@@ -581,7 +620,7 @@ void imu_start_gyro_cal(void) ...@@ -581,7 +620,7 @@ void imu_start_gyro_cal(void)
// start the timer to get the imu data // start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim); 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[0]=0;
gyro_center[1]=0; gyro_center[1]=0;
gyro_center[2]=0; gyro_center[2]=0;
...@@ -590,7 +629,7 @@ void imu_start_gyro_cal(void) ...@@ -590,7 +629,7 @@ void imu_start_gyro_cal(void)
uint8_t imu_is_gyro_calibrated(void) uint8_t imu_is_gyro_calibrated(void)
{ {
if(ram_data[DARWIN_IMU_CNTRL]) if(ram_data[IMU_CNTRL])
return 0x00; return 0x00;
else else
return 0x01; return 0x01;
...@@ -610,30 +649,3 @@ void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z) ...@@ -610,30 +649,3 @@ void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
*z=accel_data[2]; *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);
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment