Adding in encoder interface for SSI and incremental and spinning up t…

…ask to queue data

CM7/Core/Inc/encoder.h

@@ -0,0 +1,35 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include "stm32h7xx.h"
+#include "stm32h7xx_hal_spi.h"
+#include "foc_ctrl.h"
+#include "cmsis_os.h"
+#include <stdint.h>
+
+typedef struct {
+    SPI_HandleTypeDef *hspi;
+    TIM_HandleTypeDef *enc_timer;
+    uint32_t prev_cnt_value;
+    foc_ctrl_t *controller;
+    osThreadId_t thread;
+} encoder_t;
+
+extern const osThreadAttr_t encoder_observer_attributes;
+
+void vEncoderObserver(void *pv_params);
+
+/**
+ * Initialize Encoder
+*/
+void encoder_init(encoder_t *encoder, TIM_HandleTypeDef *enc_timer_handle, SPI_HandleTypeDef *spi_handle, foc_ctrl_t * controller);
+
+/**
+ * @brief Notifies encoder that poll interval has elapsed, sends speed and position data to foc controller
+ * NOTE: This function is meant to be called from the global ISR for whichever timer is used for polling
+ *
+ * @param encoder
+ */
+void encoder_poll_interval_notify(encoder_t *encoder);
+
+#endif /* SSI_ENCODER_H */

CM7/Core/Inc/foc_ctrl.h

@@ -66,7 +66,7 @@ typedef struct {
 void foc_ctrl_init(foc_ctrl_t *controller);
 
 /* Enqueue a single frame of controller observation */
-osStatus_t foc_queue_frame(foc_ctrl_t *controller, foc_data_t *phase_currents);
+osStatus_t foc_queue_frame(foc_ctrl_t *controller, foc_data_t *data);
 
 /* Wait for a command to be sent from the controller */
 osStatus_t foc_retrieve_cmd(foc_ctrl_t *controller, pwm_signal_t duty_cycles[3]);

CM7/Core/Src/encoder.c

@@ -0,0 +1,99 @@
+#include "encoder.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <math.h>
+
+#include "stm32h7xx.h"
+#include "stm32h7xx_hal_spi.h"
+
+#define ENCODER_MAX_COUNT (1 << 13)
+#define PULSES_PER_REV         (8191)
+
+void encoder_init(encoder_t *encoder, TIM_HandleTypeDef *enc_timer_handle, SPI_HandleTypeDef *spi_handle, foc_ctrl_t *controller)
+{
+    /* Assert Params */
+    assert(spi_handle);
+    assert(enc_timer_handle);
+    assert(controller);
+
+    /* Start Incremental Encoder */
+    encoder->enc_timer = enc_timer_handle;
+    assert(HAL_TIM_Encoder_Start(encoder->enc_timer, TIM_CHANNEL_ALL) == HAL_OK);
+
+    encoder->controller = controller;
+
+    /* Save SSI interface */
+    encoder->hspi = spi_handle;
+}
+
+static int16_t encoder_get_angle_ssi(encoder_t *encoder, float *angle)
+{
+    uint16_t counts = 0;
+
+    if (HAL_SPI_Receive(encoder->hspi, (uint8_t *) &counts, 1, 10) != HAL_OK)
+        return 1;
+
+    /* convert counts to radians */
+    *angle = (((float) counts) / ENCODER_MAX_COUNT) * 2 * M_PI;
+
+    return 0;
+}
+
+static void encoder_z_pulse_handler(encoder_t *encoder)
+{
+    encoder->enc_timer->Instance->CNT = 0;
+}
+
+static float encoder_get_angle_incr(encoder_t *encoder)
+{
+    uint16_t counts = encoder->enc_timer->Instance->CNT;
+
+    /* convert counts to radians */
+    return (((float) (counts % (PULSES_PER_REV + 1))) / PULSES_PER_REV) * 2 * M_PI;
+}
+
+static float encoder_calculate_velocity(encoder_t *encoder, float dt)
+{
+    /* Use timer in encoder mode to calculate velocity using previous count value */
+    uint32_t current_count = encoder->enc_timer->Instance->CNT;
+    int32_t count_delta = current_count - encoder->prev_cnt_value;
+
+    /* Update previous value with current */
+    encoder->prev_cnt_value = current_count;
+
+    return ((float) count_delta / PULSES_PER_REV) * 2 * M_PI / dt;
+}
+
+const osThreadAttr_t encoder_observer_attributes = {
+	.name = "Encoder Observer",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityHigh,
+};
+
+void vEncoderObserver(void *pv_params)
+{
+	osStatus_t status;
+    float ssi_position;
+
+    foc_data_t msg = {.type = FOCDATA_ROTOR_POSITION};
+
+	encoder_t *encoder = (encoder_t *)pv_params;
+	assert(encoder);
+
+	for (;;)
+	{
+        /* Retrieve SSI position */
+        encoder_get_angle_ssi(encoder, &ssi_position);
+
+        /* Fuse SSI position with incremental position */
+        msg.payload.rotor_position = (ssi_position + encoder_get_angle_incr(encoder)) / 2;
+
+        /* Enqueue reading */
+        foc_queue_frame(encoder->controller, &msg);
+
+        /* Wait */
+        osDelay(10);
+	}
+}

CM7/Core/Src/main.c

@@ -25,7 +25,7 @@
 
 #include "proteus_config.h"
 #include "gatedriver.h"
-#include "ssi_encoder.h"
+#include "encoder.h"
 #include "foc_ctrl.h"
 #include "ipcc.h"
 #include <stdio.h>
@@ -91,6 +91,9 @@ gatedriver_t gatedrv_right;
 foc_ctrl_t ctrl_right;
 foc_ctrl_t ctrl_left;
 
+encoder_t encoder_left;
+encoder_t encoder_right;
+
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -254,10 +257,10 @@ int main(void)
   foc_ctrl_init(&ctrl_right);
 
   gatedrv_init(&gatedrv_left, &htim1, &hadc1, &ctrl_left);
-  //ssi_encoder_t *ssi_encoder_left = ssi_encoder_init(&hspi2);
+  encoder_init(&encoder_left, &htim2, &hspi2, &ctrl_left);
 
   gatedrv_init(&gatedrv_right, &htim8, &hadc3, &ctrl_right);
-  //ssi_encoder_t *ssi_encoder_right = ssi_encoder_init(&hspi4);
+  encoder_init(&encoder_right, &htim4, &hspi4, &ctrl_right);
   printf("MC Initialized...\r\n");
   /* USER CODE END RTOS_MUTEX */
 
@@ -284,12 +287,18 @@ int main(void)
   ctrl_left.thread = osThreadNew(vFOCctrl, &ctrl_left, &foc_ctrl_attributes);
   assert(ctrl_left.thread);
 
+  encoder_left.thread = osThreadNew(vEncoderObserver, &encoder_left, &encoder_observer_attributes);
+  assert(encoder_left.thread);
+
   gatedrv_right.write_thread = osThreadNew(vPhaseActor, &gatedrv_right, &phase_actor_attributes);
   assert(gatedrv_right.write_thread);
 
   ctrl_right.thread = osThreadNew(vFOCctrl, &ctrl_right, &foc_ctrl_attributes);
   assert(ctrl_right.thread);
 
+  encoder_right.thread = osThreadNew(vEncoderObserver, &encoder_right, &encoder_observer_attributes);
+  assert(encoder_right.thread);
+
   printf("Tasks Created...\r\n");
   /* USER CODE END RTOS_THREADS */
 
@@ -977,10 +986,10 @@ static void MX_TIM2_Init(void)
   htim2.Instance = TIM2;
   htim2.Init.Prescaler = 0;
   htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim2.Init.Period = 4294967295;
+  htim2.Init.Period = 8191;
   htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
@@ -1026,10 +1035,10 @@ static void MX_TIM4_Init(void)
   htim4.Instance = TIM4;
   htim4.Init.Prescaler = 0;
   htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim4.Init.Period = 65535;
+  htim4.Init.Period = 8191;
   htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
@@ -1397,11 +1406,11 @@ 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 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);
+    //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;

