From 497252e128f91291fd723301a8d5b7149799aa96 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 3 Apr 2016 18:27:07 +0000
Subject: [PATCH] IMU module workng. Chnaged the priorities of the IMU and
dynamixel slave interrupts to avoid hanging when calling the imu_stop()
function.
---
include/darwin_registers.h | 2 +-
src/adc_dma.c | 2 +-
src/darwin_dyn_slave.c | 2 +-
src/imu.c | 17 ++++++++++++-----
4 files changed, 15 insertions(+), 8 deletions(-)
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 27921e9..3a579af 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -375,7 +375,7 @@ typedef enum {
#define IMU_MEM_LENGTH 15
#define IMU_START 0x01
#define IMU_STOP 0x02
-#define IMU_START_CAL 0x03
+#define IMU_START_CAL 0x04
#define IMU_RUNNING 0x10
#define IMU_CALIBRATING 0x20
#define IMU_ACCEL_DET 0x40
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 8185492..1928371 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -106,7 +106,7 @@ uint16_t adc_convert_temperature(uint16_t 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
}
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 3c701a1..e00f195 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -134,7 +134,7 @@ void darwin_dyn_slave_init(void)
darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle);
// 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);
}
diff --git a/src/imu.c b/src/imu.c
index 02b924b..8175e80 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -1,6 +1,5 @@
#include "imu.h"
#include "ram.h"
-#include "gpio.h"
#define IMU_SPI SPI2
#define IMU_ENABEL_SPI_CLK __SPI2_CLK_ENABLE()
@@ -106,7 +105,7 @@ const uint8_t gyro_data_len=6;
int16_t gyro_center[3];
int32_t gyro_data[3];
uint8_t gyro_calibration;
-uint8_t gyro_calibration_num_samples;
+uint16_t gyro_calibration_num_samples;
// accelerometer variables
const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
const uint8_t accel_conf_len=5;
@@ -259,7 +258,7 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
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+1]=value/256;
}
@@ -506,7 +505,7 @@ void imu_init(void)
HAL_SPI_Init(&hspi2);
/* 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);
/* imu timer configuration */
@@ -519,7 +518,7 @@ void imu_init(void)
HAL_TIM_Base_Init(&htim);
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_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE);
@@ -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)
{
+ uint16_t num_samples;
+
if(ram_in_range(DARWIN_IMU_CNTRL,address,length))
{
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,
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);
}
--
GitLab