Issue #20: Fix overflow 16-bit timers, extern timers and integrate with parameter database

Author: koroliLich

KPI_Rover/Database/ulDatabase.h

@@ -17,10 +17,11 @@ enum ulDatabase_ParamType {
 };
 
 enum ulDatabase_ParamId {
-	PARAM_MOTOR_P_GAIN,
-	PARAM_HEADING_OFFSET,
-	PARAM_BATTERY_LEVEL,
-	PARAM_ENCODER_COUNT,
+	MOTOR_FL_RPM,
+	MOTOR_FR_RPM,
+	MOTOR_RL_RPM,
+	MOTOR_RR_RPM,
+	ENCODER_CONTROL_PERIOD_MS,
 	PARAM_COUNT
 };
 

KPI_Rover/Encoders/drvEncoder.c

@@ -1,30 +1,25 @@
 #include "drvEncoder.h"
 
 
+extern TIM_HandleTypeDef htim1;
 extern TIM_HandleTypeDef htim2;
-/*other encoders
-extern TIM_HandleTypeDef htim?;
-extern TIM_HandleTypeDef htim?;
-extern TIM_HandleTypeDef htim?;
-*/
+extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim4;
+
 
 void drvEncoder_Init(void) {
+    HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
     HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
-    /* other encoders
-    HAL_TIM_Encoder_Start(&htim?, TIM_CHANNEL_ALL);
-    HAL_TIM_Encoder_Start(&htim?, TIM_CHANNEL_ALL);
-    HAL_TIM_Encoder_Start(&htim?, TIM_CHANNEL_ALL);
-    */
+    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
+    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
 }
 
 uint32_t drvEncoder_Read(uint8_t channel) {
     switch (channel) {
-        case 0: return TIM2->CNT;
-        /* other encoders
-        case 1: return TIM?->CNT;
-        case 2: return TIM?->CNT;
-        case 3: return TIM?->CNT;
-        */
+        case 0: return TIM1->CNT;
+        case 1: return TIM2->CNT;
+        case 2: return TIM3->CNT;
+        case 3: return TIM4->CNT;
         default: return 0;
     }
 }

KPI_Rover/Encoders/ulEncoder.c

@@ -1,33 +1,53 @@
 #include "ulEncoder.h"
 #include "drvEncoder.h"
+#include "Database/ulDatabase.h"
 
 #include "FreeRTOS.h"
 #include "cmsis_os2.h"
 
 
 #define TICKS_PER_REVOLUTION 820.0f
-#define MOTORS_CONTROL_PERIOD_MS 20
 #define MOTORS_COUNT 4
 
+#define TIMER_16_BIT_MAX 0x10000
+#define OVERFLOW_THRESHOLD TIMER_16_BIT_MAX / 2
+
 
 static uint32_t lastTicks_RPM[MOTORS_COUNT];
 static uint32_t lastTicks_Ethernet[MOTORS_COUNT];
 
+static uint16_t encoder_period_ms = 5;
+
 static osTimerId_t encoderTimerHandle;
 
 
+static inline int32_t ulEncoder_CalculateDiff(uint32_t current, uint32_t last) {
+    int32_t diff = (int32_t)(current - last);
+
+    // overflow in 16-bit timer
+    if (diff < -OVERFLOW_THRESHOLD) {
+        diff += TIMER_16_BIT_MAX;
+    }
+    else if (diff > OVERFLOW_THRESHOLD) {
+        diff -= TIMER_16_BIT_MAX;
+    }
+
+    return diff;
+}
+
 static void ulEncoder_TimerCallback(void *argument) {
 
 	for (int i = 0; i < MOTORS_COUNT; i++) {
         uint32_t currentTicks = drvEncoder_Read(i);
         uint32_t lastTicks = lastTicks_RPM[i];
 
-        int32_t diff = (int32_t) (currentTicks - lastTicks);
+        int32_t diff = ulEncoder_CalculateDiff(currentTicks, lastTicks);
 
-        float ticks_per_sec = (float) diff * (1000.0f / MOTORS_CONTROL_PERIOD_MS);
+        float ticks_per_sec = (float) diff * (1000.0f / encoder_period_ms);
         float rpm = ticks_per_sec * 60.0f/ TICKS_PER_REVOLUTION;
 
-        // TODO: adding rpm to the database
+        int32_t rpm_to_db = (int32_t)rpm;
+        ulDatabase_setInt32(MOTOR_FL_RPM + i, rpm_to_db);
 
         lastTicks_RPM[i] = currentTicks;
     }
@@ -37,6 +57,10 @@ void ulEncoder_Init(void) {
 
 	drvEncoder_Init();
 
+	if (!ulDatabase_getUint16(ENCODER_CONTROL_PERIOD_MS, &encoder_period_ms) || encoder_period_ms == 0) {
+		encoder_period_ms = 5;
+	}
+
     for (int i = 0; i < MOTORS_COUNT; i++) {
         uint32_t initialTicks = drvEncoder_Read(i);
         lastTicks_RPM[i] = initialTicks;
@@ -50,7 +74,7 @@ void ulEncoder_Init(void) {
     encoderTimerHandle = osTimerNew(ulEncoder_TimerCallback, osTimerPeriodic, NULL, &encoderTimer_attributes);
 
     if (encoderTimerHandle != NULL) {
-      osTimerStart(encoderTimerHandle, MOTORS_CONTROL_PERIOD_MS);
+      osTimerStart(encoderTimerHandle, encoder_period_ms);
     }
 }
 
@@ -61,9 +85,10 @@ void ulEncoder_GetDiffForEthernet(int32_t *diffOutput) {
         uint32_t currentTicks = drvEncoder_Read(i);
         uint32_t lastTicks = lastTicks_Ethernet[i];
 
-        int32_t diff = (int32_t)(currentTicks - lastTicks);
+        int32_t diff = ulEncoder_CalculateDiff(currentTicks, lastTicks);
 
         diffOutput[i] = diff;
         lastTicks_Ethernet[i] = currentTicks;
     }
 }
+

KPI_Rover/KPIRover.c

@@ -5,8 +5,14 @@
 
 
 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,
 };
 
 void KPIRover_Init(void) {
 	ulDatabase_init(ulDatabase_params, sizeof(ulDatabase_params) / sizeof(struct ulDatabase_ParamMetadata));
+	ulEncoder_Init();
 }