(#54) Fixing FOC control and adding in microsecond timer

CM7/Core/Inc/controls/filter_fo.h

@@ -255,7 +255,6 @@ static inline float
 FILTER_FO_run(FILTER_FO_Handle handle, const float inputValue)
 {
     FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle;
-
     float a1 = obj->a1;
     float b0 = obj->b0;
     float b1 = obj->b1;

CM7/Core/Inc/foc_ctrl.h

@@ -9,6 +9,20 @@
 #include "svgen.h"
 #include "pid.h"
 
+typedef enum {
+    SECTOR_1 = 0,
+    SECTOR_2,
+    SECTOR_3,
+    SECTOR_4,
+    SECTOR_5,
+    SECTOR_6,
+} sector_t;
+
+typedef enum {
+	FOC_OPEN_LOOP,
+	FOC_CLOSED_LOOP,
+} foc_mode_t;
+
 typedef float pwm_signal_t;
 
 typedef struct {
@@ -39,14 +53,14 @@ typedef struct {
 	float open_loop_ramp_position;
 	float open_loop_ramp_velocity;
 	float open_loop_ramp_jork;
-	uint32_t last_run_ms;
+	uint32_t last_run_us;
 
-	PID_Obj *q_pid;
-	PID_Obj *d_pid;
-	CLARKE_Obj *clarke_transform;
-	PARK_Obj *park_transform;
-	IPARK_Obj *ipark_transform;
-	SVGEN_Obj *svm;
+	PID_Obj q_pid;
+	PID_Obj d_pid;
+	CLARKE_Obj clarke_transform;
+	PARK_Obj park_transform;
+	IPARK_Obj ipark_transform;
+	SVGEN_Obj svm;
 } foc_ctrl_t;
 
 void foc_ctrl_init(foc_ctrl_t *controller);

CM7/Core/Inc/us_timer.h

@@ -0,0 +1,19 @@
+#ifndef US_TIMER_H
+#define US_TIMER_H
+
+#include <stdint.h>
+
+/**
+ * @brief Init microsecond timer
+ *
+ */
+void us_timer_init();
+
+/**
+ * @brief Get time in microseconds
+ *
+ * @return uint32_t
+ */
+uint32_t us_timer_get();
+
+#endif /* US_TIMER_H */

CM7/Core/Src/foc_ctrl.c

@@ -1,6 +1,8 @@
 #include "foc_ctrl.h"
 #include "proteus_config.h"
 #include "state_machine.h"
+#include "stm32h7xx_hal.h"
+#include "us_timer.h"
 #include <stdlib.h>
 #include <math.h>
 #include <assert.h>
@@ -45,7 +47,7 @@ void foc_ctrl_init(foc_ctrl_t *controller)
 	PID_setMinMax(&controller->q_pid, 0.0, 2.0);
 	PID_init(&controller->q_pid, sizeof(controller->q_pid));
 
-	//controller->last_run_us = us_timer_get();
+	controller->last_run_us = us_timer_get();
 	controller->open_loop_ramp_velocity = 0.3;
 }
 
@@ -65,13 +67,16 @@ osStatus_t foc_retrieve_cmd(foc_ctrl_t *controller, pwm_signal_t duty_cycles[3])
 	return osMessageQueueGet(controller->command_queue, duty_cycles, NULL, osWaitForever);
 }
 
-static void open_loop_ctrl(foc_ctrl_t *controller, pwm_signal_t duty_cycles[3])
+static void open_loop_ctrl(foc_ctrl_t *controller)
 {
+	pwm_signal_t duty_cycles[3];
+
 	/* Note: assuming we start from 0 angular velocity */
-	/* Assuming tick is set to 1 ms */
-	//uint32_t current_time_us = us_timer_get();
-	//uint32_t dt = (uint16_t) ((uint16_t) current_time_us - (uint16_t) controller->last_run_us);
-	uint32_t dt = 100;
+	uint32_t current_time_us = us_timer_get();
+	uint32_t dt = (uint32_t) ((uint32_t)current_time_us - (uint32_t) controller->last_run_us);
+	if ((int32_t)dt < 0) dt = 180; /* Account for wrap around of timer */
+
+	//printf("Time us: %ld\r\n", dt);
 
 	/* Ramp amplitude */
 	controller->open_loop_amplitude += ((OPEN_LOOP_MAX_AMPLITUDE ) / OPEN_LOOP_RAMP_TIME) * (dt / 1000000.0);
@@ -85,7 +90,7 @@ static void open_loop_ctrl(foc_ctrl_t *controller, pwm_signal_t duty_cycles[3])
 	duty_cycles[1] = controller->open_loop_amplitude * (sinf(controller->open_loop_ramp_position - 2.0 * M_PI / 3.0) + 1.0) / 2 * 10.0;
 	duty_cycles[2] = controller->open_loop_amplitude * (sinf(controller->open_loop_ramp_position + 2.0 * M_PI / 3.0) + 1.0) / 2 * 10.0;
 
-	//controller->last_run_us = current_time_us;
+	controller->last_run_us = current_time_us;
 	if (controller->open_loop_ramp_velocity < OPEN_LOOP_VELOCITY)
 		controller->open_loop_ramp_velocity *= 1.003;
 
@@ -94,6 +99,7 @@ static void open_loop_ctrl(foc_ctrl_t *controller, pwm_signal_t duty_cycles[3])
     //       (uint32_t)(duty_cycles[0] * 1000),
     //       (uint32_t)(duty_cycles[1] * 1000),
     //       (uint32_t)(duty_cycles[2] * 1000));
+	osMessageQueuePut(controller->command_queue, duty_cycles, 0U, 0U);
 }
 
 static void closed_loop_ctrl(foc_ctrl_t *controller, foc_data_t *msg)
@@ -112,18 +118,18 @@ static void closed_loop_ctrl(foc_ctrl_t *controller, foc_data_t *msg)
 		/* If its a phase current measurement, run control pipeline */
 		case FOCDATA_PHASE_CURRENT:
 			//TODO: Convert raw ADC reading of phases to current values
-			CLARKE_run(controller->clarke_transform, msg->payload.phase_currents, alpha_beta);
-			PARK_run(controller->park_transform, alpha_beta, id_iq);
+			CLARKE_run(&controller->clarke_transform, msg->payload.phase_currents, alpha_beta);
+			PARK_run(&controller->park_transform, alpha_beta, id_iq);
 
 			/*
 			* Here, adjust I_q based on the desired current, reference for I_d is always 0
 			* I_q is actually being varied
 			*/
-			PID_run_parallel(controller->q_pid, 0, id_iq[D], 0, &id_iq_pid[D]);
-			PID_run_parallel(controller->d_pid, controller->ref_current, id_iq[Q], 0, &id_iq_pid[Q]);
+			PID_run_parallel(&controller->q_pid, 0, id_iq[D], 0, &id_iq_pid[D]);
+			PID_run_parallel(&controller->d_pid, controller->ref_current, id_iq[Q], 0, &id_iq_pid[Q]);
 
-			IPARK_run(controller->ipark_transform, id_iq, alpha_beta);
-			SVGEN_run(controller->svm, alpha_beta, v_abc_pu);
+			IPARK_run(&controller->ipark_transform, id_iq, alpha_beta);
+			SVGEN_run(&controller->svm, alpha_beta, v_abc_pu);
 
 			calc_cmd[0] = (v_abc_pu[0] + 1.0) / 2.0;
 			calc_cmd[1] = (v_abc_pu[1] + 1.0) / 2.0;
@@ -135,14 +141,14 @@ static void closed_loop_ctrl(foc_ctrl_t *controller, foc_data_t *msg)
 
 		/* If its a new rotor position, update Park and Inverse Park Transforms */
 		case FOCDATA_ROTOR_POSITION:
-			PARK_setup(controller->park_transform, msg->payload.rotor_position);
-			IPARK_setup(controller->ipark_transform, msg->payload.rotor_position);
+			PARK_setup(&controller->park_transform, msg->payload.rotor_position);
+			IPARK_setup(&controller->ipark_transform, msg->payload.rotor_position);
 			controller->rotor_position = msg->payload.rotor_position;
 			break;
 
 		/* If its a new DC bus voltage reading, update the Space Vector Modulation block */
 		case FOCDATA_DC_BUS_VOLTAGE:
-			SVGEN_setup(controller->svm, 1/msg->payload.dc_bus_voltage);
+			SVGEN_setup(&controller->svm, 1/msg->payload.dc_bus_voltage);
 			controller->dc_bus_voltage = msg->payload.dc_bus_voltage;
 			break;
 
@@ -182,8 +188,7 @@ void vFOCctrl(void *pv_params)
 			switch (get_state()) {
 				case START:
 					/* Open Loop Startup Procedure */
-					open_loop_ctrl(controller, duty_cycles);
-					controller->last_run_ms = HAL_GetTick();
+					open_loop_ctrl(controller);
 					break;
 				case START_RUN:
 					/* Ensure ready to begin closed loop control */
@@ -201,7 +206,7 @@ void vFOCctrl(void *pv_params)
 				case FAULTED:
 					/* Do Nothing, Ensure signals are safe */
 					controller->open_loop_amplitude = 0.0;
-					controller->last_run_ms = HAL_GetTick();
+					controller->last_run_us = us_timer_get();
 					//TODO: this
 					break;
 				case STOP_NOW:
@@ -212,8 +217,10 @@ void vFOCctrl(void *pv_params)
 					/* Unknown State */
 					break;
 			}
-			open_loop_ctrl(controller, duty_cycles);
-			osMessageQueuePut(controller->command_queue, duty_cycles, 0U, 0U);
+			//uint32_t first_time = us_timer_get();
+			//closed_loop_ctrl(controller, &msg);
+			open_loop_ctrl(controller);
+			//printf("Time us:%ld\r\n", us_timer_get() - first_time);
 		}
 
 		/* Yield to other tasks */

