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

IMU module workng.

Chnaged the priorities of the IMU and dynamixel slave interrupts to avoid hanging when calling the imu_stop() function.
parent 41a3faac
No related branches found
No related tags found
No related merge requests found
...@@ -375,7 +375,7 @@ typedef enum { ...@@ -375,7 +375,7 @@ typedef enum {
#define IMU_MEM_LENGTH 15 #define IMU_MEM_LENGTH 15
#define IMU_START 0x01 #define IMU_START 0x01
#define IMU_STOP 0x02 #define IMU_STOP 0x02
#define IMU_START_CAL 0x03 #define IMU_START_CAL 0x04
#define IMU_RUNNING 0x10 #define IMU_RUNNING 0x10
#define IMU_CALIBRATING 0x20 #define IMU_CALIBRATING 0x20
#define IMU_ACCEL_DET 0x40 #define IMU_ACCEL_DET 0x40
......
...@@ -106,7 +106,7 @@ uint16_t adc_convert_temperature(uint16_t value) ...@@ -106,7 +106,7 @@ uint16_t adc_convert_temperature(uint16_t value)
{ {
float conv_value; float conv_value;
conv_value=(TEMP_V25-value*VOLTAGE_DELTA)*TEMP_INV_SLOPE+25.0; conv_value=(value*VOLTAGE_DELTA-TEMP_V25)*TEMP_INV_SLOPE+25.0;
return conv_value*(1<<10);// fixed point format 6 | 10 return conv_value*(1<<10);// fixed point format 6 | 10
} }
......
...@@ -134,7 +134,7 @@ void darwin_dyn_slave_init(void) ...@@ -134,7 +134,7 @@ void darwin_dyn_slave_init(void)
darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle); HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle);
// initialize the timer interrupts // initialize the timer interrupts
HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0); HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 2);
HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn); HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
} }
......
#include "imu.h" #include "imu.h"
#include "ram.h" #include "ram.h"
#include "gpio.h"
#define IMU_SPI SPI2 #define IMU_SPI SPI2
#define IMU_ENABEL_SPI_CLK __SPI2_CLK_ENABLE() #define IMU_ENABEL_SPI_CLK __SPI2_CLK_ENABLE()
...@@ -106,7 +105,7 @@ const uint8_t gyro_data_len=6; ...@@ -106,7 +105,7 @@ const uint8_t gyro_data_len=6;
int16_t gyro_center[3]; int16_t gyro_center[3];
int32_t gyro_data[3]; int32_t gyro_data[3];
uint8_t gyro_calibration; uint8_t gyro_calibration;
uint8_t gyro_calibration_num_samples; uint16_t gyro_calibration_num_samples;
// accelerometer variables // accelerometer variables
const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1; const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
const uint8_t accel_conf_len=5; const uint8_t accel_conf_len=5;
...@@ -259,7 +258,7 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out) ...@@ -259,7 +258,7 @@ 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++)
{ {
value=(data_in[i*2]+(data_in[i*2+1]<<8))<<1; value=(data_in[i*2]+(data_in[i*2+1]<<8));
data_out[i*2]=value%256; data_out[i*2]=value%256;
data_out[i*2+1]=value/256; data_out[i*2+1]=value/256;
} }
...@@ -506,7 +505,7 @@ void imu_init(void) ...@@ -506,7 +505,7 @@ void imu_init(void)
HAL_SPI_Init(&hspi2); HAL_SPI_Init(&hspi2);
/* Configure the SPI interrupt priority */ /* Configure the SPI interrupt priority */
HAL_NVIC_SetPriority(IMU_SPI_IRQn, 2, 1); HAL_NVIC_SetPriority(IMU_SPI_IRQn, 1, 2);
HAL_NVIC_EnableIRQ(IMU_SPI_IRQn); HAL_NVIC_EnableIRQ(IMU_SPI_IRQn);
/* imu timer configuration */ /* imu timer configuration */
...@@ -519,7 +518,7 @@ void imu_init(void) ...@@ -519,7 +518,7 @@ void imu_init(void)
HAL_TIM_Base_Init(&htim); HAL_TIM_Base_Init(&htim);
IMU_ENABLE_TIMER_CLK; IMU_ENABLE_TIMER_CLK;
HAL_NVIC_SetPriority(IMU_TIMER_IRQn, 2, 0); HAL_NVIC_SetPriority(IMU_TIMER_IRQn, 1, 3);
HAL_NVIC_EnableIRQ(IMU_TIMER_IRQn); HAL_NVIC_EnableIRQ(IMU_TIMER_IRQn);
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE); __HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE);
...@@ -604,6 +603,8 @@ uint8_t imu_in_range(unsigned short int address,unsigned short int length) ...@@ -604,6 +603,8 @@ 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) 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(ram_in_range(DARWIN_IMU_CNTRL,address,length))
{ {
if(data[DARWIN_IMU_CNTRL-address]&IMU_START) if(data[DARWIN_IMU_CNTRL-address]&IMU_START)
...@@ -613,5 +614,11 @@ void imu_process_write_cmd(unsigned short int address,unsigned short int length, ...@@ -613,5 +614,11 @@ void imu_process_write_cmd(unsigned short int address,unsigned short int length,
else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL) else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL)
imu_start_gyro_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