Issue #22: Implementation the FreeRTOS timer

Author: koroliLich

KPI_Rover/IMU/ulImu.c

@@ -1,4 +1,16 @@
 #include "ulImu.h"
+#include "cmsis_os.h"
+#include "ulog.h"
+#include "Database/ulDatabase.h"
+
+
+#define IMU_POLL_PERIOD_MS 20
+
+
+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 MPU_Config_t imuCfg = {
@@ -22,15 +34,19 @@ static const float GyroScales[] = {
 };
 
 
-static float accel_bias[3] = {0.0f, 0.0f, 0.0f};
-static float gyro_bias[3]  = {0.0f, 0.0f, 0.0f};
+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;
 
@@ -41,6 +57,20 @@ HAL_StatusTypeDef ulImu_Init(I2C_HandleTypeDef *hi2c) {
 
     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;
 }
 

KPI_Rover/IMU/ulImu.h

@@ -2,13 +2,11 @@
 #define IMU_ULIMU_H_
 
 #include "drvImu.h"
-#include "Database/ulDatabase.h"
 
 
 #define GRAVITY_MS2		9.81f
 
 
 HAL_StatusTypeDef ulImu_Init(I2C_HandleTypeDef *hi2c);
-void ulImu_Update(I2C_HandleTypeDef *hi2c);
 
 #endif /* IMU_ULIMU_H_ */

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,
@@ -20,7 +20,10 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = {
 		{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);
 }