CM7/Core/Src/main.c

@@ -67,6 +67,7 @@ SPI_HandleTypeDef hspi4;
 TIM_HandleTypeDef htim1;
 TIM_HandleTypeDef htim2;
 TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim6;
 TIM_HandleTypeDef htim8;
 
 UART_HandleTypeDef huart4;
@@ -109,6 +110,7 @@ static void MX_ADC1_Init(void);
 static void MX_ADC3_Init(void);
 static void MX_SPI1_Init(void);
 static void MX_UART4_Init(void);
+static void MX_TIM6_Init(void);
 void StartDefaultTask(void *argument);
 
 static void MX_NVIC_Init(void);
@@ -231,12 +233,14 @@ int main(void)
   MX_ADC3_Init();
   MX_SPI1_Init();
   MX_UART4_Init();
+  MX_TIM6_Init();
 
   /* Initialize interrupts */
   MX_NVIC_Init();
   /* USER CODE BEGIN 2 */
 
   ipcc_init(&ipcc, &hdma_memtomem_dma2_stream1, 1, 2);
+  us_timer_init(&htim6);
 
   /* USER CODE END 2 */
 
@@ -933,7 +937,7 @@ static void MX_TIM1_Init(void)
   sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
   sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
   sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
-  sBreakDeadTimeConfig.DeadTime = 2;
+  sBreakDeadTimeConfig.DeadTime = 0;
   sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
   sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
   sBreakDeadTimeConfig.BreakFilter = 0;
