diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index f82b02218adc5cd3992f923cfcb8f86d843e9b29..3db5c23dae1f1375848c66af058584351ed7ad17 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -58,7 +58,7 @@
#define EEPROM_IMU_BASE_ADDRESS ((unsigned short int)0x0065)
/* ADC */
-#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x01B6)
+#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x01B9)
#define EEPROM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x000F)
/* EEPROM default values */
diff --git a/include/darwin_imu.h b/include/darwin_imu.h
index 9fd5df80885c3dddca00ddd272d5ad8be8bc226d..84ac958da037b4b92b378eed0c6cf002fa8c6287 100755
--- a/include/darwin_imu.h
+++ b/include/darwin_imu.h
@@ -12,6 +12,7 @@ extern "C" {
#include "stm32_time.h"
#include "darwin_time.h"
#include "bno055_common.h"
+#include "scheduler.h"
typedef struct
{
@@ -22,17 +23,21 @@ typedef struct
unsigned int eeprom_base_address;
TComm device;
TTime time;
+ TScheduler *scheduler;
unsigned char current_page;
op_mode_t operation_mode;
power_mode_t power_mode;
+ unsigned char present;
+ unsigned char data_phase;
+ unsigned char calibrated;
}TIMU;
//public functions
-uint8_t imu_init(TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
-void imu_start(void);
-void imu_stop(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 darwin_imu_init(TMemory *memory,TScheduler *scheduler,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+void darwin_imu_start(void);
+void darwin_imu_stop(void);
+void darwin_imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+void darwin_imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
#ifdef __cplusplus
}
diff --git a/include/darwin_imu_registers.h b/include/darwin_imu_registers.h
index a54a456c8628921b06bc95bc9856dd121c07cd59..15a6147ad5d64152d16ebdf662d1ec0a710792ae 100644
--- a/include/darwin_imu_registers.h
+++ b/include/darwin_imu_registers.h
@@ -1,64 +1,67 @@
#ifndef _DARWIN_IMU_REGISTERS_H
#define _DARWIN_IMU_REGISTERS_H
-#define RAM_IMU_LENGTH 46
+#define RAM_IMU_LENGTH 48
#define IMU_CONTROL_OFFSET 0// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | | stop | start
#define IMU_START 0x01
#define IMU_STOP 0x02
-#define IMU_ACCEL_X_OFFSET 0
-#define IMU_ACCEL_Y_OFFSET 2
-#define IMU_ACCEL_Z_OFFSET 4
-#define IMU_COMPASS_X_OFFSET 6
-#define IMU_COMPASS_Y_OFFSET 8
-#define IMU_COMPASS_Z_OFFSET 10
-#define IMU_GYRO_X_OFFSET 12
-#define IMU_GYRO_Y_OFFSET 14
-#define IMU_GYRO_Z_OFFSET 16
-#define IMU_HEADING_OFFSET 18
-#define IMU_ROLL_OFFSET 20
-#define IMU_PITCH_OFFSET 22
-#define IMU_QUATERNION_W_OFFSET 24
-#define IMU_QUATERNION_X_OFFSET 26
-#define IMU_QUATERNION_Y_OFFSET 28
-#define IMU_QUATERNION_Z_OFFSET 30
-#define IMU_LIN_ACCEL_X_OFFSET 32
-#define IMU_LIN_ACCEL_Y_OFFSET 34
-#define IMU_LIN_ACCEL_Z_OFFSET 36
-#define IMU_GRAVITY_X_OFFSET 38
-#define IMU_GRAVITY_Y_OFFSET 40
-#define IMU_GRAVITY_Z_OFFSET 42
-#define IMU_TEMPERATURE_OFFSET 44
-#define IMU_CALIBRATION_STATUS_OFFSET 45
+#define IMU_DUMMY_OFFSET 0
+#define IMU_ACCEL_X_OFFSET 2
+#define IMU_ACCEL_Y_OFFSET 4
+#define IMU_ACCEL_Z_OFFSET 6
+#define IMU_COMPASS_X_OFFSET 8
+#define IMU_COMPASS_Y_OFFSET 10
+#define IMU_COMPASS_Z_OFFSET 12
+#define IMU_GYRO_X_OFFSET 14
+#define IMU_GYRO_Y_OFFSET 16
+#define IMU_GYRO_Z_OFFSET 18
+#define IMU_HEADING_OFFSET 20
+#define IMU_ROLL_OFFSET 22
+#define IMU_PITCH_OFFSET 24
+#define IMU_QUATERNION_W_OFFSET 26
+#define IMU_QUATERNION_X_OFFSET 28
+#define IMU_QUATERNION_Y_OFFSET 30
+#define IMU_QUATERNION_Z_OFFSET 32
+#define IMU_LIN_ACCEL_X_OFFSET 34
+#define IMU_LIN_ACCEL_Y_OFFSET 36
+#define IMU_LIN_ACCEL_Z_OFFSET 38
+#define IMU_GRAVITY_X_OFFSET 40
+#define IMU_GRAVITY_Y_OFFSET 42
+#define IMU_GRAVITY_Z_OFFSET 44
+#define IMU_TEMPERATURE_OFFSET 46
+#define IMU_CALIBRATION_STATUS_OFFSET 47
-#define EEPROM_IMU_LENGTH 22
+#define EEPROM_IMU_LENGTH 23
-#define IMU_CAL_DATA_OFFSET 0
+#define IMU_CAL_DATA_VALID_OFFSET 0
+#define IMU_CAL_DATA_OFFSET 1
-#define imu_eeprom_data(name,section_name,base_address,CAL_DATA0,CAL_DATA1,CAL_DATA2,CAL_DATA3,CAL_DATA4,CAL_DATA5,CAL_DATA6,CAL_DATA7,CAL_DATA8,CAL_DATA9,CAL_DATA10,CAL_DATA11,CAL_DATA12,CAL_DATA13,CAL_DATA14,CAL_DATA15,CAL_DATA16,CAL_DATA17,CAL_DATA18,CAL_DATA19,CAL_DATA20,CAL_DATA21) \
+#define imu_eeprom_data(name,section_name,base_address,CAL_DATA_VALID,CAL_DATA0,CAL_DATA1,CAL_DATA2,CAL_DATA3,CAL_DATA4,CAL_DATA5,CAL_DATA6,CAL_DATA7,CAL_DATA8,CAL_DATA9,CAL_DATA10,CAL_DATA11,CAL_DATA12,CAL_DATA13,CAL_DATA14,CAL_DATA15,CAL_DATA16,CAL_DATA17,CAL_DATA18,CAL_DATA19,CAL_DATA20,CAL_DATA21) \
unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
- CAL_DATA0&0x00FF,base_address+IMU_CAL_DATA_OFFSET \
- CAL_DATA1&0x00FF,base_address+IMU_CAL_DATA_OFFSET+1\
- CAL_DATA2&0x00FF,base_address+IMU_CAL_DATA_OFFSET+2 \
- CAL_DATA3&0x00FF,base_address+IMU_CAL_DATA_OFFSET+3 \
- CAL_DATA4&0x00FF,base_address+IMU_CAL_DATA_OFFSET+4 \
- CAL_DATA5&0x00FF,base_address+IMU_CAL_DATA_OFFSET+5 \
- CAL_DATA6&0x00FF,base_address+IMU_CAL_DATA_OFFSET+6 \
- CAL_DATA7&0x00FF,base_address+IMU_CAL_DATA_OFFSET+7 \
- CAL_DATA8&0x00FF,base_address+IMU_CAL_DATA_OFFSET+8 \
- CAL_DATA9&0x00FF,base_address+IMU_CAL_DATA_OFFSET+9 \
- CAL_DATA10&0x00FF,base_address+IMU_CAL_DATA_OFFSET+10 \
- CAL_DATA11&0x00FF,base_address+IMU_CAL_DATA_OFFSET+11 \
- CAL_DATA12&0x00FF,base_address+IMU_CAL_DATA_OFFSET+12 \
- CAL_DATA13&0x00FF,base_address+IMU_CAL_DATA_OFFSET+13 \
- CAL_DATA14&0x00FF,base_address+IMU_CAL_DATA_OFFSET+14 \
- CAL_DATA15&0x00FF,base_address+IMU_CAL_DATA_OFFSET+15 \
- CAL_DATA16&0x00FF,base_address+IMU_CAL_DATA_OFFSET+16 \
- CAL_DATA17&0x00FF,base_address+IMU_CAL_DATA_OFFSET+17 \
- CAL_DATA18&0x00FF,base_address+IMU_CAL_DATA_OFFSET+18 \
- CAL_DATA19&0x00FF,base_address+IMU_CAL_DATA_OFFSET+19 \
- CAL_DATA20&0x00FF,base_address+IMU_CAL_DATA_OFFSET+20 \
+ CAL_DATA_VALID&0x00FF,base_address+IMU_CAL_DATA_VALID_OFFSET, \
+ CAL_DATA0&0x00FF,base_address+IMU_CAL_DATA_OFFSET, \
+ CAL_DATA1&0x00FF,base_address+IMU_CAL_DATA_OFFSET+1,\
+ CAL_DATA2&0x00FF,base_address+IMU_CAL_DATA_OFFSET+2, \
+ CAL_DATA3&0x00FF,base_address+IMU_CAL_DATA_OFFSET+3, \
+ CAL_DATA4&0x00FF,base_address+IMU_CAL_DATA_OFFSET+4, \
+ CAL_DATA5&0x00FF,base_address+IMU_CAL_DATA_OFFSET+5, \
+ CAL_DATA6&0x00FF,base_address+IMU_CAL_DATA_OFFSET+6, \
+ CAL_DATA7&0x00FF,base_address+IMU_CAL_DATA_OFFSET+7, \
+ CAL_DATA8&0x00FF,base_address+IMU_CAL_DATA_OFFSET+8, \
+ CAL_DATA9&0x00FF,base_address+IMU_CAL_DATA_OFFSET+9, \
+ CAL_DATA10&0x00FF,base_address+IMU_CAL_DATA_OFFSET+10, \
+ CAL_DATA11&0x00FF,base_address+IMU_CAL_DATA_OFFSET+11, \
+ CAL_DATA12&0x00FF,base_address+IMU_CAL_DATA_OFFSET+12, \
+ CAL_DATA13&0x00FF,base_address+IMU_CAL_DATA_OFFSET+13, \
+ CAL_DATA14&0x00FF,base_address+IMU_CAL_DATA_OFFSET+14, \
+ CAL_DATA15&0x00FF,base_address+IMU_CAL_DATA_OFFSET+15, \
+ CAL_DATA16&0x00FF,base_address+IMU_CAL_DATA_OFFSET+16, \
+ CAL_DATA17&0x00FF,base_address+IMU_CAL_DATA_OFFSET+17, \
+ CAL_DATA18&0x00FF,base_address+IMU_CAL_DATA_OFFSET+18, \
+ CAL_DATA19&0x00FF,base_address+IMU_CAL_DATA_OFFSET+19, \
+ CAL_DATA20&0x00FF,base_address+IMU_CAL_DATA_OFFSET+20, \
CAL_DATA21&0x00FF,base_address+IMU_CAL_DATA_OFFSET+21 \
};
diff --git a/src/darwin_balance.c b/src/darwin_balance.c
index 754427e4d56c2ffe73b9c24332eba706554bef7f..fb6d8adbaa2bd31ed9a57193799f356196286345 100755
--- a/src/darwin_balance.c
+++ b/src/darwin_balance.c
@@ -13,7 +13,7 @@ void balance(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES])
int32_t gyro_x,gyro_y,gyro_z;
// get the values of the gyroscope
- imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
+ darwin_imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
// compensate the servo angle values
offsets[R_KNEE]=-((((int64_t)gyro_y*(int64_t)darwin_balance.knee_gain)/6000)>>9);
offsets[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)darwin_balance.ankle_pitch_gain)/6000)>>9);
@@ -31,7 +31,7 @@ void balance_fallen(void *data)
int32_t accel_x,accel_y,accel_z;
- imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+ darwin_imu_get_accel_data(&accel_x,&accel_y,&accel_z);
if(abs(accel_y)>abs(accel_z))
{
if(accel_y>0)
diff --git a/src/darwin_fw.c b/src/darwin_fw.c
index 39e89af0b08ad5bc5e105903b1942d6c8847b208..113d374f38039180829c5618721a2b9376350aeb 100755
--- a/src/darwin_fw.c
+++ b/src/darwin_fw.c
@@ -36,7 +36,7 @@ int main(void)
// initialize adc
adc_init(darwin_memory,scheduler_low_priority,EEPROM_ADC_DMA_BASE_ADDRESS,RAM_ADC_DMA_BASE_ADDRESS);
// initialize imu
- imu_init(darwin_memory,RAM_IMU_BASE_ADDRESS,EEPROM_IMU_BASE_ADDRESS);
+ darwin_imu_init(darwin_memory,scheduler_high_priority,RAM_IMU_BASE_ADDRESS,EEPROM_IMU_BASE_ADDRESS);
// initialize motion manager
darwin_mm_init(scheduler_low_priority,darwin_memory);
/* initialize the ram module */
@@ -46,8 +46,6 @@ int main(void)
usb_connect();
HAL_Delay(1000);
- imu_start();
-
darwin_dyn_slave_start();
while(1);/* main function does not return */
diff --git a/src/darwin_imu.c b/src/darwin_imu.c
index c6f05b311f16695eb4d3419b7f439b070b677b20..65c50e574e2564e249c9a589083bdfa72188d306 100755
--- a/src/darwin_imu.c
+++ b/src/darwin_imu.c
@@ -4,43 +4,35 @@
#include "ram.h"
#include "eeprom.h"
-#define IMU_TIMER TIM6
-#define IMU_TIMER_IRQn TIM6_DAC_IRQn
-#define IMU_TIMER_IRQHandler TIM6_DAC_IRQHandler
-#define IMU_ENABLE_TIMER_CLK __HAL_RCC_TIM6_CLK_ENABLE()
-
// private variables
-TIMU imu;
+TIMU darwin_imu;
-TIM_HandleTypeDef htim;
+/* eeprom data */
+imu_eeprom_data(darwin_imu,".eeprom",EEPROM_IMU_BASE_ADDRESS,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00);
-// private functions
-void imu_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
-{
-}
-void imu_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+// private functions
+void darwin_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;
- ram_read_table(imu->memory,address,length,data);
-}
+ ram_offset=address-imu->ram_base_address;
+ if(ram_in_range(imu->ram_base_address+IMU_CONTROL_OFFSET,address,length))
+ {
+ if(data[IMU_CONTROL_OFFSET-ram_offset]&IMU_START)
+ darwin_imu_start();
+ else if(data[IMU_CONTROL_OFFSET-ram_offset]&IMU_STOP)
+ darwin_imu_stop();
+ }
-/* comm device callbacks */
-unsigned char darwin_imu_dma_send_cb(TIMU *imu)
-{
- return 0x00;
}
-unsigned char darwin_imu_dma_receive_cb(TIMU *imu)
+void darwin_imu_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
- if(imu!=0x00000000)
- {
- if(imu->device.time!=0x00000000)
- time_cancel_timeout(imu->device.time);
- }
+ TIMU *imu=(TIMU *)module;
- return 0x00;
+ ram_read_table(imu->memory,address,length,data);
}
unsigned char darwin_imu_read_registers(TIMU *imu,unsigned char address, unsigned char length,unsigned char *data)
@@ -108,6 +100,67 @@ void darwin_imu_change_register_page(TIMU *imu,unsigned char page_id)
}
}
+/* load calibration from EEPROM */
+void darwin_imu_load_calibration(TIMU *imu)
+{
+ unsigned short int data;
+ unsigned char cal_data[22];
+ unsigned char i;
+
+ if(EE_ReadVariable(imu->eeprom_base_address+IMU_CAL_DATA_VALID_OFFSET,&data)==0)
+ {
+ if((data&0x00FF)==0x55)// calibration data valid
+ {
+ for(i=0;i<22;i++)
+ {
+ if(EE_ReadVariable(imu->eeprom_base_address+IMU_CAL_DATA_OFFSET+i,&data)==0)
+ cal_data[i]=data&0x00FF;
+ }
+ darwin_imu_change_register_page(imu,0x00);
+ darwin_imu_write_registers(imu,0x55,22,cal_data);
+ }
+ }
+}
+
+void darwin_imu_save_calibration(TIMU *imu)
+{
+ unsigned char cal_data[22];
+ unsigned char i;
+
+ darwin_imu_change_register_page(imu,0x00);
+ darwin_imu_read_registers(imu,0x55,22,cal_data);
+ for(i=0;i<22;i++)
+ EE_WriteVariable(imu->eeprom_base_address+IMU_CAL_DATA_OFFSET+i,cal_data[i]);
+
+ EE_WriteVariable(imu->eeprom_base_address+IMU_CAL_DATA_VALID_OFFSET,0x55);
+}
+
+
+/* comm device callbacks */
+unsigned char darwin_imu_dma_send_cb(TIMU *imu)
+{
+ return 0x00;
+}
+
+unsigned char darwin_imu_dma_receive_cb(TIMU *imu)
+{
+ if(imu!=0x00000000)
+ {
+ if(imu->device.time!=0x00000000)
+ time_cancel_timeout(imu->device.time);
+ if(imu->data_phase==0x01)
+ {
+ if(imu->memory->data[imu->ram_base_address+IMU_CALIBRATION_STATUS_OFFSET]==0xFF && imu->calibrated==0x00)
+ {
+ scheduler_enable_channel(darwin_imu.scheduler,SCHED_CH3);
+ imu->calibrated=0x01;
+ }
+ }
+ }
+
+ return 0x00;
+}
+
unsigned char darwin_imu_detect_sensor(TIMU *imu)
{
TBNO055IMUInfo info;
@@ -120,39 +173,52 @@ unsigned char darwin_imu_detect_sensor(TIMU *imu)
return 0x01;
}
-void IMU_TIMER_IRQHandler(void)
+/* scheduler callback function */
+void darwin_imu_request_data(TIMU *imu)
{
unsigned char cmd[4];
+ op_mode_t mode=ndof_mode;
- if(__HAL_TIM_GET_FLAG(&htim, TIM_FLAG_UPDATE) != RESET)
+ if(imu->present==0x01)
{
- if(__HAL_TIM_GET_IT_SOURCE(&htim, TIM_IT_UPDATE) !=RESET)
+ if(imu->device.time!=0x00000000)
+ {
+ if(time_is_timeout(imu->device.time))
+ comm_cancel_dma_receive(&imu->device);
+ time_set_timeout(imu->device.time,5000);
+ }
+ // start DMA transfer
+ cmd[0]=0xAA;
+ cmd[1]=0x01;
+ cmd[2]=0x08;
+ cmd[3]=46;
+ comm_send(&imu->device,cmd,4);
+ comm_receive_dma(&imu->device,&imu->memory->data[imu->ram_base_address+IMU_DUMMY_OFFSET],48);
+ }
+ else
+ {
+ if(darwin_imu_detect_sensor(imu))
{
- __HAL_TIM_CLEAR_IT(&htim, TIM_IT_UPDATE);
- // start DMA transfer
- cmd[0]=0xAA;
- cmd[1]=0x01;
- cmd[2]=0x08;
- cmd[3]=46;
- comm_send(&imu.device,cmd,4);
- comm_receive_dma(&imu.device,&imu.memory->data[imu.ram_base_address+IMU_ACCEL_X_OFFSET],48);
+ darwin_imu_read_registers(&darwin_imu,0x07,1,&darwin_imu.current_page);// read the current register page
+ darwin_imu_write_registers(&darwin_imu,0x3D,1,(unsigned char *)&mode);// set ndof operation mode
+ scheduler_enable_channel(darwin_imu.scheduler,SCHED_CH4);
+ darwin_imu.present=0x01;
}
}
}
// public functions
-uint8_t imu_init(TMemory *memory,unsigned short int ram_base_address, unsigned short int eeprom_base_address)
+uint8_t darwin_imu_init(TMemory *memory,TScheduler *scheduler,unsigned short int ram_base_address, unsigned short int eeprom_base_address)
{
- TIM_MasterConfigTypeDef sMasterConfig = {0};
TUART_IRQ_Priorities priorities;
UART_InitTypeDef Init;
op_mode_t mode=ndof_mode;
// initialize timeout object
- time_init(&imu.time,darwin_time_get_counts_per_us(),darwin_time_get_counts);
+ time_init(&darwin_imu.time,darwin_time_get_counts_per_us(),darwin_time_get_counts);
// initialize serial port
- comm_init(&imu.device,0x01,&imu.time);
+ comm_init(&darwin_imu.device,0x01,&darwin_imu.time);
Init.BaudRate = 115200;
Init.WordLength = UART_WORDLENGTH_8B;
@@ -162,71 +228,73 @@ uint8_t imu_init(TMemory *memory,unsigned short int ram_base_address, unsigned s
Init.HwFlowCtl = UART_HWCONTROL_NONE;
Init.OverSampling = UART_OVERSAMPLING_16;
- priorities.irq_priority=2;
+ priorities.irq_priority=1;
priorities.irq_subpriority=0;
- priorities.dma_rx_priority=2;
+ priorities.dma_rx_priority=1;
priorities.dma_rx_subpriority=1;
- priorities.dma_tx_priority=2;
+ priorities.dma_tx_priority=1;
priorities.dma_tx_subpriority=2;
- usart3_init(&imu.device,&Init,&priorities);
- imu.device.dma_send_cb=(unsigned char (*)(void *))darwin_imu_dma_send_cb;
- imu.device.dma_receive_cb=(unsigned char (*)(void *))darwin_imu_dma_receive_cb;
- imu.device.data=&imu;
+ usart3_init(&darwin_imu.device,&Init,&priorities);
+ darwin_imu.device.dma_send_cb=(unsigned char (*)(void *))darwin_imu_dma_send_cb;
+ darwin_imu.device.dma_receive_cb=(unsigned char (*)(void *))darwin_imu_dma_receive_cb;
+ darwin_imu.device.data=&darwin_imu;
+
+ /* scheduler configuration */
+ darwin_imu.scheduler=scheduler;
+ scheduler_set_channel(scheduler,SCHED_CH2,(void(*)(void *))darwin_imu_request_data,50,&darwin_imu);
+ scheduler_set_channel_one_shot(scheduler,SCHED_CH3,(void(*)(void *))darwin_imu_save_calibration,10,&darwin_imu);
+ scheduler_set_channel_one_shot(scheduler,SCHED_CH4,(void(*)(void *))darwin_imu_load_calibration,10,&darwin_imu);
- if(darwin_imu_detect_sensor(&imu))
- {
- darwin_imu_read_registers(&imu,0x07,1,&imu.current_page);// read the current register page
- darwin_imu_write_registers(&imu,0x3D,1,(unsigned char *)&mode);// set ndof operation mode
- /* imu timer configuration */
- htim.Instance=IMU_TIMER;
- htim.Init.Prescaler=84;
- htim.Init.CounterMode=TIM_COUNTERMODE_UP;
- htim.Init.Period=10000;
- HAL_TIM_Base_Init(&htim);
- IMU_ENABLE_TIMER_CLK;
-
- sMasterConfig.MasterOutputTrigger=TIM_TRGO_RESET;
- sMasterConfig.MasterSlaveMode=TIM_MASTERSLAVEMODE_DISABLE;
- HAL_TIMEx_MasterConfigSynchronization(&htim, &sMasterConfig);
-
- HAL_NVIC_SetPriority(IMU_TIMER_IRQn, 3, 0);
- HAL_NVIC_EnableIRQ(IMU_TIMER_IRQn);
- }
/* 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))
+ mem_module_init(&darwin_imu.mem_module);
+ darwin_imu.mem_module.write_cmd=darwin_imu_write_cmd;
+ darwin_imu.mem_module.read_cmd=darwin_imu_read_cmd;
+ if(!mem_module_add_ram_segment(&darwin_imu.mem_module,ram_base_address,RAM_IMU_LENGTH))
return 0x00;
- imu.ram_base_address=ram_base_address;
- if(!mem_module_add_eeprom_segment(&imu.mem_module,eeprom_base_address,EEPROM_IMU_LENGTH))
+ darwin_imu.ram_base_address=ram_base_address;
+ if(!mem_module_add_eeprom_segment(&darwin_imu.mem_module,eeprom_base_address,EEPROM_IMU_LENGTH))
return 0x00;
- imu.eeprom_base_address=eeprom_base_address;
- if(!mem_add_module(memory,&imu.mem_module))
+ darwin_imu.eeprom_base_address=eeprom_base_address;
+ if(!mem_add_module(memory,&darwin_imu.mem_module))
return 0x00;
- imu.mem_module.data=&imu;
- imu.memory=memory;
+ darwin_imu.mem_module.data=&darwin_imu;
+ darwin_imu.memory=memory;
EE_update_num_variables(EEPROM_IMU_LENGTH);
+ if(darwin_imu_detect_sensor(&darwin_imu))
+ {
+ darwin_imu_read_registers(&darwin_imu,0x07,1,&darwin_imu.current_page);// read the current register page
+ darwin_imu_write_registers(&darwin_imu,0x3D,1,(unsigned char *)&mode);// set ndof operation mode
+ scheduler_enable_channel(darwin_imu.scheduler,SCHED_CH4);
+ darwin_imu.present=0x01;
+ }
+ else
+ darwin_imu.present=0x00;
+ /* initialize internal variables */
+ darwin_imu.data_phase=0x00;
+ darwin_imu.calibrated=0x00;
+
return 0x01;
}
-void imu_start(void)
+void darwin_imu_start(void)
{
- HAL_TIM_Base_Start_IT(&htim);
+ darwin_imu.data_phase=0x01;
+ scheduler_enable_channel(darwin_imu.scheduler,SCHED_CH2);
}
-void imu_stop(void)
+void darwin_imu_stop(void)
{
- HAL_TIM_Base_Stop_IT(&htim);
+ scheduler_disable_channel(darwin_imu.scheduler,SCHED_CH2);
+ darwin_imu.data_phase=0x00;
}
-void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
+void darwin_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 darwin_imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
{
}