From f3e6b6aa3c4cb5574a5453a2e3bb2e1ba6ed4b20 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 10 Feb 2020 22:59:41 +0100
Subject: [PATCH] Updated some firmware source files.
---
include/darwin_conf.h | 57 +++--
include/darwin_imu.h | 59 +++++
include/darwin_imu_registers.h | 24 +++
include/imu.h | 26 ---
include/imu_registers.h | 28 ---
src/CMakeLists.txt | 4 +-
src/cm730_fw.c | 10 +-
src/{imu.c => darwin_imu.c} | 381 +++++++++++++++++----------------
src/darwin_sch.c | 4 +
9 files changed, 331 insertions(+), 262 deletions(-)
create mode 100755 include/darwin_imu.h
create mode 100644 include/darwin_imu_registers.h
delete mode 100755 include/imu.h
delete mode 100644 include/imu_registers.h
rename src/{imu.c => darwin_imu.c} (59%)
diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index e3b83c3..548407c 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -1,21 +1,33 @@
#ifndef _DARWIN_CONF_H
#define _DARWIN_CONF_H
-#define RAM_SIZE 1024
-#define EEPROM_SIZE 256
+#define RAM_SIZE 4096
+#define EEPROM_SIZE 128
-/* Dynamixel slave configuration */
+/* Dynamixel slave memory map */
+/* dynamixel slave */
#define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000)
#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010)
-#define DARWIN_DEVICE_MODEL 0x001D
-#define DARWIN_FIRMWARE_VERSION 0x0001
-#define DARWIN_DEVICE_ID 0x0003
-#define DEFAULT_BAUDRATE 0x0022
-#define DEFAULT_RETURN_DELAY 0x0000
-#define DEFAULT_RETURN_LEVEL 0x0002
+/* dynamixel manager */
+#define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0006)
+#define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0080)
+
+/* motion manager */
+#define EEPROM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0011)
+#define RAM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0088)
+
+/* action motion module */
+#define RAM_ACTION_MM_BASE_ADDRESS ((unsigned short int)0x009E)
+
+/* IMU */
+#define RAM_IMU_MM_BASE_ADDRESS ((unsigned short int)0x00A0)
-#define NUM_MOTION_PAGES 46
+/* balance */
+#define EEPROM_BALANCE_BASE_ADDRESS ((unsigned short int)0x0007)
+#define RAM_BALANCE_BASE_ADDRESS ((unsigned short int)0x00AF)
+
+#define NUM_MOTION_PAGES 256
#define MAX_DYN_MASTER_TX_BUFFER_LEN 1024
#define MAX_DYN_MASTER_RX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
@@ -32,13 +44,26 @@
#define MODULE_MAX_NUM_MODELS 32
#define MM_MAX_NUM_MOTION_MODULES 8
-/* GPIO configuration */
-#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100)
+/* EEPROM default values */
+/* Dynamixel slave */
+#define DARWIN_DEVICE_MODEL 0x7300
+#define DARWIN_FIRMWARE_VERSION 0x0001
+#define DARWIN_DEVICE_ID 0x0001
+
+#define DEFAULT_BAUDRATE 0x0010
+#define DEFAULT_RETURN_DELAY 0x0000
+#define DEFAULT_RETURN_LEVEL 0x0002
+
+/* dynamixel manager */
+#define DYN_MANAGER_PERIOD 0x0007
-/* ADC configuration */
-#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115)
+/* motion manager */
+#define MMANAGER_PERIOD 0x0001
-/* ADC configuration */
-#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F)
+/* balance */
+#define DEFAULT_BALANCE_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
+#define DEFAULT_BALANCE_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
+#define DEFAULT_BALANCE_ANKLE_PITCH_GAIN 0xE666 // 0.9
+#define DEFAULT_BALANCE_HIP_ROLL_GAIN 0x8000 // 0.5
#endif
diff --git a/include/darwin_imu.h b/include/darwin_imu.h
new file mode 100755
index 0000000..700e04d
--- /dev/null
+++ b/include/darwin_imu.h
@@ -0,0 +1,59 @@
+#ifndef _DARWIN_IMU_H
+#define _DARWIN_IMU_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "darwin_imu_registers.h"
+#include "memory.h"
+#include "stm32_time.h"
+#include "darwin_time.h"
+
+typedef enum {IMU_DONE=0,IMU_BUSY,IMU_TIMEOUT} imu_op_t;
+typedef enum {IMU_GET_ACCEL,IMU_GET_GYRO} imu_state_t;
+
+typedef struct
+{
+ // internal buffers
+ uint8_t spi_rd_data[32];
+ uint8_t spi_num_rd_data;
+ uint8_t spi_wr_data[32];
+ uint8_t spi_num_wr_data;
+ // control attributes
+ volatile uint8_t stopped;
+ uint8_t stop_flag;
+ volatile imu_op_t op_state;
+ uint8_t current_device_id;
+ // gyroscope attributes
+ uint8_t gyro_calibration;
+ uint16_t gyro_calibration_num_samples;
+ uint8_t gyro_conf_data[6];
+ int16_t gyro_center[3];
+ int16_t gyro_data[3];
+ // accelerometer attributes
+ uint8_t accel_conf_data[5];
+ int16_t accel_data[3];
+ // memory attributes
+ TMemModule mem_module;
+ TMemory *memory;
+ unsigned int ram_base_address;
+ TTime time;
+}TIMU;
+
+//public functions
+uint8_t imu_init(TMemory *memory,unsigned int ram_base_address);
+void imu_start(void);
+void imu_stop(void);
+void imu_set_calibration_samples(uint16_t num_samples);
+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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/include/darwin_imu_registers.h b/include/darwin_imu_registers.h
new file mode 100644
index 0000000..baa7157
--- /dev/null
+++ b/include/darwin_imu_registers.h
@@ -0,0 +1,24 @@
+#ifndef _DARWIN_IMU_REGISTERS_H
+#define _DARWIN_IMU_REGISTERS_H
+
+#define RAM_IMU_LENGTH 15
+
+#define IMU_CONTROL_OFFSET 0// 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_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 IMU_CAL_SAMPLES_OFFSET 1
+#define IMU_GYRO_X_OFFSET 3
+#define IMU_GYRO_Y_OFFSET 5
+#define IMU_GYRO_Z_OFFSET 7
+#define IMU_ACCEL_X_OFFSET 9
+#define IMU_ACCEL_Y_OFFSET 11
+#define IMU_ACCEL_Z_OFFSET 13
+
+
+#endif
diff --git a/include/imu.h b/include/imu.h
deleted file mode 100755
index b74bf3e..0000000
--- a/include/imu.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _IMU_H
-#define _IMU_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "imu_registers.h"
-#include "memory.h"
-
-//public functions
-uint8_t imu_init(TMemory *memory);
-void imu_start(void);
-void imu_stop(void);
-void imu_set_calibration_samples(uint16_t num_samples);
-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);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/imu_registers.h b/include/imu_registers.h
deleted file mode 100644
index 92c38a8..0000000
--- a/include/imu_registers.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#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/CMakeLists.txt b/src/CMakeLists.txt
index dedacdb..a827100 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -4,7 +4,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_sch.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_time.c
#${CMAKE_CURRENT_SOURCE_DIR}/adc_dma.c
- #${CMAKE_CURRENT_SOURCE_DIR}/imu.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/darwin_imu.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_slave.c
#${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c
#${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master_v2.c
@@ -23,7 +23,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart3.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart1.c PARENT_SCOPE)
-set(header_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../include ${CMAKE_CURRENT_SOURCE_DIR}/../stm32_libraries/f1/usart/include PARENT_SCOPE)
+set(header_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../include ${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/include PARENT_SCOPE)
set(configuration_file ${CMAKE_CURRENT_SOURCE_DIR}/../include/darwin_conf.h PARENT_SCOPE)
set(processor STM32F103xE PARENT_SCOPE)
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 83fe297..c5ec884 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -33,17 +33,11 @@ int main(void)
// gpio_init(&darwin_memory);
// initialize adc
// adc_init(&darwin_memory);
- // initialize imu
-// imu_init(&darwin_memory);
/* initialize the dynamixel slave interface */
darwin_dyn_slave_init(&darwin_memory,scheduler);
+ // initialize imu
+ imu_init(darwin_memory,RAM_IMU_MM_BASE_ADDRESS);
/* initialize motion manager module */
-// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
-// period=eeprom_data&0x00FF;
-// EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
-// period+=((eeprom_data&0x00FF)<<8);
-// manager_init(period);
-// gpio_set_led(LED_4);
/* initialize the ram module */
ram_init(darwin_memory);
diff --git a/src/imu.c b/src/darwin_imu.c
similarity index 59%
rename from src/imu.c
rename to src/darwin_imu.c
index 37d017c..0a44fa2 100755
--- a/src/imu.c
+++ b/src/darwin_imu.c
@@ -1,4 +1,4 @@
-#include "imu.h"
+#include "darwin_imu.h"
#include "ram.h"
#define IMU_SPI SPI2
@@ -84,50 +84,16 @@
#define GYRO_INT1_TSH_ZL 0x37
#define GYRO_INT1_DURATION 0x38
-typedef enum {IMU_DONE=0,IMU_BUSY} imu_op_t;
-typedef enum {IMU_GET_ACCEL,IMU_GET_GYRO} imu_state_t;
-
// private variables
-uint8_t imu_spi_rd_data[32];
-uint8_t imu_spi_num_rd_data;
-uint8_t imu_spi_wr_data[32];
-uint8_t imu_spi_num_wr_data;
-volatile imu_op_t imu_op_state;
-uint8_t imu_current_device_id;
-uint8_t imu_stop_flag;
-volatile uint8_t imu_stopped;
-// gyroscope variables
-const uint8_t gyro_conf_reg=GYRO_CNTRL_REG1;
-const uint8_t gyro_conf_len=6;
-uint8_t gyro_conf_data[6];
-const uint8_t gyro_data_reg=GYRO_OUT_X_L;
-const uint8_t gyro_data_len=6;
-int16_t gyro_center[3];
-int32_t gyro_data[3];
-uint8_t gyro_calibration;
-uint16_t gyro_calibration_num_samples;
-// accelerometer variables
-const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
-const uint8_t accel_conf_len=5;
-uint8_t accel_conf_data[5];
-int32_t accel_data[3];
-const uint8_t accel_data_reg=ACCEL_OUT_X_L;
-const uint8_t accel_data_len=6;
+TIMU imu;
SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim;
-TMemModule imu_mem_module;
-
// private functions
-void imu_wait_op_done(void)
-{
- while(imu_op_state!=IMU_DONE);
-}
-
uint8_t imu_is_op_done(void)
{
- if(imu_op_state==IMU_DONE)
+ if(imu.op_state==IMU_DONE)
return 0x01;
else
return 0x00;
@@ -139,33 +105,49 @@ void imu_enable_device(uint8_t device_id)
HAL_GPIO_WritePin(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN,GPIO_PIN_RESET);
else if(device_id==GYRO_ID)
HAL_GPIO_WritePin(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN,GPIO_PIN_RESET);
- imu_current_device_id=device_id;
+ imu.current_device_id=device_id;
}
void imu_disable_device(void)
{
- if(imu_current_device_id==ACCEL_ID)
+ if(imu.current_device_id==ACCEL_ID)
HAL_GPIO_WritePin(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN,GPIO_PIN_SET);
- else if(imu_current_device_id==GYRO_ID)
+ else if(imu.current_device_id==GYRO_ID)
HAL_GPIO_WritePin(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN,GPIO_PIN_SET);
- imu_current_device_id=0x00;
+ imu.current_device_id=0x00;
+}
+
+unsigned char imu_wait_op_done(void)
+{
+ time_set_timeout(&imu.time,50000);
+ while(imu.op_state!=IMU_DONE)
+ {
+ HAL_Delay(1);
+ if(time_is_timeout(&imu.time))
+ {
+ imu_disable_device();
+ imu.op_state=IMU_DONE;
+ return IMU_TIMEOUT;
+ }
+ }
+ return IMU_DONE;
}
void imu_spi_start_read(uint8_t address,uint8_t length)
{
uint8_t i;
- imu_spi_num_rd_data=length+1;
- imu_spi_num_wr_data=length+1;
+ imu.spi_num_rd_data=length+1;
+ imu.spi_num_wr_data=length+1;
- imu_spi_wr_data[0]=address | 0xC0;
- imu_spi_rd_data[0]=0xFF;
+ imu.spi_wr_data[0]=address | 0xC0;
+ imu.spi_rd_data[0]=0xFF;
for(i=1;i<=length;i++)
{
- imu_spi_wr_data[i]=0xFF;
- imu_spi_rd_data[i]=0xFF;
+ imu.spi_wr_data[i]=0xFF;
+ imu.spi_rd_data[i]=0xFF;
}
- imu_op_state=IMU_BUSY;
+ imu.op_state=IMU_BUSY;
// start the read operation
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE);
__HAL_SPI_ENABLE(&hspi2);
@@ -175,25 +157,25 @@ void imu_spi_get_data(uint8_t *data)
{
uint8_t i;
- for(i=0;i<imu_spi_num_rd_data;i++)
- data[i]=imu_spi_rd_data[i+1];
+ for(i=0;i<imu.spi_num_rd_data;i++)
+ data[i]=imu.spi_rd_data[i+1];
}
void imu_spi_start_write(uint8_t address,uint8_t *data,uint8_t length)
{
uint8_t i;
- imu_spi_num_rd_data=length+1;
- imu_spi_num_wr_data=length+1;
+ imu.spi_num_rd_data=length+1;
+ imu.spi_num_wr_data=length+1;
- imu_spi_wr_data[0]=address | 0x40;
- imu_spi_rd_data[0]=0xFF;
+ imu.spi_wr_data[0]=address | 0x40;
+ imu.spi_rd_data[0]=0xFF;
for(i=1;i<=length;i++)
{
- imu_spi_wr_data[i]=data[i-1];
- imu_spi_rd_data[i]=0xFF;
+ imu.spi_wr_data[i]=data[i-1];
+ imu.spi_rd_data[i]=0xFF;
}
- imu_op_state=IMU_BUSY;
+ imu.op_state=IMU_BUSY;
// start the read operation
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE);
__HAL_SPI_ENABLE(&hspi2);
@@ -206,18 +188,22 @@ uint8_t imu_accel_detect(void)
imu_enable_device(ACCEL_ID);
imu_spi_start_read(ACCEL_WHO_AM_I,1);
- imu_wait_op_done();
- imu_spi_get_data(&data);
- if(data==ACCEL_ID)
+ if(imu_wait_op_done()==IMU_DONE)
{
- ram_data[IMU_CNTRL]|=IMU_ACCEL_DET;
- return 0x01;
+ imu_spi_get_data(&data);
+ if(data==ACCEL_ID)
+ {
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_ACCEL_DET;
+ return 0x01;
+ }
+ else
+ {
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&=(~IMU_ACCEL_DET);
+ return 0x00;
+ }
}
else
- {
- ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00;
- }
}
void imu_accel_start(void)
@@ -233,25 +219,25 @@ void imu_accel_stop(void)
void imu_accel_get_config(void)
{
imu_enable_device(ACCEL_ID);
- imu_spi_start_read(accel_conf_reg,accel_conf_len);
- imu_wait_op_done();
- imu_spi_get_data(accel_conf_data);
+ imu_spi_start_read(ACCEL_CNTRL_REG1,5);
+ if(imu_wait_op_done()==IMU_DONE)
+ imu_spi_get_data(imu.accel_conf_data);
}
void imu_accel_set_config(void)
{
- accel_conf_data[0]=0x3F;
- accel_conf_data[3]=0x10;
+ imu.accel_conf_data[0]=0x3F;
+ imu.accel_conf_data[3]=0x10;
imu_enable_device(ACCEL_ID);
- imu_spi_start_write(accel_conf_reg,accel_conf_data,accel_conf_len);
+ imu_spi_start_write(ACCEL_CNTRL_REG1,imu.accel_conf_data,5);
imu_wait_op_done();
}
void imu_accel_get_data(void)
{
imu_enable_device(ACCEL_ID);
- imu_spi_start_read(accel_data_reg,accel_data_len);
+ imu_spi_start_read(ACCEL_OUT_X_L,6);
}
void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
@@ -260,9 +246,9 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
for(i=0;i<3;i++)
{
- accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8));
- data_out[i*2]=accel_data[i]%256;
- data_out[i*2+1]=accel_data[i]/256;
+ imu.accel_data[i]=(int16_t)((data_in[i*2]+(data_in[i*2+1]<<8))*2);
+ data_out[i*2]=imu.accel_data[i]&0x00FF;
+ data_out[i*2+1]=(imu.accel_data[i]>>8)&0x00FF;
}
}
@@ -273,18 +259,22 @@ uint8_t imu_gyro_detect(void)
imu_enable_device(GYRO_ID);
imu_spi_start_read(GYRO_WHO_AM_I,1);
- imu_wait_op_done();
- imu_spi_get_data(&data);
- if(data==GYRO_ID)
+ if(imu_wait_op_done()==IMU_DONE)
{
- ram_data[IMU_CNTRL]|=IMU_GYRO_DET;
- return 0x01;
+ imu_spi_get_data(&data);
+ if(data==GYRO_ID)
+ {
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_GYRO_DET;
+ return 0x01;
+ }
+ else
+ {
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&=(~IMU_GYRO_DET);
+ return 0x00;
+ }
}
else
- {
- ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00;
- }
}
void imu_gyro_start(void)
@@ -299,26 +289,26 @@ void imu_gyro_stop(void)
void imu_gyro_set_config(void)
{
- gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up
- gyro_conf_data[3]=0x20;// Full scale = 2000dps
+ imu.gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up
+ imu.gyro_conf_data[3]=0x20;// Full scale = 2000dps
imu_enable_device(GYRO_ID);
- imu_spi_start_write(gyro_conf_reg,gyro_conf_data,gyro_conf_len);
+ imu_spi_start_write(GYRO_CNTRL_REG1,imu.gyro_conf_data,6);
imu_wait_op_done();
}
void imu_gyro_get_config(void)
{
imu_enable_device(GYRO_ID);
- imu_spi_start_read(gyro_conf_reg,gyro_conf_len);
- imu_wait_op_done();
- imu_spi_get_data(gyro_conf_data);
+ imu_spi_start_read(GYRO_CNTRL_REG1,6);
+ if(imu_wait_op_done()==IMU_DONE)
+ imu_spi_get_data(imu.gyro_conf_data);
}
void imu_gyro_get_data(void)
{
imu_enable_device(GYRO_ID);
- imu_spi_start_read(gyro_data_reg,gyro_data_len);
+ imu_spi_start_read(GYRO_OUT_X_L,6);
}
void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
@@ -327,36 +317,44 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
for(i=0;i<3;i++)
{
- gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*70)-gyro_center[i];
- data_out[i*2]=gyro_data[i]%256;
- data_out[i*2+1]=gyro_data[i]/256;
+ imu.gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*35)/2-imu.gyro_center[i];
+ data_out[i*2]=imu.gyro_data[i]&0x00FF;
+ data_out[i*2+1]=(imu.gyro_data[i]>>8)&0x00FF;
}
}
void imu_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
+ TIMU *imu=(TIMU *)module;
+ unsigned short int ram_offset;
uint16_t num_samples;
- if(ram_in_range(IMU_CNTRL,address,length))
+ ram_offset=address-imu->ram_base_address;
+ if(ram_in_range(imu->ram_base_address+IMU_CONTROL_OFFSET,address,length))
{
- if(data[IMU_CNTRL-address]&IMU_START)
+ if(data[IMU_CONTROL_OFFSET-ram_offset]&IMU_START)
imu_start();
- else if(data[IMU_CNTRL-address]&IMU_STOP)
+ else if(data[IMU_CONTROL_OFFSET-ram_offset]&IMU_STOP)
imu_stop();
- else if(data[IMU_CNTRL-address]&IMU_START_CAL)
+ else if(data[IMU_CONTROL_OFFSET-ram_offset]&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);
+ if(ram_in_window(imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,2,address,length))
+ {
+ ram_read_word(imu->memory,imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,&num_samples);
+ if(ram_in_range(imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,address,length))
+ num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES_OFFSET-ram_offset];
+ if(ram_in_range(imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET+1,address,length))
+ num_samples=(num_samples&0x00FF)+(data[IMU_CAL_SAMPLES_OFFSET+1-ram_offset]<<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);
+ TIMU *imu=(TIMU *)module;
+
+ ram_read_table(imu->memory,address,length,data);
}
// interrupt handlers
@@ -367,27 +365,27 @@ void IMU_SPI_IRQHANDLER(void)
/* SPI in mode Receiver and Overrun not occurred ---------------------------*/
if((__HAL_SPI_GET_IT_SOURCE(&hspi2, SPI_IT_RXNE) != RESET) && (__HAL_SPI_GET_FLAG(&hspi2, SPI_FLAG_RXNE) != RESET) && (__HAL_SPI_GET_FLAG(&hspi2, SPI_FLAG_OVR) == RESET))
{
- imu_spi_rd_data[num_rd]=hspi2.Instance->DR;
+ imu.spi_rd_data[num_rd]=hspi2.Instance->DR;
num_rd++;
- if(num_rd >= imu_spi_num_rd_data)
+ if(num_rd >= imu.spi_num_rd_data)
{
imu_disable_device();
- imu_op_state=IMU_DONE;
+ imu.op_state=IMU_DONE;
num_rd=0;
- num_wr=0;
}
}
/* SPI in mode Tramitter ---------------------------------------------------*/
if((__HAL_SPI_GET_IT_SOURCE(&hspi2, SPI_IT_TXE) != RESET) && (__HAL_SPI_GET_FLAG(&hspi2, SPI_FLAG_TXE) != RESET))
{
- if(num_wr < imu_spi_num_wr_data)
+ if(num_wr < imu.spi_num_wr_data)
{
- hspi2.Instance->DR=imu_spi_wr_data[num_wr];
+ hspi2.Instance->DR=imu.spi_wr_data[num_wr];
num_wr++;
}
else
{
+ num_wr=0;
__HAL_SPI_DISABLE_IT(&hspi2, SPI_IT_TXE);
}
}
@@ -407,66 +405,80 @@ void IMU_TIMER_IRQHandler(void)
__HAL_TIM_CLEAR_IT(&htim, TIM_IT_UPDATE);
if(imu_is_op_done())
{
- if(imu_stop_flag==0x01)
+ if(imu.stop_flag==0x01)
{
// stop the timer to get the imu data
HAL_TIM_Base_Stop_IT(&htim);
- imu_stop_flag=0x00;
- imu_stopped=0x01;
+ imu.stop_flag=0x00;
+ imu.stopped=0x01;
}
else
{
switch(state)
{
- case IMU_GET_ACCEL: if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
+ case IMU_GET_ACCEL: if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_GYRO_DET)
{
imu_spi_get_data(data);
- imu_gyro_convert_data(data,&ram_data[IMU_GYRO_X]);
- if(gyro_calibration)
+ imu_gyro_convert_data(data,&imu.memory->data[imu.ram_base_address+IMU_GYRO_X_OFFSET]);
+ if(imu.gyro_calibration)
{
- gyro_accum[0]+=gyro_data[0];
- gyro_accum[1]+=gyro_data[1];
- gyro_accum[2]+=gyro_data[2];
+ gyro_accum[0]+=imu.gyro_data[0];
+ gyro_accum[1]+=imu.gyro_data[1];
+ gyro_accum[2]+=imu.gyro_data[2];
num_samples++;
- if(num_samples==gyro_calibration_num_samples)
+ if(num_samples==imu.gyro_calibration_num_samples)
{
num_samples=0;
- gyro_center[0]=gyro_accum[0]/gyro_calibration_num_samples;
- gyro_center[1]=gyro_accum[1]/gyro_calibration_num_samples;
- gyro_center[2]=gyro_accum[2]/gyro_calibration_num_samples;
+ imu.gyro_center[0]=gyro_accum[0]/imu.gyro_calibration_num_samples;
+ imu.gyro_center[1]=gyro_accum[1]/imu.gyro_calibration_num_samples;
+ imu.gyro_center[2]=gyro_accum[2]/imu.gyro_calibration_num_samples;
gyro_accum[0]=0;
gyro_accum[1]=0;
gyro_accum[2]=0;
- ram_data[IMU_CNTRL]&=(~IMU_CALIBRATING);
- if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)
- imu_stop_flag=0x01;
- gyro_calibration=0x00;
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&=(~IMU_CALIBRATING);
+ if((imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_RUNNING)==0x00)
+ imu.stop_flag=0x01;
+ imu.gyro_calibration=0x00;
}
}
}
- if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
+ if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_ACCEL_DET)
+ {
+ time_set_timeout(&imu.time,50000);
imu_accel_get_data();
+ }
state=IMU_GET_GYRO;
break;
- case IMU_GET_GYRO: if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
+ case IMU_GET_GYRO: if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_ACCEL_DET)
{
imu_spi_get_data(data);
- imu_accel_convert_data(data,&ram_data[IMU_ACCEL_X]);
+ imu_accel_convert_data(data,&imu.memory->data[imu.ram_base_address+IMU_ACCEL_X_OFFSET]);
}
- if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
+ if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_GYRO_DET)
+ {
+ time_set_timeout(&imu.time,50000);
imu_gyro_get_data();
+ }
state=IMU_GET_ACCEL;
break;
}
}
}
+ else
+ {
+ if(time_is_timeout(&imu.time))
+ {
+ imu_disable_device();
+ imu.op_state=IMU_DONE;
+ }
+ }
}
}
}
// public functions
-uint8_t imu_init(TMemory *memory)
+uint8_t imu_init(TMemory *memory,unsigned int ram_base_address)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
@@ -502,22 +514,6 @@ uint8_t imu_init(TMemory *memory)
HAL_GPIO_WritePin(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN,GPIO_PIN_SET);
HAL_GPIO_WritePin(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN,GPIO_PIN_SET);
- /* initialize variables */
- imu_spi_num_wr_data=0x00;
- imu_spi_num_rd_data=0x00;
- for(i=0;i<32;i++)
- {
- imu_spi_rd_data[i]=0x00;
- imu_spi_wr_data[i]=0x00;
- }
- imu_op_state=IMU_DONE;
- imu_current_device_id=0x00;
- gyro_center[0]=0;
- gyro_center[1]=0;
- gyro_center[2]=0;
- gyro_calibration=0x00;
- imu_set_calibration_samples(1024);
-
// configure the SPI module
hspi2.Instance = IMU_SPI;
hspi2.Init.Mode = SPI_MODE_MASTER;
@@ -539,7 +535,7 @@ uint8_t imu_init(TMemory *memory)
/* imu timer configuration */
htim.Instance=IMU_TIMER;
htim.Init.Period = 1000;
- htim.Init.Prescaler = 72;
+ htim.Init.Prescaler = 84;
htim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim.Init.CounterMode = TIM_COUNTERMODE_UP;
htim.Init.RepetitionCounter=0;
@@ -551,9 +547,39 @@ uint8_t imu_init(TMemory *memory)
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE);
+ /* 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_base_address,RAM_IMU_LENGTH))
+ return 0x00;
+ imu.ram_base_address=ram_base_address;
+ if(!mem_add_module(memory,&imu.mem_module))
+ return 0x00;
+ imu.mem_module.data=&imu;
+ imu.memory=memory;
+
+ time_init(&imu.time,darwin_time_get_counts_per_us(),darwin_time_get_counts);
+
+ /* initialize variables */
+ imu.spi_num_wr_data=0x00;
+ imu.spi_num_rd_data=0x00;
+ for(i=0;i<32;i++)
+ {
+ imu.spi_rd_data[i]=0x00;
+ imu.spi_wr_data[i]=0x00;
+ }
+ imu.op_state=IMU_DONE;
+ imu.current_device_id=0x00;
+ imu.gyro_center[0]=0;
+ imu.gyro_center[1]=0;
+ imu.gyro_center[2]=0;
+ imu.gyro_calibration=0x00;
+ imu_set_calibration_samples(1024);
+
// detect the sensors
if(imu_gyro_detect())
- {
+ {
imu_gyro_get_config();
imu_gyro_set_config();
}
@@ -563,21 +589,12 @@ uint8_t imu_init(TMemory *memory)
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[IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
+ if((imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_RUNNING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -585,33 +602,33 @@ void imu_start(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
- ram_data[IMU_CNTRL]|=IMU_RUNNING;
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_RUNNING;
}
}
void imu_stop(void)
{
- if(ram_data[IMU_CNTRL]&IMU_RUNNING)
+ if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_RUNNING)
{
- imu_stop_flag=0x01;
- while(!imu_stopped);
- imu_stopped=0x00;
+ imu.stop_flag=0x01;
+ while(!imu.stopped);
+ imu.stopped=0x00;
imu_accel_stop();
imu_gyro_stop();
- ram_data[IMU_CNTRL]&=(~IMU_RUNNING);
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&=(~IMU_RUNNING);
}
}
void imu_set_calibration_samples(uint16_t num_samples)
{
- gyro_calibration_num_samples=num_samples;
- ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256;
- ram_data[IMU_CAL_SAMPLES+1]=gyro_calibration_num_samples/256;
+ imu.gyro_calibration_num_samples=num_samples;
+ imu.memory->data[imu.ram_base_address+IMU_CAL_SAMPLES_OFFSET]=imu.gyro_calibration_num_samples%256;
+ imu.memory->data[imu.ram_base_address+IMU_CAL_SAMPLES_OFFSET+1]=imu.gyro_calibration_num_samples/256;
}
void imu_start_gyro_cal(void)
{
- if((ram_data[IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
+ if((imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_CALIBRATING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -620,16 +637,16 @@ void imu_start_gyro_cal(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
}
- ram_data[IMU_CNTRL]|=IMU_CALIBRATING;
- gyro_center[0]=0;
- gyro_center[1]=0;
- gyro_center[2]=0;
- gyro_calibration=0x01;
+ imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_CALIBRATING;
+ imu.gyro_center[0]=0;
+ imu.gyro_center[1]=0;
+ imu.gyro_center[2]=0;
+ imu.gyro_calibration=0x01;
}
uint8_t imu_is_gyro_calibrated(void)
{
- if(ram_data[IMU_CNTRL])
+ if(imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET])
return 0x00;
else
return 0x01;
@@ -637,15 +654,15 @@ uint8_t imu_is_gyro_calibrated(void)
void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
{
- *x=gyro_data[0];
- *y=gyro_data[1];
- *z=gyro_data[2];
+ *x=imu.gyro_data[0];
+ *y=imu.gyro_data[1];
+ *z=imu.gyro_data[2];
}
void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
{
- *x=accel_data[0];
- *y=accel_data[1];
- *z=accel_data[2];
+ *x=imu.accel_data[0];
+ *y=imu.accel_data[1];
+ *z=imu.accel_data[2];
}
diff --git a/src/darwin_sch.c b/src/darwin_sch.c
index f27bee6..dd1deb5 100644
--- a/src/darwin_sch.c
+++ b/src/darwin_sch.c
@@ -13,6 +13,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC1) !=RESET)
{
__HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC1);
+ __HAL_TIM_CLEAR_FLAG(&darwin_sch_timer_handle,TIM_FLAG_CC1);
scheduler_interrupt(&darwin_scheduler,SCHED_CH1);
}
}
@@ -22,6 +23,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC2) !=RESET)
{
__HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC2);
+ __HAL_TIM_CLEAR_FLAG(&darwin_sch_timer_handle,TIM_FLAG_CC2);
scheduler_interrupt(&darwin_scheduler,SCHED_CH2);
}
}
@@ -31,6 +33,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC3) !=RESET)
{
__HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC3);
+ __HAL_TIM_CLEAR_FLAG(&darwin_sch_timer_handle,TIM_FLAG_CC3);
scheduler_interrupt(&darwin_scheduler,SCHED_CH3);
}
}
@@ -40,6 +43,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC4) !=RESET)
{
__HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC4);
+ __HAL_TIM_CLEAR_FLAG(&darwin_sch_timer_handle,TIM_FLAG_CC4);
scheduler_interrupt(&darwin_scheduler,SCHED_CH4);
}
}
--
GitLab