@@ -1050,6 +1054,44 @@ static void MX_TIM4_Init(void)
 
 }
 
+/**
+  * @brief TIM6 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM6_Init(void)
+{
+
+  /* USER CODE BEGIN TIM6_Init 0 */
+
+  /* USER CODE END TIM6_Init 0 */
+
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM6_Init 1 */
+
+  /* USER CODE END TIM6_Init 1 */
+  htim6.Instance = TIM6;
+  htim6.Init.Prescaler = 480;
+  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim6.Init.Period = 65535;
+  htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM6_Init 2 */
+
+  /* USER CODE END TIM6_Init 2 */
+
+}
+
 /**
   * @brief TIM8 Initialization Function
   * @param None
@@ -1113,7 +1155,7 @@ static void MX_TIM8_Init(void)
   sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
   sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
   sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
-  sBreakDeadTimeConfig.DeadTime = 2;
+  sBreakDeadTimeConfig.DeadTime = 0;
   sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
   sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
   sBreakDeadTimeConfig.BreakFilter = 0;
@@ -1345,7 +1387,7 @@ static void MX_GPIO_Init(void)
 void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
-  int curr_time = HAL_GetTick();
+  int curr_time = us_timer_get();
   float duty_cycles[3] = {0.5, 0.5, 0.5};
   gatedrv_write_pwm(&gatedrv_left, duty_cycles);
   gatedrv_write_pwm(&gatedrv_right, duty_cycles);
@@ -1355,17 +1397,17 @@ void StartDefaultTask(void *argument)
   {
     HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
     osDelay(500);
-    //printf("U: %ld A, V: %ld A, W: %ld A, Time: %ld ms\r\n",
-    //       (uint32_t)(duty_cycles[0] * 100),
-    //       (uint32_t)(duty_cycles[1] * 100),
-    //       (uint32_t)(duty_cycles[2] * 100),
-    //       HAL_GetTick() - curr_time);
+    printf("U: %ld A, V: %ld A, W: %ld A, Time: %ld us\r\n",
+           (uint32_t)(duty_cycles[0] * 100),
+           (uint32_t)(duty_cycles[1] * 100),
+           (uint32_t)(duty_cycles[2] * 100),
+           us_timer_get() - curr_time);
     duty_cycles[0] = duty_cycles[0] + 0.01 >= 1.0 ? 0.0 : duty_cycles[0] + 0.01;
     duty_cycles[1] = duty_cycles[1] + 0.01 >= 1.0 ? 0.0 : duty_cycles[1] + 0.01;
     duty_cycles[2] = duty_cycles[2] + 0.01 >= 1.0 ? 0.0 : duty_cycles[2] + 0.01;
     gatedrv_write_pwm(&gatedrv_left, duty_cycles);
     gatedrv_write_pwm(&gatedrv_right, duty_cycles);
-    curr_time = HAL_GetTick();
+    curr_time = us_timer_get();
     //printf("Failure: %d\r\n", ipcc_transfer(&ipcc, &msg));
   }
   /* USER CODE END 5 */