Makefile/CM4/Makefile

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

Makefile/CM7/Makefile

@@ -1,5 +1,5 @@
 ##########################################################################################################################
-# File automatically-generated by tool: [projectgenerator] version: [4.2.0-B44] date: [Mon Jun 03 20:45:16 EDT 2024] 
+# File automatically-generated by tool: [projectgenerator] version: [4.2.0-B44] date: [Tue Jun 04 16:23:36 EDT 2024] 
 ##########################################################################################################################
 
 # ------------------------------------------------
@@ -42,7 +42,7 @@ C_SOURCES =  \
 ../../CM7/Core/Src/stm32h7xx_hal_msp.c \
 ../../CM7/Core/Src/gatedriver.c \
 ../../CM7/Core/Src/foc_ctrl.c \
-../../CM7/Core/Src/ssi_encoder.c \
+../../CM7/Core/Src/encoder.c \
 ../../CM7/Core/Src/ipcc.c \
 ../../CM7/Core/Src/state_machine.c \
 ../../CM7/Core/Src/us_timer.c \

proteus.ioc

@@ -622,7 +622,7 @@ ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false-CortexM7,2-MX_GPIO_Init-GPIO-false-HAL-true-CortexM7,3-MX_DMA_Init-DMA-false-HAL-true-CortexM7,4-MX_SPI2_Init-SPI2-false-HAL-true-CortexM7,5-MX_SPI3_Init-SPI3-false-HAL-true-CortexM7,6-MX_SPI4_Init-SPI4-false-HAL-true-CortexM7,7-MX_TIM1_Init-TIM1-false-HAL-true-CortexM7,8-MX_TIM2_Init-TIM2-false-HAL-true-CortexM7,9-MX_TIM4_Init-TIM4-false-HAL-true-CortexM7,10-MX_TIM8_Init-TIM8-false-HAL-true-CortexM7,11-MX_CRC_Init-CRC-false-HAL-true-CortexM7,12-MX_FREERTOS_Init-FREERTOS_M7-false-HAL-false-CortexM7,13-MX_ADC1_Init-ADC1-false-HAL-true-CortexM7,14-MX_ADC3_Init-ADC3-false-HAL-true-CortexM7,15-MX_SPI1_Init-SPI1-false-HAL-true-CortexM7,16-MX_UART4_Init-UART4-false-HAL-true-CortexM7,17-MX_FDCAN1_Init-FDCAN1-true-HAL-false-CortexM7,1-MX_GPIO_Init-GPIO-false-HAL-true-CortexM4,2-MX_DMA_Init-DMA-false-HAL-true-CortexM4,3-MX_FDCAN1_Init-FDCAN1-false-HAL-true-CortexM4,4-MX_QUADSPI_Init-QUADSPI-false-HAL-true-CortexM4,5-MX_CRC_Init-CRC-true-HAL-false-CortexM4,6-MX_FREERTOS_Init-FREERTOS_M4-false-HAL-false-CortexM4,7-MX_UART4_Init-UART4-true-HAL-false-CortexM4,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true-CortexM7,0-MX_CORTEX_M4_Init-CORTEX_M4-false-HAL-true-CortexM4
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false-CortexM7,2-MX_GPIO_Init-GPIO-false-HAL-true-CortexM7,3-MX_DMA_Init-DMA-false-HAL-true-CortexM7,4-MX_SPI2_Init-SPI2-false-HAL-true-CortexM7,5-MX_SPI3_Init-SPI3-false-HAL-true-CortexM7,6-MX_SPI4_Init-SPI4-false-HAL-true-CortexM7,7-MX_TIM1_Init-TIM1-false-HAL-true-CortexM7,8-MX_TIM2_Init-TIM2-false-HAL-true-CortexM7,9-MX_TIM4_Init-TIM4-false-HAL-true-CortexM7,10-MX_TIM8_Init-TIM8-false-HAL-true-CortexM7,11-MX_CRC_Init-CRC-false-HAL-true-CortexM7,12-MX_FREERTOS_Init-FREERTOS_M7-false-HAL-false-CortexM7,13-MX_ADC1_Init-ADC1-false-HAL-true-CortexM7,14-MX_ADC3_Init-ADC3-false-HAL-true-CortexM7,15-MX_SPI1_Init-SPI1-false-HAL-true-CortexM7,16-MX_UART4_Init-UART4-false-HAL-true-CortexM7,17-MX_FDCAN1_Init-FDCAN1-true-HAL-false-CortexM7,18-MX_TIM6_Init-TIM6-false-HAL-true-CortexM7,1-MX_GPIO_Init-GPIO-false-HAL-true-CortexM4,2-MX_DMA_Init-DMA-false-HAL-true-CortexM4,3-MX_FDCAN1_Init-FDCAN1-false-HAL-true-CortexM4,4-MX_QUADSPI_Init-QUADSPI-false-HAL-true-CortexM4,5-MX_CRC_Init-CRC-true-HAL-false-CortexM4,6-MX_FREERTOS_Init-FREERTOS_M4-false-HAL-false-CortexM4,7-MX_UART4_Init-UART4-true-HAL-false-CortexM4,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true-CortexM7,0-MX_CORTEX_M4_Init-CORTEX_M4-false-HAL-true-CortexM4
 RCC.ADCFreq_Value=75000000
 RCC.AHB12Freq_Value=64000000
 RCC.AHB4Freq_Value=64000000
@@ -790,6 +790,12 @@ TIM1.Pulse-PWM\ Generation3\ CH3\ CH3N=(PWM_PERIOD_CYCLES) / 4
 TIM1.Pulse-PWM\ Generation4\ No\ Output=(PWM_PERIOD_CYCLES)
 TIM1.RepetitionCounter=REP_COUNTER
 TIM1.TIM_MasterOutputTrigger=TIM_TRGO_OC4REF
+TIM2.EncoderMode=TIM_ENCODERMODE_TI12
+TIM2.IPParameters=Period,EncoderMode
+TIM2.Period=8191
+TIM4.EncoderMode=TIM_ENCODERMODE_TI12
+TIM4.IPParameters=Period,EncoderMode
+TIM4.Period=8191
 TIM6.IPParameters=Prescaler
 TIM6.Prescaler=480
 TIM8.Channel-PWM\ Generation1\ CH1\ CH1N=TIM_CHANNEL_1