Issue #22: ecu stm32 implement imu driver

KPI_Rover/Database/ulDatabase.h

@@ -23,6 +23,13 @@ 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_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,106 @@
+#include "ulImu.h"
+#include "cmsis_os.h"
+#include "ulog.h"
+#include "Database/ulDatabase.h"
+
+
+#define IMU_POLL_PERIOD_MS 20
+
+
+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 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
+};
+
+
+void ulImu_Update(I2C_HandleTypeDef *hi2c);
+
+
+static void ulImu_CalibrateGyro(I2C_HandleTypeDef *hi2c) {
+    // TODO calibrate
+    ulDatabase_setUint8(IMU_IS_CALIBRATED, 1);
+}
+
+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;
+    }
+
+    ulImu_CalibrateGyro(hi2c);
+
+    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;
+}
+
+void ulImu_Update(I2C_HandleTypeDef *hi2c) {
+    MPU_RawData_t raw;
+    uint8_t is_calibrate;
+
+    if (ulDatabase_getUint8(IMU_IS_CALIBRATED, &is_calibrate) && is_calibrate == 0) {
+        ulImu_CalibrateGyro(hi2c);
+        return;
+    }
+
+    if (drvImu_GetRawData(hi2c, &raw) != HAL_OK) {
+    	return;
+    }
+
+    float ax_ms2 = (raw.Accel_X - accel_bias[0]) / AccelScales[imuCfg.AccelRange] * GRAVITY_MS2;
+    float ay_ms2 = (raw.Accel_Y - accel_bias[1]) / AccelScales[imuCfg.AccelRange] * GRAVITY_MS2;
+    float az_ms2 = (raw.Accel_Z - accel_bias[2]) / AccelScales[imuCfg.AccelRange] * 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,12 @@
+#ifndef IMU_ULIMU_H_
+#define IMU_ULIMU_H_
+
+#include "drvImu.h"
+
+
+#define GRAVITY_MS2		9.81f
+
+
+HAL_StatusTypeDef ulImu_Init(I2C_HandleTypeDef *hi2c);
+
+#endif /* IMU_ULIMU_H_ */

KPI_Rover/KPIRover.c

@@ -1,19 +1,30 @@
 #include "KPIRover.h"
 #include "cmsis_os.h"
+#include "ulog.h"
 
 #include "Database/ulDatabase.h"
-
+#include "IMU/ulImu.h"
 
 static struct ulDatabase_ParamMetadata ulDatabase_params[] = {
 		{0, INT32, false, 0}, // MOTOR_FL_RPM,
 		{0, INT32, false, 0}, // MOTOR_FR_RPM,
 		{0, INT32, false, 0}, // MOTOR_RL_RPM,
 		{0, INT32, false, 0}, // MOTOR_RR_RPM,
 		{0, UINT16, true, 5}, // ENCODER_CONTROL_PERIOD_MS,
-		{0, FLOAT, true, 820.0f} // ENCODER_TICKS_PER_REVOLUTION,
+		{0, FLOAT, true, 820.0f}, // ENCODER_TICKS_PER_REVOLUTION,
+		{0, FLOAT, false, 0.0f}, // IMU_ACCEL_X
+		{0, FLOAT, false, 0.0f}, // IMU_ACCEL_Y
+		{0, FLOAT, false, 0.0f}, // IMU_ACCEL_Z
+		{0, FLOAT, false, 0.0f}, // IMU_GYRO_X
+		{0, FLOAT, false, 0.0f}, // IMU_GYRO_Y
+		{0, FLOAT, false, 0.0f}, // IMU_GYRO_Z
+		{0, UINT8, false, 0}, // IMU_IS_CALIBRATED
 };
 
+extern I2C_HandleTypeDef hi2c3;
+
 void KPIRover_Init(void) {
 	ulDatabase_init(ulDatabase_params, sizeof(ulDatabase_params) / sizeof(struct ulDatabase_ParamMetadata));
 	ulEncoder_Init();
+	ulImu_Init(&hi2c3);
 }