CM7/Core/Src/stm32h7xx_hal_msp.c

@@ -637,6 +637,17 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 
   /* USER CODE END TIM1_MspInit 1 */
   }
+  else if(htim_base->Instance==TIM6)
+  {
+  /* USER CODE BEGIN TIM6_MspInit 0 */
+
+  /* USER CODE END TIM6_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM6_CLK_ENABLE();
+  /* USER CODE BEGIN TIM6_MspInit 1 */
+
+  /* USER CODE END TIM6_MspInit 1 */
+  }
 
 }
 
@@ -843,6 +854,17 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
 
   /* USER CODE END TIM1_MspDeInit 1 */
   }
+  else if(htim_base->Instance==TIM6)
+  {
+  /* USER CODE BEGIN TIM6_MspDeInit 0 */
+
+  /* USER CODE END TIM6_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM6_CLK_DISABLE();
+  /* USER CODE BEGIN TIM6_MspDeInit 1 */
+
+  /* USER CODE END TIM6_MspDeInit 1 */
+  }
 
 }
 

CM7/Core/Src/us_timer.c

@@ -0,0 +1,18 @@
+#include "us_timer.h"
+
+#include "stm32h7xx.h"
+#include "stm32h7xx_hal_tim.h"
+
+static TIM_HandleTypeDef *timer;
+
+void us_timer_init(TIM_HandleTypeDef *htim)
+{
+    timer = htim;
+    HAL_TIM_Base_Start(timer);
+}
+
+uint32_t us_timer_get()
+{
+    // the count is prescaled in CubeMX to increment once per microsecond, so the count corresponds to microseconds
+    return timer->Instance->CNT / 2;
+}

Makefile/CM4/Makefile

@@ -1,5 +1,5 @@
 ##########################################################################################################################
-# File automatically-generated by tool: [projectgenerator] version: [4.2.0-B44] date: [Mon Jun 03 17:30:05 EDT 2024] 
+# File automatically-generated by tool: [projectgenerator] version: [4.2.0-B44] date: [Mon Jun 03 20:45:16 EDT 2024] 
 ##########################################################################################################################
 
 # ------------------------------------------------