Issue #22: ecu stm32 implement imu driver

Core/Src/main.c

@@ -847,7 +847,7 @@ void StartDefaultTask(void *argument)
   /* Infinite loop */
   for(;;)
   {
-	  ULOG_DEBUG("Hello from default task %d", count++);
+	  //ULOG_DEBUG("Hello from default task %d", count++);
 	  osDelay(101); // Wait for 1 second
   }
   /* USER CODE END 5 */

KPI_Rover/Database/ulDatabase.h

@@ -23,6 +23,15 @@ enum ulDatabase_ParamId {
 	MOTOR_RR_RPM,
 	ENCODER_CONTROL_PERIOD_MS,
 	ENCODER_TICKS_PER_REVOLUTION,
+	IMU_ACCEL_X,
+	IMU_ACCEL_Y,
+	IMU_ACCEL_Z,
+	IMU_GYRO_X,
+	IMU_GYRO_Y,
+	IMU_GYRO_Z,
+	IMU_CALIB_CMD,
+	IMU_CALIB_STATUS,
+	IMU_IS_CALIBRATED,
 	PARAM_COUNT
 };
 

KPI_Rover/IMU/drvImu.c

@@ -0,0 +1,62 @@
+#include "drvImu.h"
+
+
+#define I2C_TIMEOUT 100
+
+
+HAL_StatusTypeDef drvImu_Init(I2C_HandleTypeDef *hi2c, MPU_Config_t *config) {
+	uint8_t check;
+	uint8_t data;
+	HAL_StatusTypeDef status;
+
+	status = HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, REG_WHO_AM_I, 1, &check, 1, I2C_TIMEOUT);
+	if (status != HAL_OK || check != 0x68) {
+		return HAL_ERROR;
+	}
+
+	data = 0x00;
+	status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, REG_PWR_MGMT_1, 1, &data, 1, I2C_TIMEOUT);
+    if (status != HAL_OK) {
+    	return status;
+    }
+
+    data = config->DlpfConfig;
+    status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, REG_DLPF_CONFIG, 1, &data, 1, I2C_TIMEOUT);
+    if (status != HAL_OK) {
+    	return status;
+    }
+
+    data = (config->GyroRange << 3);
+    status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, REG_GYRO_CONFIG, 1, &data, 1, I2C_TIMEOUT);
+    if (status != HAL_OK) {
+    	return status;
+    }
+
+    data = (config->AccelRange << 3);
+    status = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, REG_ACCEL_CONFIG, 1, &data, 1, I2C_TIMEOUT);
+
+    return status;
+}
+
+HAL_StatusTypeDef drvImu_GetRawData(I2C_HandleTypeDef *hi2c, MPU_RawData_t *pRawData) {
+    uint8_t buffer[14];
+    HAL_StatusTypeDef status;
+
+    status = HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, REG_ACCEL_XOUT_H, 1, buffer, 14, I2C_TIMEOUT);
+
+    if (status == HAL_OK) {
+        pRawData->Accel_X = (int16_t)(buffer[0] << 8 | buffer[1]);
+        pRawData->Accel_Y = (int16_t)(buffer[2] << 8 | buffer[3]);
+        pRawData->Accel_Z = (int16_t)(buffer[4] << 8 | buffer[5]);
+
+        // if we need
+        pRawData->Temp    = (int16_t)(buffer[6] << 8 | buffer[7]);
+
+        pRawData->Gyro_X  = (int16_t)(buffer[8] << 8 | buffer[9]);
+        pRawData->Gyro_Y  = (int16_t)(buffer[10] << 8 | buffer[11]);
+        pRawData->Gyro_Z  = (int16_t)(buffer[12] << 8 | buffer[13]);
+    }
+
+    return status;
+}
+

KPI_Rover/IMU/drvImu.h

@@ -0,0 +1,57 @@
+#ifndef IMU_DRVIMU_H_
+#define IMU_DRVIMU_H_
+
+#include "main.h"
+
+
+#define MPU6050_ADDR 			(0x68 << 1)
+
+#define REG_WHO_AM_I  			0x75
+#define REG_PWR_MGMT_1 			0x6B
+#define REG_GYRO_CONFIG     	0x1B
+#define REG_ACCEL_CONFIG    	0x1C
+#define REG_DLPF_CONFIG         0x1A
+#define REG_ACCEL_XOUT_H 		0x3B
+
+#define ACCEL_SENS_2G       	16384.0f
+#define ACCEL_SENS_4G       	8192.0f
+#define ACCEL_SENS_8G       	4096.0f
+#define ACCEL_SENS_16G       	2048.0f
+
+#define GYRO_SENS_250s       	131.0f
+#define GYRO_SENS_500s       	65.5f
+#define GYRO_SENS_1000s       	32.8f
+#define GYRO_SENS_2000s       	16.4f
+
+
+typedef enum {
+    MPU_ACC_2G  = 0x00,
+    MPU_ACC_4G  = 0x01,
+    MPU_ACC_8G  = 0x02,
+    MPU_ACC_16G = 0x03
+} MPU_AccelRange_e;
+
+typedef enum {
+    MPU_GYRO_250s  = 0x00,
+    MPU_GYRO_500s  = 0x01,
+    MPU_GYRO_1000s = 0x02,
+    MPU_GYRO_2000s = 0x03
+} MPU_GyroRange_e;
+
+typedef struct {
+    MPU_AccelRange_e AccelRange;
+    MPU_GyroRange_e  GyroRange;
+    uint8_t          DlpfConfig;
+} MPU_Config_t;
+
+typedef struct {
+    int16_t Accel_X, Accel_Y, Accel_Z;
+    int16_t Temp; // if we need
+    int16_t Gyro_X, Gyro_Y, Gyro_Z;
+} MPU_RawData_t;
+
+
+HAL_StatusTypeDef drvImu_Init(I2C_HandleTypeDef *hi2c, MPU_Config_t *config);
+HAL_StatusTypeDef drvImu_GetRawData(I2C_HandleTypeDef *hi2c, MPU_RawData_t *pRawData);
+
+#endif /* IMU_DRVIMU_H_ */

KPI_Rover/IMU/ulImu.c

@@ -0,0 +1,206 @@
+#include "ulImu.h"
+#include "cmsis_os.h"
+#include "ulog.h"
+#include "Database/ulDatabase.h"
+#include <float.h>
+
+
+#define IMU_POLL_PERIOD_MS 20
+
+#define CALIB_SAMPLES 100
+
+
+static osTimerId_t imuTimerHandle;
+
+
+static float accel_bias[3] = {0.0f, 0.0f, 0.0f};
+static float gyro_bias[3]  = {0.0f, 0.0f, 0.0f};
+
+static int32_t calib_sum[3] = {0, 0, 0};
+static uint16_t calib_count = 0;
+
+static float acc_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
+static float acc_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
+
+static float accel_scale_corr[3] = {1.0f, 1.0f, 1.0f};
+
+
+static MPU_Config_t imuCfg = {
+    .AccelRange = MPU_ACC_2G,
+    .GyroRange  = MPU_GYRO_250s,
+    .DlpfConfig = 0x03  // 44 Hz filtering, 4.8 delay
+};
+
+static const float AccelScales[] = {
+    ACCEL_SENS_2G,
+    ACCEL_SENS_4G,
+    ACCEL_SENS_8G,
+    ACCEL_SENS_16G
+};
+
+static const float GyroScales[] = {
+    GYRO_SENS_250s,
+    GYRO_SENS_500s,
+    GYRO_SENS_1000s,
+    GYRO_SENS_2000s
+};
+
+
+static void ulImu_Update(I2C_HandleTypeDef *hi2c);
+
+
+static void ulImu_Calibration(I2C_HandleTypeDef *hi2c, uint8_t cmd) {
+    MPU_RawData_t raw;
+    uint8_t current_status = STATUS_IDLE;
+    ulDatabase_getUint8(IMU_CALIB_STATUS, &current_status);
+
+    if (current_status == STATUS_IDLE) {
+    	calib_sum[0] = 0;
+    	calib_sum[1] = 0;
+    	calib_sum[2] = 0;
+    	calib_count = 0;
+        ulDatabase_setUint8(IMU_CALIB_STATUS, STATUS_IN_PROGRESS);
+        return;
+    }
+
+    if (current_status == STATUS_IN_PROGRESS) {
+
+    	if (cmd == CMD_CALIB_APPLY) {
+
+    		for (int i = 0; i < 3; i++) {
+    			if (acc_max[i] != -FLT_MAX && acc_min[i] != FLT_MAX) {
+    				accel_bias[i] = (acc_max[i] + acc_min[i]) / 2.0f;
+
+    				float measured_1g = (acc_max[i] - acc_min[i]) / 2.0f;
+    				float expected_1g = AccelScales[imuCfg.AccelRange];
+
+    				if (measured_1g > (expected_1g * 0.5f)) {
+    					accel_scale_corr[i] = measured_1g / AccelScales[imuCfg.AccelRange];
+            		}
+    			}
+
+    			acc_max[i] = -FLT_MAX;
+    			acc_min[i] = FLT_MAX;
+            }
+
+            ulDatabase_setUint8(IMU_CALIB_STATUS, STATUS_DONE);
+            ulDatabase_setUint8(IMU_IS_CALIBRATED, 1);
+            return;
+        }
+
+        if (drvImu_GetRawData(hi2c, &raw) != HAL_OK) {
+        	ulDatabase_setUint8(IMU_CALIB_STATUS, STATUS_ERROR);
+        	return;
+        }
+
+        if (cmd == CMD_CALIB_GYRO) {
+            calib_sum[0] += raw.Gyro_X;
+            calib_sum[1] += raw.Gyro_Y;
+            calib_sum[2] += raw.Gyro_Z;
+        }
+        else if (cmd >= CMD_CALIB_ACC_Z_POS && cmd <= CMD_CALIB_ACC_Y_NEG) {
+            calib_sum[0] += raw.Accel_X;
+            calib_sum[1] += raw.Accel_Y;
+            calib_sum[2] += raw.Accel_Z;
+        }
+
+        calib_count++;
+
+        if (calib_count >= CALIB_SAMPLES) {
+            float avg_x = (float)calib_sum[0] / CALIB_SAMPLES;
+            float avg_y = (float)calib_sum[1] / CALIB_SAMPLES;
+            float avg_z = (float)calib_sum[2] / CALIB_SAMPLES;
+
+            if (cmd == CMD_CALIB_GYRO) {
+                gyro_bias[0] = avg_x;
+                gyro_bias[1] = avg_y;
+                gyro_bias[2] = avg_z;
+            }
+            else {
+
+                float avgs[3] = {avg_x, avg_y, avg_z};
+
+                for (int i = 0; i < 3; i++) {
+                    if (avgs[i] > acc_max[i]) {
+                    	acc_max[i] = avgs[i];
+                    }
+                    if (avgs[i] < acc_min[i]) {
+                    	acc_min[i] = avgs[i];
+                    }
+                }
+            }
+
+            ulDatabase_setUint8(IMU_CALIB_STATUS, STATUS_DONE);
+        }
+    }
+}
+
+
+void ulImu_TimerCallback(void *argument) {
+	I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef *)argument;
+	ulImu_Update(hi2c);
+}
+
+HAL_StatusTypeDef ulImu_Init(I2C_HandleTypeDef *hi2c) {
+    HAL_StatusTypeDef status;
+
+    status = drvImu_Init(hi2c, &imuCfg);
+    if (status != HAL_OK) {
+    	return status;
+    }
+
+    const osTimerAttr_t imuTimer_attributes = {
+    	.name = "IMUTimer"
+    };
+
+    imuTimerHandle = osTimerNew(ulImu_TimerCallback, osTimerPeriodic, (void*)hi2c, &imuTimer_attributes);
+    if (imuTimerHandle == NULL) {
+        ULOG_ERROR("Failed to create IMU timer");
+        return HAL_ERROR;
+    }
+    if (osTimerStart(imuTimerHandle, IMU_POLL_PERIOD_MS) != osOK) {
+        ULOG_ERROR("Failed to start IMU timer");
+        return HAL_ERROR;
+    }
+
+    return HAL_OK;
+}
+
+static void ulImu_Update(I2C_HandleTypeDef *hi2c) {
+    uint8_t cmd = CMD_IDLE;
+
+    if (ulDatabase_getUint8(IMU_CALIB_CMD, &cmd) && cmd != CMD_IDLE) {
+    	ulImu_Calibration(hi2c, cmd);
+        return;
+    }
+    else {
+        uint8_t status = STATUS_IDLE;
+        ulDatabase_getUint8(IMU_CALIB_STATUS, &status);
+
+        if (status != STATUS_IDLE) {
+            ulDatabase_setUint8(IMU_CALIB_STATUS, STATUS_IDLE);
+        }
+    }
+
+    MPU_RawData_t raw;
+    if (drvImu_GetRawData(hi2c, &raw) != HAL_OK) {
+    	return;
+    }
+
+    float ax_ms2 = (raw.Accel_X - accel_bias[0]) / (AccelScales[imuCfg.AccelRange] * accel_scale_corr[0]) * GRAVITY_MS2;
+    float ay_ms2 = (raw.Accel_Y - accel_bias[1]) / (AccelScales[imuCfg.AccelRange] * accel_scale_corr[1]) * GRAVITY_MS2;
+    float az_ms2 = (raw.Accel_Z - accel_bias[2]) / (AccelScales[imuCfg.AccelRange] * accel_scale_corr[2]) * GRAVITY_MS2;
+
+    float gx_angle = (raw.Gyro_X - gyro_bias[0]) / GyroScales[imuCfg.GyroRange];
+    float gy_angle = (raw.Gyro_Y - gyro_bias[1]) / GyroScales[imuCfg.GyroRange];
+    float gz_angle = (raw.Gyro_Z - gyro_bias[2]) / GyroScales[imuCfg.GyroRange];
+
+
+    ulDatabase_setFloat(IMU_ACCEL_X, ax_ms2);
+    ulDatabase_setFloat(IMU_ACCEL_Y, ay_ms2);
+    ulDatabase_setFloat(IMU_ACCEL_Z, az_ms2);
+
+    ulDatabase_setFloat(IMU_GYRO_X, gx_angle);
+    ulDatabase_setFloat(IMU_GYRO_Y, gy_angle);
+    ulDatabase_setFloat(IMU_GYRO_Z, gz_angle);
+}

KPI_Rover/IMU/ulImu.h

@@ -0,0 +1,32 @@
+#ifndef IMU_ULIMU_H_
+#define IMU_ULIMU_H_
+
+#include "drvImu.h"
+
+
+#define GRAVITY_MS2		9.81f
+
+
+typedef enum {
+    CMD_IDLE = 0,
+    CMD_CALIB_GYRO = 1,
+    CMD_CALIB_ACC_Z_POS = 2,
+    CMD_CALIB_ACC_Z_NEG = 3,
+    CMD_CALIB_ACC_X_POS = 4,
+    CMD_CALIB_ACC_X_NEG = 5,
+    CMD_CALIB_ACC_Y_POS = 6,
+    CMD_CALIB_ACC_Y_NEG = 7,
+    CMD_CALIB_APPLY = 8
+} ImuCalibCmd_t;
+
+typedef enum {
+    STATUS_IDLE = 0,
+    STATUS_IN_PROGRESS = 1,
+    STATUS_DONE = 2,
+    STATUS_ERROR = 3
+} ImuCalibStatus_t;
+
+
+HAL_StatusTypeDef ulImu_Init(I2C_HandleTypeDef *hi2c);
+
+#endif /* IMU_ULIMU_H_ */