diff --git a/CM7/Core/Inc/encoder.h b/CM7/Core/Inc/encoder.h new file mode 100644 index 0000000..9290547 --- /dev/null +++ b/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 + +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 */ diff --git a/CM7/Core/Inc/foc_ctrl.h b/CM7/Core/Inc/foc_ctrl.h index bca8fa3..b46497f 100644 --- a/CM7/Core/Inc/foc_ctrl.h +++ b/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]); diff --git a/CM7/Core/Inc/ssi_encoder.h b/CM7/Core/Inc/ssi_encoder.h deleted file mode 100644 index dc82682..0000000 --- a/CM7/Core/Inc/ssi_encoder.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef SSI_ENCODER_H -#define SSI_ENCODER_H - -#include "stm32h7xx.h" -#include "stm32h7xx_hal_spi.h" -#include - -typedef struct ssi_encoder { - SPI_HandleTypeDef *hspi; -} ssi_encoder_t; - -/** - * @brief Creates new ssi encoder operating on given spi peripheral - * - * @param spi_handle - * @return ssi_encoder_t* pointer to new ssi encoder object - */ -ssi_encoder_t *ssi_encoder_init(SPI_HandleTypeDef *spi_handle); - -/** - * @brief Reads absolute angle from encoder and converts it to radians - * NOTE: The datasheet for the encoder states that the minimum delay - * between reads must be between 12.5 us and 20.5 us - * - * @param encoder - * @param angle pointer to angle in radians - * @return int16_t nonzero if the read fails, 0 on success - */ -int16_t ssi_encoder_get_angle(ssi_encoder_t *encoder, float *angle); - -#endif /* SSI_ENCODER_H */ \ No newline at end of file diff --git a/CM7/Core/Src/encoder.c b/CM7/Core/Src/encoder.c new file mode 100644 index 0000000..b6967b0 --- /dev/null +++ b/CM7/Core/Src/encoder.c @@ -0,0 +1,99 @@ +#include "encoder.h" + +#include +#include +#include +#include + +#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); + } +} diff --git a/CM7/Core/Src/main.c b/CM7/Core/Src/main.c index 2fd1270..d2061f5 100644 --- a/CM7/Core/Src/main.c +++ b/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 @@ -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; diff --git a/CM7/Core/Src/ssi_encoder.c b/CM7/Core/Src/ssi_encoder.c deleted file mode 100644 index 39420ca..0000000 --- a/CM7/Core/Src/ssi_encoder.c +++ /dev/null @@ -1,37 +0,0 @@ -#include "ssi_encoder.h" - -#include -#include -#include - -#include "stm32h7xx.h" -#include "stm32h7xx_hal_spi.h" - -#define ENCODER_MAX_COUNT (1 << 13) - -ssi_encoder_t *ssi_encoder_init(SPI_HandleTypeDef *spi_handle) -{ - // make sure the handle exists - assert(spi_handle); - - // allocate new struct - ssi_encoder_t *encoder = malloc(sizeof(ssi_encoder_t)); - assert(encoder); - - encoder->hspi = spi_handle; - - return encoder; -} - -int16_t ssi_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; -} \ No newline at end of file diff --git a/Makefile/CM4/Makefile b/Makefile/CM4/Makefile index 633ab5a..1b1dc82 100644 --- a/Makefile/CM4/Makefile +++ b/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] ########################################################################################################################## # ------------------------------------------------ diff --git a/Makefile/CM7/Makefile b/Makefile/CM7/Makefile index b41a9e9..b1fcd92 100644 --- a/Makefile/CM7/Makefile +++ b/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 \ diff --git a/proteus.ioc b/proteus.ioc index 82b3440..2aeccfe 100644 --- a/proteus.ioc +++ b/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