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

Updated some firmware source files.

parent ac6ea819
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/darwin_conf.h
+ 41
− 16
View file @ f3e6b6aa
#ifndef _DARWIN_CONF_H #ifndef _DARWIN_CONF_H
#define _DARWIN_CONF_H #define _DARWIN_CONF_H
#define RAM_SIZE 1024 #define RAM_SIZE 4096
#define EEPROM_SIZE 256 #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_ADDRESS1 ((unsigned short int)0x0000)
#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010) #define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010)
#define DARWIN_DEVICE_MODEL 0x001D /* dynamixel manager */
#define DARWIN_FIRMWARE_VERSION 0x0001 #define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0006)
#define DARWIN_DEVICE_ID 0x0003 #define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0080)
#define DEFAULT_BAUDRATE 0x0022
#define DEFAULT_RETURN_DELAY 0x0000 /* motion manager */
#define DEFAULT_RETURN_LEVEL 0x0002 #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_TX_BUFFER_LEN 1024
#define MAX_DYN_MASTER_RX_BUFFER_LEN 1024 #define MAX_DYN_MASTER_RX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 #define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
...@@ -32,13 +44,26 @@ ...@@ -32,13 +44,26 @@
#define MODULE_MAX_NUM_MODELS 32 #define MODULE_MAX_NUM_MODELS 32
#define MM_MAX_NUM_MOTION_MODULES 8 #define MM_MAX_NUM_MOTION_MODULES 8
/* GPIO configuration */ /* EEPROM default values */
#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100) /* 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 */ /* motion manager */
#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115) #define MMANAGER_PERIOD 0x0001
/* ADC configuration */ /* balance */
#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F) #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 #endif
include/imu.h → include/darwin_imu.h
+ 59
− 0
View file @ f3e6b6aa
#ifndef _IMU_H #ifndef _DARWIN_IMU_H
#define _IMU_H #define _DARWIN_IMU_H
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
#include "stm32f1xx.h" #include "stm32f1xx.h"
#include "imu_registers.h" #include "darwin_imu_registers.h"
#include "memory.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 //public functions
uint8_t imu_init(TMemory *memory); uint8_t imu_init(TMemory *memory,unsigned int ram_base_address);
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);
......
include/darwin_imu_registers.h 0 → 100644
+ 24
− 0
View file @ f3e6b6aa
#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
include/imu_registers.h deleted 100644 → 0
+ 0
− 28
View file @ ac6ea819
#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
src/CMakeLists.txt
+ 2
− 2
View file @ f3e6b6aa
...@@ -4,7 +4,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c ...@@ -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_sch.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_time.c ${CMAKE_CURRENT_SOURCE_DIR}/darwin_time.c
#${CMAKE_CURRENT_SOURCE_DIR}/adc_dma.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_slave.c
#${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c
#${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master_v2.c #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master_v2.c
...@@ -23,7 +23,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.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/usart3.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart1.c PARENT_SCOPE) ${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(configuration_file ${CMAKE_CURRENT_SOURCE_DIR}/../include/darwin_conf.h PARENT_SCOPE)
set(processor STM32F103xE PARENT_SCOPE) set(processor STM32F103xE PARENT_SCOPE)
......
src/cm730_fw.c
+ 2
− 8
View file @ f3e6b6aa
...@@ -33,17 +33,11 @@ int main(void) ...@@ -33,17 +33,11 @@ int main(void)
// gpio_init(&darwin_memory); // gpio_init(&darwin_memory);
// initialize adc // initialize adc
// adc_init(&darwin_memory); // adc_init(&darwin_memory);
// initialize imu
// imu_init(&darwin_memory);
/* initialize the dynamixel slave interface */ /* initialize the dynamixel slave interface */
darwin_dyn_slave_init(&darwin_memory,scheduler); darwin_dyn_slave_init(&darwin_memory,scheduler);
// initialize imu
imu_init(darwin_memory,RAM_IMU_MM_BASE_ADDRESS);
/* initialize motion manager module */ /* 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 */ /* initialize the ram module */
ram_init(darwin_memory); ram_init(darwin_memory);
......
src/imu.c → src/darwin_imu.c
+ 199
− 182
View file @ f3e6b6aa
#include "imu.h" #include "darwin_imu.h"
#include "ram.h" #include "ram.h"
#define IMU_SPI SPI2 #define IMU_SPI SPI2
...@@ -84,50 +84,16 @@ ...@@ -84,50 +84,16 @@
#define GYRO_INT1_TSH_ZL 0x37 #define GYRO_INT1_TSH_ZL 0x37
#define GYRO_INT1_DURATION 0x38 #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 // private variables
uint8_t imu_spi_rd_data[32]; TIMU imu;
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;
SPI_HandleTypeDef hspi2; SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim; TIM_HandleTypeDef htim;
TMemModule imu_mem_module;
// private functions // private functions
void imu_wait_op_done(void)
{
while(imu_op_state!=IMU_DONE);
}
uint8_t imu_is_op_done(void) uint8_t imu_is_op_done(void)
{ {
if(imu_op_state==IMU_DONE) if(imu.op_state==IMU_DONE)
return 0x01; return 0x01;
else else
return 0x00; return 0x00;
...@@ -139,33 +105,49 @@ void imu_enable_device(uint8_t device_id) ...@@ -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); HAL_GPIO_WritePin(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN,GPIO_PIN_RESET);
else if(device_id==GYRO_ID) else if(device_id==GYRO_ID)
HAL_GPIO_WritePin(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN,GPIO_PIN_RESET); 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) 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); 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); 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) void imu_spi_start_read(uint8_t address,uint8_t length)
{ {
uint8_t i; uint8_t i;
imu_spi_num_rd_data=length+1; imu.spi_num_rd_data=length+1;
imu_spi_num_wr_data=length+1; imu.spi_num_wr_data=length+1;
imu_spi_wr_data[0]=address | 0xC0; imu.spi_wr_data[0]=address | 0xC0;
imu_spi_rd_data[0]=0xFF; imu.spi_rd_data[0]=0xFF;
for(i=1;i<=length;i++) for(i=1;i<=length;i++)
{ {
imu_spi_wr_data[i]=0xFF; imu.spi_wr_data[i]=0xFF;
imu_spi_rd_data[i]=0xFF; imu.spi_rd_data[i]=0xFF;
} }
imu_op_state=IMU_BUSY; imu.op_state=IMU_BUSY;
// start the read operation // start the read operation
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE); __HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE);
__HAL_SPI_ENABLE(&hspi2); __HAL_SPI_ENABLE(&hspi2);
...@@ -175,25 +157,25 @@ void imu_spi_get_data(uint8_t *data) ...@@ -175,25 +157,25 @@ void imu_spi_get_data(uint8_t *data)
{ {
uint8_t i; uint8_t i;
for(i=0;i<imu_spi_num_rd_data;i++) for(i=0;i<imu.spi_num_rd_data;i++)
data[i]=imu_spi_rd_data[i+1]; data[i]=imu.spi_rd_data[i+1];
} }
void imu_spi_start_write(uint8_t address,uint8_t *data,uint8_t length) void imu_spi_start_write(uint8_t address,uint8_t *data,uint8_t length)
{ {
uint8_t i; uint8_t i;
imu_spi_num_rd_data=length+1; imu.spi_num_rd_data=length+1;
imu_spi_num_wr_data=length+1; imu.spi_num_wr_data=length+1;
imu_spi_wr_data[0]=address | 0x40; imu.spi_wr_data[0]=address | 0x40;
imu_spi_rd_data[0]=0xFF; imu.spi_rd_data[0]=0xFF;
for(i=1;i<=length;i++) for(i=1;i<=length;i++)
{ {
imu_spi_wr_data[i]=data[i-1]; imu.spi_wr_data[i]=data[i-1];
imu_spi_rd_data[i]=0xFF; imu.spi_rd_data[i]=0xFF;
} }
imu_op_state=IMU_BUSY; imu.op_state=IMU_BUSY;
// start the read operation // start the read operation
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE); __HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_TXE);
__HAL_SPI_ENABLE(&hspi2); __HAL_SPI_ENABLE(&hspi2);
...@@ -206,18 +188,22 @@ uint8_t imu_accel_detect(void) ...@@ -206,18 +188,22 @@ uint8_t imu_accel_detect(void)
imu_enable_device(ACCEL_ID); imu_enable_device(ACCEL_ID);
imu_spi_start_read(ACCEL_WHO_AM_I,1); imu_spi_start_read(ACCEL_WHO_AM_I,1);
imu_wait_op_done(); if(imu_wait_op_done()==IMU_DONE)
imu_spi_get_data(&data);
if(data==ACCEL_ID)
{ {
ram_data[IMU_CNTRL]|=IMU_ACCEL_DET; imu_spi_get_data(&data);
return 0x01; 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 else
{
ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00; return 0x00;
}
} }
void imu_accel_start(void) void imu_accel_start(void)
...@@ -233,25 +219,25 @@ void imu_accel_stop(void) ...@@ -233,25 +219,25 @@ void imu_accel_stop(void)
void imu_accel_get_config(void) void imu_accel_get_config(void)
{ {
imu_enable_device(ACCEL_ID); imu_enable_device(ACCEL_ID);
imu_spi_start_read(accel_conf_reg,accel_conf_len); imu_spi_start_read(ACCEL_CNTRL_REG1,5);
imu_wait_op_done(); if(imu_wait_op_done()==IMU_DONE)
imu_spi_get_data(accel_conf_data); imu_spi_get_data(imu.accel_conf_data);
} }
void imu_accel_set_config(void) void imu_accel_set_config(void)
{ {
accel_conf_data[0]=0x3F; imu.accel_conf_data[0]=0x3F;
accel_conf_data[3]=0x10; imu.accel_conf_data[3]=0x10;
imu_enable_device(ACCEL_ID); 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(); imu_wait_op_done();
} }
void imu_accel_get_data(void) void imu_accel_get_data(void)
{ {
imu_enable_device(ACCEL_ID); 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) 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) ...@@ -260,9 +246,9 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
for(i=0;i<3;i++) for(i=0;i<3;i++)
{ {
accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)); imu.accel_data[i]=(int16_t)((data_in[i*2]+(data_in[i*2+1]<<8))*2);
data_out[i*2]=accel_data[i]%256; data_out[i*2]=imu.accel_data[i]&0x00FF;
data_out[i*2+1]=accel_data[i]/256; data_out[i*2+1]=(imu.accel_data[i]>>8)&0x00FF;
} }
} }
...@@ -273,18 +259,22 @@ uint8_t imu_gyro_detect(void) ...@@ -273,18 +259,22 @@ uint8_t imu_gyro_detect(void)
imu_enable_device(GYRO_ID); imu_enable_device(GYRO_ID);
imu_spi_start_read(GYRO_WHO_AM_I,1); imu_spi_start_read(GYRO_WHO_AM_I,1);
imu_wait_op_done(); if(imu_wait_op_done()==IMU_DONE)
imu_spi_get_data(&data);
if(data==GYRO_ID)
{ {
ram_data[IMU_CNTRL]|=IMU_GYRO_DET; imu_spi_get_data(&data);
return 0x01; 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 else
{
ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00; return 0x00;
}
} }
void imu_gyro_start(void) void imu_gyro_start(void)
...@@ -299,26 +289,26 @@ void imu_gyro_stop(void) ...@@ -299,26 +289,26 @@ void imu_gyro_stop(void)
void imu_gyro_set_config(void) void imu_gyro_set_config(void)
{ {
gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up imu.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[3]=0x20;// Full scale = 2000dps
imu_enable_device(GYRO_ID); 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(); imu_wait_op_done();
} }
void imu_gyro_get_config(void) void imu_gyro_get_config(void)
{ {
imu_enable_device(GYRO_ID); imu_enable_device(GYRO_ID);
imu_spi_start_read(gyro_conf_reg,gyro_conf_len); imu_spi_start_read(GYRO_CNTRL_REG1,6);
imu_wait_op_done(); if(imu_wait_op_done()==IMU_DONE)
imu_spi_get_data(gyro_conf_data); imu_spi_get_data(imu.gyro_conf_data);
} }
void imu_gyro_get_data(void) void imu_gyro_get_data(void)
{ {
imu_enable_device(GYRO_ID); 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) 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) ...@@ -327,36 +317,44 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
for(i=0;i<3;i++) 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]; 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]=gyro_data[i]%256; data_out[i*2]=imu.gyro_data[i]&0x00FF;
data_out[i*2+1]=gyro_data[i]/256; 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) 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; 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(); imu_start();
else if(data[IMU_CNTRL-address]&IMU_STOP) else if(data[IMU_CONTROL_OFFSET-ram_offset]&IMU_STOP)
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(); imu_start_gyro_cal();
} }
ram_read_word(IMU_CAL_SAMPLES,&num_samples); if(ram_in_window(imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,2,address,length))
if(ram_in_range(IMU_CAL_SAMPLES,address,length)) {
num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES-address]; ram_read_word(imu->memory,imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,&num_samples);
if(ram_in_range(IMU_CAL_SAMPLES+1,address,length)) if(ram_in_range(imu->ram_base_address+IMU_CAL_SAMPLES_OFFSET,address,length))
num_samples=(num_samples&0x00FF)+(data[IMU_CAL_SAMPLES+1-address]<<8); num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES_OFFSET-ram_offset];
imu_set_calibration_samples(num_samples); 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) 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 // interrupt handlers
...@@ -367,27 +365,27 @@ void IMU_SPI_IRQHANDLER(void) ...@@ -367,27 +365,27 @@ void IMU_SPI_IRQHANDLER(void)
/* SPI in mode Receiver and Overrun not occurred ---------------------------*/ /* 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)) 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++; num_rd++;
if(num_rd >= imu_spi_num_rd_data) if(num_rd >= imu.spi_num_rd_data)
{ {
imu_disable_device(); imu_disable_device();
imu_op_state=IMU_DONE; imu.op_state=IMU_DONE;
num_rd=0; num_rd=0;
num_wr=0;
} }
} }
/* SPI in mode Tramitter ---------------------------------------------------*/ /* 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((__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++; num_wr++;
} }
else else
{ {
num_wr=0;
__HAL_SPI_DISABLE_IT(&hspi2, SPI_IT_TXE); __HAL_SPI_DISABLE_IT(&hspi2, SPI_IT_TXE);
} }
} }
...@@ -407,66 +405,80 @@ void IMU_TIMER_IRQHandler(void) ...@@ -407,66 +405,80 @@ void IMU_TIMER_IRQHandler(void)
__HAL_TIM_CLEAR_IT(&htim, TIM_IT_UPDATE); __HAL_TIM_CLEAR_IT(&htim, TIM_IT_UPDATE);
if(imu_is_op_done()) if(imu_is_op_done())
{ {
if(imu_stop_flag==0x01) if(imu.stop_flag==0x01)
{ {
// stop the timer to get the imu data // stop the timer to get the imu data
HAL_TIM_Base_Stop_IT(&htim); HAL_TIM_Base_Stop_IT(&htim);
imu_stop_flag=0x00; imu.stop_flag=0x00;
imu_stopped=0x01; imu.stopped=0x01;
} }
else else
{ {
switch(state) 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_spi_get_data(data);
imu_gyro_convert_data(data,&ram_data[IMU_GYRO_X]); imu_gyro_convert_data(data,&imu.memory->data[imu.ram_base_address+IMU_GYRO_X_OFFSET]);
if(gyro_calibration) if(imu.gyro_calibration)
{ {
gyro_accum[0]+=gyro_data[0]; gyro_accum[0]+=imu.gyro_data[0];
gyro_accum[1]+=gyro_data[1]; gyro_accum[1]+=imu.gyro_data[1];
gyro_accum[2]+=gyro_data[2]; gyro_accum[2]+=imu.gyro_data[2];
num_samples++; num_samples++;
if(num_samples==gyro_calibration_num_samples) if(num_samples==imu.gyro_calibration_num_samples)
{ {
num_samples=0; num_samples=0;
gyro_center[0]=gyro_accum[0]/gyro_calibration_num_samples; imu.gyro_center[0]=gyro_accum[0]/imu.gyro_calibration_num_samples;
gyro_center[1]=gyro_accum[1]/gyro_calibration_num_samples; imu.gyro_center[1]=gyro_accum[1]/imu.gyro_calibration_num_samples;
gyro_center[2]=gyro_accum[2]/gyro_calibration_num_samples; imu.gyro_center[2]=gyro_accum[2]/imu.gyro_calibration_num_samples;
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[IMU_CNTRL]&=(~IMU_CALIBRATING); imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&=(~IMU_CALIBRATING);
if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00) if((imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]&IMU_RUNNING)==0x00)
imu_stop_flag=0x01; imu.stop_flag=0x01;
gyro_calibration=0x00; 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(); imu_accel_get_data();
}
state=IMU_GET_GYRO; state=IMU_GET_GYRO;
break; 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_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(); imu_gyro_get_data();
}
state=IMU_GET_ACCEL; state=IMU_GET_ACCEL;
break; break;
} }
} }
} }
else
{
if(time_is_timeout(&imu.time))
{
imu_disable_device();
imu.op_state=IMU_DONE;
}
}
} }
} }
} }
// public functions // public functions
uint8_t imu_init(TMemory *memory) uint8_t imu_init(TMemory *memory,unsigned int ram_base_address)
{ {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i; uint8_t i;
...@@ -502,22 +514,6 @@ uint8_t imu_init(TMemory *memory) ...@@ -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_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN,GPIO_PIN_SET);
HAL_GPIO_WritePin(IMU_GYRO_CS_PORT,IMU_GYRO_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 // configure the SPI module
hspi2.Instance = IMU_SPI; hspi2.Instance = IMU_SPI;
hspi2.Init.Mode = SPI_MODE_MASTER; hspi2.Init.Mode = SPI_MODE_MASTER;
...@@ -539,7 +535,7 @@ uint8_t imu_init(TMemory *memory) ...@@ -539,7 +535,7 @@ uint8_t imu_init(TMemory *memory)
/* imu timer configuration */ /* imu timer configuration */
htim.Instance=IMU_TIMER; htim.Instance=IMU_TIMER;
htim.Init.Period = 1000; htim.Init.Period = 1000;
htim.Init.Prescaler = 72; htim.Init.Prescaler = 84;
htim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim.Init.CounterMode = TIM_COUNTERMODE_UP; htim.Init.CounterMode = TIM_COUNTERMODE_UP;
htim.Init.RepetitionCounter=0; htim.Init.RepetitionCounter=0;
...@@ -551,9 +547,39 @@ uint8_t imu_init(TMemory *memory) ...@@ -551,9 +547,39 @@ uint8_t imu_init(TMemory *memory)
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE); __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 // detect the sensors
if(imu_gyro_detect()) if(imu_gyro_detect())
{ {
imu_gyro_get_config(); imu_gyro_get_config();
imu_gyro_set_config(); imu_gyro_set_config();
} }
...@@ -563,21 +589,12 @@ uint8_t imu_init(TMemory *memory) ...@@ -563,21 +589,12 @@ uint8_t imu_init(TMemory *memory)
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; return 0x01;
} }
void imu_start(void) 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 // start the accelerometer
imu_accel_start(); imu_accel_start();
...@@ -585,33 +602,33 @@ void imu_start(void) ...@@ -585,33 +602,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[IMU_CNTRL]|=IMU_RUNNING; imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_RUNNING;
} }
} }
void imu_stop(void) 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; 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[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) void imu_set_calibration_samples(uint16_t num_samples)
{ {
gyro_calibration_num_samples=num_samples; imu.gyro_calibration_num_samples=num_samples;
ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256; imu.memory->data[imu.ram_base_address+IMU_CAL_SAMPLES_OFFSET]=imu.gyro_calibration_num_samples%256;
ram_data[IMU_CAL_SAMPLES+1]=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) 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 // start the accelerometer
imu_accel_start(); imu_accel_start();
...@@ -620,16 +637,16 @@ void imu_start_gyro_cal(void) ...@@ -620,16 +637,16 @@ 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[IMU_CNTRL]|=IMU_CALIBRATING; imu.memory->data[imu.ram_base_address+IMU_CONTROL_OFFSET]|=IMU_CALIBRATING;
gyro_center[0]=0; imu.gyro_center[0]=0;
gyro_center[1]=0; imu.gyro_center[1]=0;
gyro_center[2]=0; imu.gyro_center[2]=0;
gyro_calibration=0x01; imu.gyro_calibration=0x01;
} }
uint8_t imu_is_gyro_calibrated(void) 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; return 0x00;
else else
return 0x01; return 0x01;
...@@ -637,15 +654,15 @@ uint8_t imu_is_gyro_calibrated(void) ...@@ -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) void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
{ {
*x=gyro_data[0]; *x=imu.gyro_data[0];
*y=gyro_data[1]; *y=imu.gyro_data[1];
*z=gyro_data[2]; *z=imu.gyro_data[2];
} }
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)
{ {
*x=accel_data[0]; *x=imu.accel_data[0];
*y=accel_data[1]; *y=imu.accel_data[1];
*z=accel_data[2]; *z=imu.accel_data[2];
} }
src/darwin_sch.c
+ 4
− 0
View file @ f3e6b6aa
...@@ -13,6 +13,7 @@ void TIM1_CC_IRQHandler(void) ...@@ -13,6 +13,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC1) !=RESET) 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_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); scheduler_interrupt(&darwin_scheduler,SCHED_CH1);
} }
} }
...@@ -22,6 +23,7 @@ void TIM1_CC_IRQHandler(void) ...@@ -22,6 +23,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC2) !=RESET) 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_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); scheduler_interrupt(&darwin_scheduler,SCHED_CH2);
} }
} }
...@@ -31,6 +33,7 @@ void TIM1_CC_IRQHandler(void) ...@@ -31,6 +33,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC3) !=RESET) 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_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); scheduler_interrupt(&darwin_scheduler,SCHED_CH3);
} }
} }
...@@ -40,6 +43,7 @@ void TIM1_CC_IRQHandler(void) ...@@ -40,6 +43,7 @@ void TIM1_CC_IRQHandler(void)
if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC4) !=RESET) 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_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); scheduler_interrupt(&darwin_scheduler,SCHED_CH4);
} }
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment