diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 27921e9c06cfb71d1b54b93ab02b3e5254aeb75a..3a579afe35a99f43d5dc7118db5ec11027c3fc1f 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 818549216acd6de83475d85241382dda2edb2f3b..1928371c122637e67f6e25af1c139ef63b209be4 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 3c701a1d709ef38c08fde876cde5f23baf105148..e00f1952cf84b9a0e1579d0f4e43883c983eb08e 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 02b924b949eeab971aa782a85d8e41c5a6429444..8175e80efb25fd04a7ac6755ff812c12c918ae06 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);
 }