diff --git a/Core/Src/main.c b/Core/Src/main.c index 824509b..51964a3 100644 --- a/Core/Src/main.c +++ b/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 */ diff --git a/KPI_Rover/Database/ulDatabase.h b/KPI_Rover/Database/ulDatabase.h index 724e9ec..a9cdc1f 100644 --- a/KPI_Rover/Database/ulDatabase.h +++ b/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 }; diff --git a/KPI_Rover/IMU/drvImu.c b/KPI_Rover/IMU/drvImu.c new file mode 100644 index 0000000..329498c --- /dev/null +++ b/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; +} + diff --git a/KPI_Rover/IMU/drvImu.h b/KPI_Rover/IMU/drvImu.h new file mode 100644 index 0000000..2be6b9d --- /dev/null +++ b/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_ */ diff --git a/KPI_Rover/IMU/ulImu.c b/KPI_Rover/IMU/ulImu.c new file mode 100644 index 0000000..3a65b5c --- /dev/null +++ b/KPI_Rover/IMU/ulImu.c @@ -0,0 +1,206 @@ +#include "ulImu.h" +#include "cmsis_os.h" +#include "ulog.h" +#include "Database/ulDatabase.h" +#include + + +#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, ¤t_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); +} diff --git a/KPI_Rover/IMU/ulImu.h b/KPI_Rover/IMU/ulImu.h new file mode 100644 index 0000000..5fc4748 --- /dev/null +++ b/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_ */ diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c index d04d367..2dd5b2f 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -2,7 +2,7 @@ #include "cmsis_os.h" #include "Database/ulDatabase.h" - +#include "IMU/ulImu.h" static struct ulDatabase_ParamMetadata ulDatabase_params[] = { {0, INT32, false, 0}, // MOTOR_FL_RPM, @@ -10,10 +10,22 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = { {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_CALIB_CMD + {0, UINT8, false, 0}, // IMU_CALIB_STATUS + {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); }