From cdbd5c4bfdf1041562e87391128d8cd9a259a15a Mon Sep 17 00:00:00 2001
From: MQjehovah <1421706030@qq.com>
Date: Wed, 23 Aug 2023 11:05:48 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E5=AE=9A=E6=97=B6=E5=99=A8?=
=?UTF-8?q?=E9=A9=B1=E5=8A=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
project/mdk/stm32_template.uvprojx | 18 +-
src/Bsp/board.c | 2 +-
src/Bsp/gpio.c | 4 +-
src/Bsp/gpio.h | 2 +-
src/Bsp/stm32f4xx_hal_conf.h | 2 +-
src/Bsp/timer.c | 282 +++++++++++++++++++++++++++++
src/Bsp/timer.h | 30 +++
src/Foc/hall.c | 116 ++++++++++++
src/Foc/hall.h | 44 +++++
src/Foc/main.c | 11 ++
src/Foc/math.c | 110 +++++++++++
src/Foc/math.h | 53 ++++++
src/Foc/svpwm.c | 87 +++++++++
src/Foc/svpwm.h | 18 ++
src/Foc/type.h | 26 +++
15 files changed, 796 insertions(+), 9 deletions(-)
create mode 100644 src/Bsp/timer.c
create mode 100644 src/Bsp/timer.h
create mode 100644 src/Foc/hall.c
create mode 100644 src/Foc/hall.h
create mode 100644 src/Foc/main.c
create mode 100644 src/Foc/math.c
create mode 100644 src/Foc/math.h
create mode 100644 src/Foc/svpwm.c
create mode 100644 src/Foc/svpwm.h
create mode 100644 src/Foc/type.h
diff --git a/project/mdk/stm32_template.uvprojx b/project/mdk/stm32_template.uvprojx
index 398b2e7..70d0467 100644
--- a/project/mdk/stm32_template.uvprojx
+++ b/project/mdk/stm32_template.uvprojx
@@ -10,8 +10,8 @@
stm32f407_template
0x4
ARM-ADS
- 5060960::V5.06 update 7 (build 960)::.\ARMCC
- 0
+ 6150000::V6.15::ARMCLANG
+ 1
STM32F407ZGTx
@@ -313,7 +313,7 @@
1
- 4
+ 2
0
0
1
@@ -328,7 +328,7 @@
1
0
0
- 5
+ 3
3
1
1
@@ -478,6 +478,11 @@
1
..\..\src\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c
+
+ stm32f4xx_ll_tim.c
+ 1
+ ..\..\src\STM32F4xx_HAL_Driver\Src\stm32f4xx_ll_tim.c
+
@@ -513,6 +518,11 @@
1
..\..\src\Bsp\stm32f4xx_it.c
+
+ timer.c
+ 1
+ ..\..\src\Bsp\timer.c
+
diff --git a/src/Bsp/board.c b/src/Bsp/board.c
index 3ad70ee..567f7fd 100644
--- a/src/Bsp/board.c
+++ b/src/Bsp/board.c
@@ -39,7 +39,7 @@ void SystemClock_Config(void)
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
- RCC_OscInitStruct.PLL.PLLM = 8;
+ RCC_OscInitStruct.PLL.PLLM = 25;
RCC_OscInitStruct.PLL.PLLN = 336;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
diff --git a/src/Bsp/gpio.c b/src/Bsp/gpio.c
index eed7065..5de18b6 100644
--- a/src/Bsp/gpio.c
+++ b/src/Bsp/gpio.c
@@ -42,11 +42,11 @@ void led_ctrl(uint8_t cmd)
{
if (cmd)
{
- HAL_GPIO_WritePin(LED1_GPIO_Port, LED0_Pin, GPIO_PIN_SET);
+ HAL_GPIO_WritePin(LED0_GPIO_Port, LED0_Pin, GPIO_PIN_SET);
}
else
{
- HAL_GPIO_WritePin(LED1_GPIO_Port, LED0_Pin, GPIO_PIN_RESET);
+ HAL_GPIO_WritePin(LED0_GPIO_Port, LED0_Pin, GPIO_PIN_RESET);
}
}
diff --git a/src/Bsp/gpio.h b/src/Bsp/gpio.h
index 082d402..e7c8257 100644
--- a/src/Bsp/gpio.h
+++ b/src/Bsp/gpio.h
@@ -18,7 +18,7 @@ extern "C"
/* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "stm32f4xx_hal.h"
#define LED0_Pin GPIO_PIN_15
#define LED0_GPIO_Port GPIOA
diff --git a/src/Bsp/stm32f4xx_hal_conf.h b/src/Bsp/stm32f4xx_hal_conf.h
index 5acf6bc..45cb5d4 100644
--- a/src/Bsp/stm32f4xx_hal_conf.h
+++ b/src/Bsp/stm32f4xx_hal_conf.h
@@ -62,7 +62,7 @@
/* #define HAL_SD_MODULE_ENABLED */
/* #define HAL_MMC_MODULE_ENABLED */
/* #define HAL_SPI_MODULE_ENABLED */
-/* #define HAL_TIM_MODULE_ENABLED */
+#define HAL_TIM_MODULE_ENABLED
#define HAL_UART_MODULE_ENABLED
/* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */
diff --git a/src/Bsp/timer.c b/src/Bsp/timer.c
new file mode 100644
index 0000000..7efe8c4
--- /dev/null
+++ b/src/Bsp/timer.c
@@ -0,0 +1,282 @@
+/**
+ * @file timer.c
+ * @author jimingqing
+ * @date 2023-08-14
+ * @version 0.0.1
+ * @copyright XiazhiTech Copyright (c) 2023
+ *
+ * @brief
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "timer.h"
+#include "board.h"
+#include "stm32f4xx_ll_tim.h"
+
+TIM_HandleTypeDef htim1;
+TIM_HandleTypeDef htim3;
+
+/**
+ * @brief TIM1 Initialization Function
+ * @param None
+ * @retval None
+ */
+void MX_TIM1_Init(void)
+{
+ TIM_SlaveConfigTypeDef sSlaveConfig = {0};
+ TIM_MasterConfigTypeDef sMasterConfig = {0};
+ TIM_OC_InitTypeDef sConfigOC = {0};
+ TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
+
+ /**
+ * 定时器主频84M 不分频
+ */
+ htim1.Instance = TIM1;
+ htim1.Init.Prescaler = 0;
+ htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
+ htim1.Init.Period = (84 * 1000000 / 16000);
+ htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
+ htim1.Init.RepetitionCounter = 1;
+ htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+ if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sSlaveConfig.SlaveMode = TIM_SLAVEMODE_TRIGGER;
+ sSlaveConfig.InputTrigger = TIM_TS_ITR1;
+ if (HAL_TIM_SlaveConfigSynchro(&htim1, &sSlaveConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sConfigOC.OCMode = TIM_OCMODE_PWM1;
+ sConfigOC.Pulse = 0;
+ sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+ sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+ sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+ sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
+ sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sConfigOC.OCMode = TIM_OCMODE_PWM2;
+ sConfigOC.Pulse = ((84 * 1000000 / 16000) - 1);
+ if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_ENABLE;
+ sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_ENABLE;
+ sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_1;
+ // sBreakDeadTimeConfig.DeadTime = ((DEAD_TIME_COUNTS) / 2);
+ sBreakDeadTimeConfig.DeadTime = 100;
+ sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
+ sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_LOW;
+ sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
+ if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+}
+
+/**
+ * @brief TIM3 Initialization Function
+ * @param None
+ * @retval None
+ */
+void MX_TIM3_Init(void)
+{
+
+ TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+ TIM_HallSensor_InitTypeDef sConfig = {0};
+ TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+ htim3.Instance = TIM3;
+ htim3.Init.Prescaler = 0;
+ htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
+ htim3.Init.Period = 65535;
+ htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+ if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
+ sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
+ sConfig.IC1Filter = 11;
+ sConfig.Commutation_Delay = 0;
+ if (HAL_TIMEx_HallSensor_Init(&htim3, &sConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC2REF;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
+ {
+ Error_Handler();
+ }
+}
+
+/**
+ * @brief TIM_Base MSP Initialization
+ * This function configures the hardware resources used in this example
+ * @param htim_base: TIM_Base handle pointer
+ * @retval None
+ */
+void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim_base)
+{
+ GPIO_InitTypeDef GPIO_InitStruct = {0};
+ if (htim_base->Instance == TIM1)
+ {
+ /* Peripheral clock enable */
+ __HAL_RCC_TIM1_CLK_ENABLE();
+ __HAL_RCC_GPIOA_CLK_ENABLE();
+ __HAL_RCC_GPIOB_CLK_ENABLE();
+
+ /**TIM1 GPIO Configuration
+ PA10 ------> TIM1_CH3
+ PA9 ------> TIM1_CH2
+ PA8 ------> TIM1_CH1
+ PB13 ------> TIM1_CH1N
+ PB14 ------> TIM1_CH2N
+ PB15 ------> TIM1_CH3N
+ */
+ GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+ GPIO_InitStruct.Pin = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
+ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+ }
+ else if (htim_base->Instance == TIM3)
+ {
+ /* Peripheral clock enable */
+ __HAL_RCC_TIM3_CLK_ENABLE();
+ __HAL_RCC_GPIOC_CLK_ENABLE();
+ /**TIM3 GPIO Configuration
+ PC8 ------> TIM3_CH3
+ PC7 ------> TIM3_CH2
+ PC6 ------> TIM3_CH1
+ */
+ GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
+ HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+ }
+}
+
+/**
+ * @brief TIM_Base MSP De-Initialization
+ * This function freeze the hardware resources used in this example
+ * @param htim_base: TIM_Base handle pointer
+ * @retval None
+ */
+void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef *htim_base)
+{
+ if (htim_base->Instance == TIM1)
+ {
+ /* Peripheral clock disable */
+ __HAL_RCC_TIM1_CLK_DISABLE();
+
+ /* TIM1 interrupt DeInit */
+ HAL_NVIC_DisableIRQ(TIM1_BRK_TIM9_IRQn);
+ HAL_NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);
+ }
+ else if (htim_base->Instance == TIM3)
+ {
+
+ /* Peripheral clock disable */
+ __HAL_RCC_TIM3_CLK_DISABLE();
+
+ /**TIM3 GPIO Configuration
+ PC8 ------> TIM3_CH3
+ PC7 ------> TIM3_CH2
+ PC6 ------> TIM3_CH1
+ */
+ HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8);
+
+ /* TIM3 interrupt DeInit */
+ HAL_NVIC_DisableIRQ(TIM3_IRQn);
+ }
+}
+
+/**
+ * @brief This function handles first motor TIMx Update interrupt request.
+ * @param None
+ * @retval None
+ */
+void TIM1_UP_TIM10_IRQHandler(void)
+{
+ // LL_TIM_ClearFlag_UPDATE(PWM_Handle_M1.pParams_str->TIMx);
+ // R3_2_TIMx_UP_IRQHandler(&PWM_Handle_M1);
+}
+
+/**
+ * @brief This function handles first motor BRK interrupt.
+ * @param None
+ * @retval None
+ */
+void TIM1_BRK_TIM9_IRQHandler(void)
+{
+ // if (LL_TIM_IsActiveFlag_BRK(PWM_Handle_M1.pParams_str->TIMx))
+ // {
+ // LL_TIM_ClearFlag_BRK(PWM_Handle_M1.pParams_str->TIMx);
+ // R3_2_BRK_IRQHandler(&PWM_Handle_M1);
+ // }
+ // /* Systick is not executed due low priority so is necessary to call MC_Scheduler here.*/
+ // MC_Scheduler();
+}
+
+/**
+ * @brief This function handles TIMx global interrupt request for M1 Speed Sensor.
+ * @param None
+ * @retval None
+ */
+void TIM3_IRQHandler(void)
+{
+ /* HALL Timer Update IT always enabled, no need to check enable UPDATE state */
+ if (LL_TIM_IsActiveFlag_UPDATE(TIM3))
+ {
+ LL_TIM_ClearFlag_UPDATE(TIM3);
+ // HALL_TIMx_UP_IRQHandler(&HALL_M1);
+ }
+
+ /* HALL Timer CC1 IT always enabled, no need to check enable CC1 state */
+ if (LL_TIM_IsActiveFlag_CC1(TIM3))
+ {
+ LL_TIM_ClearFlag_CC1(TIM3);
+ // HALL_TIMx_CC_IRQHandler(&HALL_M1);
+ }
+}
diff --git a/src/Bsp/timer.h b/src/Bsp/timer.h
new file mode 100644
index 0000000..95bdac1
--- /dev/null
+++ b/src/Bsp/timer.h
@@ -0,0 +1,30 @@
+/**
+ * @file gpio.h
+ * @author jimingqing
+ * @date 2023-08-14
+ * @version 0.0.1
+ * @copyright XiazhiTech Copyright (c) 2023
+ *
+ * @brief
+ */
+
+#ifndef __TIMER_H__
+#define __TIMER_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+ /* Includes ------------------------------------------------------------------*/
+
+#include "stm32f4xx_hal.h"
+
+ void MX_TIM1_Init(void);
+
+#ifdef __cplusplus
+}
+#endif
+#endif /*__ __TIMER_H__ */
+
+/* END OF FILE ---------------------------------------------------------------*/
diff --git a/src/Foc/hall.c b/src/Foc/hall.c
new file mode 100644
index 0000000..435c8f8
--- /dev/null
+++ b/src/Foc/hall.c
@@ -0,0 +1,116 @@
+#include "hall.h"
+
+/**
+ * @brief 更新HALL状态
+ * @param handle
+ * @param new_state
+ */
+void calc_hall_angle(hall_handle_t *handle, uint8_t new_state)
+{
+ switch (new_state)
+ {
+ case HALL_STATE_5:
+ if (handle->state == HALL_STATE_4)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = handle->PhaseShift;
+ }
+ else if (handle->state == HALL_STATE_1)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift + S16_60_PHASE_SHIFT);
+ }
+ else
+ {
+ }
+ break;
+
+ case HALL_STATE_1:
+ if (handle->state == HALL_STATE_5)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = handle->PhaseShift + S16_60_PHASE_SHIFT;
+ }
+ else if (handle->state == HALL_STATE_3)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift + S16_120_PHASE_SHIFT);
+ }
+ else
+ {
+ }
+ break;
+
+ case HALL_STATE_3:
+ if (handle->state == HALL_STATE_1)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift + S16_120_PHASE_SHIFT);
+ }
+ else if (handle->state == HALL_STATE_2)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift + S16_120_PHASE_SHIFT +
+ S16_60_PHASE_SHIFT);
+ }
+ else
+ {
+ }
+
+ break;
+
+ case HALL_STATE_2:
+ if (handle->state == HALL_STATE_3)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift + S16_120_PHASE_SHIFT + S16_60_PHASE_SHIFT);
+ }
+ else if (handle->state == HALL_STATE_6)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift - S16_120_PHASE_SHIFT);
+ }
+ else
+ {
+ }
+ break;
+
+ case HALL_STATE_6:
+ if (handle->state == HALL_STATE_2)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift - S16_120_PHASE_SHIFT);
+ }
+ else if (handle->state == HALL_STATE_4)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift - S16_60_PHASE_SHIFT);
+ }
+ else
+ {
+ }
+ break;
+
+ case HALL_STATE_4:
+ if (handle->state == HALL_STATE_6)
+ {
+ handle->Direction = POSITIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift - S16_60_PHASE_SHIFT);
+ }
+ else if (handle->state == HALL_STATE_5)
+ {
+ handle->Direction = NEGATIVE;
+ handle->MeasuredElAngle = (int16_t)(handle->PhaseShift);
+ }
+ else
+ {
+ }
+ break;
+
+ default:
+ /* Bad hall sensor configutarion so update the speed reliability */
+ handle->SensorIsReliable = false;
+
+ break;
+ }
+}
\ No newline at end of file
diff --git a/src/Foc/hall.h b/src/Foc/hall.h
new file mode 100644
index 0000000..26c04c1
--- /dev/null
+++ b/src/Foc/hall.h
@@ -0,0 +1,44 @@
+/**
+ * @file hall.h
+ * @author jimingqing
+ * @date 2023-05-25
+ * @version 0.0.1
+ * @copyright XiazhiTech Copyright (c) 2023
+ *
+ * @brief
+ */
+#include "stm32f407xx.h"
+
+#define HALL_STATE_0 (uint8_t)0
+#define HALL_STATE_1 (uint8_t)1
+#define HALL_STATE_2 (uint8_t)2
+#define HALL_STATE_3 (uint8_t)3
+#define HALL_STATE_4 (uint8_t)4
+#define HALL_STATE_5 (uint8_t)5
+#define HALL_STATE_6 (uint8_t)6
+#define HALL_STATE_7 (uint8_t)7
+
+#define S16_120_PHASE_SHIFT (int16_t)(65536 / 3)
+#define S16_60_PHASE_SHIFT (int16_t)(65536 / 6)
+
+enum
+{
+ NEGATIVE = -1,
+ POSITIVE = 1,
+
+}
+
+typedef struct
+{
+ // |||
+ // |||
+ // |||
+ uint8_t state; // 霍尔状态真值表
+
+ int16_t phase_shift; // 同步电角度
+
+ int8_t direction; // 转子方向
+ int16_t measure_elec_angle; // 测量转子电角度
+ int16_t control_elec_angle; // 控制转子电角度
+
+} hall_handle_t;
diff --git a/src/Foc/main.c b/src/Foc/main.c
new file mode 100644
index 0000000..72e23b1
--- /dev/null
+++ b/src/Foc/main.c
@@ -0,0 +1,11 @@
+
+
+int main()
+{
+ /**
+ * 开环控制
+ * 1. 计算电角度
+ * 2.
+ */
+
+}
diff --git a/src/Foc/math.c b/src/Foc/math.c
new file mode 100644
index 0000000..943042b
--- /dev/null
+++ b/src/Foc/math.c
@@ -0,0 +1,110 @@
+#include "math.h"
+
+#define SIN_MASK 0x0300u
+#define U0_90 0x0200u
+#define U90_180 0x0300u
+#define U180_270 0x0000u
+#define U270_360 0x0100u
+
+const int16_t hSin_Cos_Table[256] = {
+ 0x0000, 0x00C9, 0x0192, 0x025B, 0x0324, 0x03ED, 0x04B6, 0x057F,
+ 0x0648, 0x0711, 0x07D9, 0x08A2, 0x096A, 0x0A33, 0x0AFB, 0x0BC4,
+ 0x0C8C, 0x0D54, 0x0E1C, 0x0EE3, 0x0FAB, 0x1072, 0x113A, 0x1201,
+ 0x12C8, 0x138F, 0x1455, 0x151C, 0x15E2, 0x16A8, 0x176E, 0x1833,
+ 0x18F9, 0x19BE, 0x1A82, 0x1B47, 0x1C0B, 0x1CCF, 0x1D93, 0x1E57,
+ 0x1F1A, 0x1FDD, 0x209F, 0x2161, 0x2223, 0x22E5, 0x23A6, 0x2467,
+ 0x2528, 0x25E8, 0x26A8, 0x2767, 0x2826, 0x28E5, 0x29A3, 0x2A61,
+ 0x2B1F, 0x2BDC, 0x2C99, 0x2D55, 0x2E11, 0x2ECC, 0x2F87, 0x3041,
+ 0x30FB, 0x31B5, 0x326E, 0x3326, 0x33DF, 0x3496, 0x354D, 0x3604,
+ 0x36BA, 0x376F, 0x3824, 0x38D9, 0x398C, 0x3A40, 0x3AF2, 0x3BA5,
+ 0x3C56, 0x3D07, 0x3DB8, 0x3E68, 0x3F17, 0x3FC5, 0x4073, 0x4121,
+ 0x41CE, 0x427A, 0x4325, 0x43D0, 0x447A, 0x4524, 0x45CD, 0x4675,
+ 0x471C, 0x47C3, 0x4869, 0x490F, 0x49B4, 0x4A58, 0x4AFB, 0x4B9D,
+ 0x4C3F, 0x4CE0, 0x4D81, 0x4E20, 0x4EBF, 0x4F5D, 0x4FFB, 0x5097,
+ 0x5133, 0x51CE, 0x5268, 0x5302, 0x539B, 0x5432, 0x54C9, 0x5560,
+ 0x55F5, 0x568A, 0x571D, 0x57B0, 0x5842, 0x58D3, 0x5964, 0x59F3,
+ 0x5A82, 0x5B0F, 0x5B9C, 0x5C28, 0x5CB3, 0x5D3E, 0x5DC7, 0x5E4F,
+ 0x5ED7, 0x5F5D, 0x5FE3, 0x6068, 0x60EB, 0x616E, 0x61F0, 0x6271,
+ 0x62F1, 0x6370, 0x63EE, 0x646C, 0x64E8, 0x6563, 0x65DD, 0x6656,
+ 0x66CF, 0x6746, 0x67BC, 0x6832, 0x68A6, 0x6919, 0x698B, 0x69FD,
+ 0x6A6D, 0x6ADC, 0x6B4A, 0x6BB7, 0x6C23, 0x6C8E, 0x6CF8, 0x6D61,
+ 0x6DC9, 0x6E30, 0x6E96, 0x6EFB, 0x6F5E, 0x6FC1, 0x7022, 0x7083,
+ 0x70E2, 0x7140, 0x719D, 0x71F9, 0x7254, 0x72AE, 0x7307, 0x735E,
+ 0x73B5, 0x740A, 0x745F, 0x74B2, 0x7504, 0x7555, 0x75A5, 0x75F3,
+ 0x7641, 0x768D, 0x76D8, 0x7722, 0x776B, 0x77B3, 0x77FA, 0x783F,
+ 0x7884, 0x78C7, 0x7909, 0x794A, 0x7989, 0x79C8, 0x7A05, 0x7A41,
+ 0x7A7C, 0x7AB6, 0x7AEE, 0x7B26, 0x7B5C, 0x7B91, 0x7BC5, 0x7BF8,
+ 0x7C29, 0x7C59, 0x7C88, 0x7CB6, 0x7CE3, 0x7D0E, 0x7D39, 0x7D62,
+ 0x7D89, 0x7DB0, 0x7DD5, 0x7DFA, 0x7E1D, 0x7E3E, 0x7E5F, 0x7E7E,
+ 0x7E9C, 0x7EB9, 0x7ED5, 0x7EEF, 0x7F09, 0x7F21, 0x7F37, 0x7F4D,
+ 0x7F61, 0x7F74, 0x7F86, 0x7F97, 0x7FA6, 0x7FB4, 0x7FC1, 0x7FCD,
+ 0x7FD8, 0x7FE1, 0x7FE9, 0x7FF0, 0x7FF5, 0x7FF9, 0x7FFD, 0x7FFE};
+
+/**
+ * @brief This function transforms stator voltage qVq and qVd, that belong to
+ * a rotor flux synchronous rotating frame, to a stationary reference
+ * frame, so as to obtain qValpha and qVbeta:
+ * Valfa= Vq*Cos(theta)+ Vd*Sin(theta)
+ * Vbeta=-Vq*Sin(theta)+ Vd*Cos(theta)
+ * @param Input: stator voltage Vq and Vd in qd_t format
+ * @param Theta: rotating frame angular position in q1.15 format
+ * @retval Stator voltage Valpha and Vbeta in qd_t format
+ */
+alphabeta_t math_rev_park(qd_t input, int16_t theta)
+{
+ alphabeta_t output;
+ int32_t alpha_tmp1, alpha_tmp2, beta_tmp1, beta_tmp2;
+
+ int32_t shindex;
+ uint16_t uhindex;
+ int16_t hSin = 0, hCos = 0;
+
+ /* 10 bit index computation */
+ shindex = ((int32_t)32768 + (int32_t)theta);
+ uhindex = (uint16_t)shindex;
+ uhindex = uhindex >> 6;
+ // uhindex /= (uint16_t)64;
+
+ /**
+ | hAngle | angle | std |
+ | (0,16384] | U0_90 | (0,0.5] |
+ | (16384,32767] | U90_180 | (0.5,0.99] |
+ | (-16384,-1] | U270_360 | (0,-0.5] |
+ | (-16384,-32768] | U180_270 | (-0.5,-1) |
+ */
+ switch ((uint16_t)(uhindex)&SIN_MASK)
+ {
+ case U0_90:
+ hSin = hSin_Cos_Table[(uint8_t)(uhindex)];
+ hCos = hSin_Cos_Table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
+ break;
+
+ case U90_180:
+ hSin = hSin_Cos_Table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
+ hCos = -hSin_Cos_Table[(uint8_t)(uhindex)];
+ break;
+
+ case U180_270:
+ hSin = -hSin_Cos_Table[(uint8_t)(uhindex)];
+ hCos = -hSin_Cos_Table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
+ break;
+
+ case U270_360:
+ hSin = -hSin_Cos_Table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
+ hCos = hSin_Cos_Table[(uint8_t)(uhindex)];
+ break;
+ default:
+ break;
+ }
+
+ /*No overflow guaranteed*/
+ alpha_tmp1 = input.q * (int32_t)hCos;
+ alpha_tmp2 = input.d * (int32_t)hSin;
+ output.alpha = (int16_t)(((alpha_tmp1) + (alpha_tmp2)) >> 15);
+
+ beta_tmp1 = input.q * (int32_t)hSin;
+ beta_tmp2 = input.d * (int32_t)hCos;
+ output.beta = (int16_t)((beta_tmp2 - beta_tmp1) >> 15);
+
+ return (output);
+}
\ No newline at end of file
diff --git a/src/Foc/math.h b/src/Foc/math.h
new file mode 100644
index 0000000..0a494d5
--- /dev/null
+++ b/src/Foc/math.h
@@ -0,0 +1,53 @@
+#include "type.h"
+
+#define SQRT_2 1.4142
+#define SQRT_3 1.732
+
+/**
+ * @brief Macro to compute logarithm of two
+ */
+#define LOG2(x) \
+ ((x) == 65535 ? 16 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 15 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 14 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 13 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 12 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 11 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 10 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 9 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 8 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 * 2 ? 7 : ((x) == 2 * 2 * 2 * 2 * 2 * 2 ? 6 : ((x) == 2 * 2 * 2 * 2 * 2 ? 5 : ((x) == 2 * 2 * 2 * 2 ? 4 : ((x) == 2 * 2 * 2 ? 3 : ((x) == 2 * 2 ? 2 : ((x) == 2 ? 1 : ((x) == 1 ? 0 : -1)))))))))))))))))
+
+/**
+ * @brief This function transforms stator currents Ia and qIb (which are
+ * directed along axes each displaced by 120 degrees) into currents
+ * Ialpha and Ibeta in a stationary qd reference frame.
+ * Ialpha = Ia
+ * Ibeta = -(2*Ib+Ia)/sqrt(3)
+ * @param Curr_Input: stator current Ia and Ib in ab_t format
+ * @retval Stator current Ialpha and Ibeta in alphabeta_t format
+ */
+alphabeta_t math_clarke(ab_t Input);
+
+/**
+ * @brief This function transforms stator values alpha and beta, which
+ * belong to a stationary qd reference frame, to a rotor flux
+ * synchronous reference frame (properly oriented), so as Iq and Id.
+ * Id= Ialpha *sin(theta)+qIbeta *cos(Theta)
+ * Iq=qIalpha *cos(Theta)-qIbeta *sin(Theta)
+ * @param Curr_Input: stator values alpha and beta in alphabeta_t format
+ * @param Theta: rotating frame angular position in q1.15 format
+ * @retval Stator current q and d in qd_t format
+ */
+qd_t math_park(alphabeta_t Input, int16_t Theta);
+
+/**
+ * @brief This function transforms stator voltage qVq and qVd, that belong to
+ * a rotor flux synchronous rotating frame, to a stationary reference
+ * frame, so as to obtain qValpha and qVbeta:
+ * Valfa= Vq*Cos(theta)+ Vd*Sin(theta)
+ * Vbeta=-Vq*Sin(theta)+ Vd*Cos(theta)
+ * @param Curr_Input: stator voltage Vq and Vd in qd_t format
+ * @param Theta: rotating frame angular position in q1.15 format
+ * @retval Stator values alpha and beta in alphabeta_t format
+ */
+alphabeta_t math_rev_park(qd_t Input, int16_t Theta);
+
+/**
+ * @brief It calculates the square root of a non-negative s32. It returns 0
+ * for negative s32.
+ * @param Input int32_t number
+ * @retval int32_t Square root of Input (0 if Input<0)
+ */
+int32_t math_sqrt(int32_t wInput);
diff --git a/src/Foc/svpwm.c b/src/Foc/svpwm.c
new file mode 100644
index 0000000..42dc1b4
--- /dev/null
+++ b/src/Foc/svpwm.c
@@ -0,0 +1,87 @@
+#include "svpwm.h"
+void svpwm(int16_t alpha, int16_t beta)
+{
+ int32_t wX, wY, wZ, wUAlpha, wUBeta, wTimePhA, wTimePhB, wTimePhC;
+
+ wUAlpha = alpha * (int32_t)pHandle->hT_Sqrt3;
+ wUBeta = -(beta * (int32_t)(pHandle->PWMperiod)) * 2;
+
+ wX = wUBeta;
+ wY = (wUBeta + wUAlpha) / 2;
+ wZ = (wUBeta - wUAlpha) / 2;
+
+ /* Sector calculation from wX, wY, wZ */
+ if (wY < 0)
+ {
+ if (wZ < 0)
+ {
+ pHandle->Sector = SECTOR_5;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wY - wZ) / (int32_t)262144);
+ wTimePhB = wTimePhA + wZ / 131072;
+ wTimePhC = wTimePhA - wY / 131072;
+ pHandle->lowDuty = wTimePhC;
+ pHandle->midDuty = wTimePhA;
+ pHandle->highDuty = wTimePhB;
+ }
+ else /* wZ >= 0 */
+ if (wX <= 0)
+ {
+ pHandle->Sector = SECTOR_4;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wX - wZ) / (int32_t)262144);
+ wTimePhB = wTimePhA + wZ / 131072;
+ wTimePhC = wTimePhB - wX / 131072;
+ pHandle->lowDuty = wTimePhC;
+ pHandle->midDuty = wTimePhB;
+ pHandle->highDuty = wTimePhA;
+ }
+ else /* wX > 0 */
+ {
+ pHandle->Sector = SECTOR_3;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wY - wX) / (int32_t)262144);
+ wTimePhC = wTimePhA - wY / 131072;
+ wTimePhB = wTimePhC + wX / 131072;
+ pHandle->lowDuty = wTimePhB;
+ pHandle->midDuty = wTimePhC;
+ pHandle->highDuty = wTimePhA;
+ }
+ }
+ else /* wY > 0 */
+ {
+ if (wZ >= 0)
+ {
+ pHandle->Sector = SECTOR_2;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wY - wZ) / (int32_t)262144);
+ wTimePhB = wTimePhA + wZ / 131072;
+ wTimePhC = wTimePhA - wY / 131072;
+ pHandle->lowDuty = wTimePhB;
+ pHandle->midDuty = wTimePhA;
+ pHandle->highDuty = wTimePhC;
+ }
+ else /* wZ < 0 */
+ if (wX <= 0)
+ {
+ pHandle->Sector = SECTOR_6;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wY - wX) / (int32_t)262144);
+ wTimePhC = wTimePhA - wY / 131072;
+ wTimePhB = wTimePhC + wX / 131072;
+ pHandle->lowDuty = wTimePhA;
+ pHandle->midDuty = wTimePhC;
+ pHandle->highDuty = wTimePhB;
+ }
+ else /* wX > 0 */
+ {
+ pHandle->Sector = SECTOR_1;
+ wTimePhA = (int32_t)(pHandle->PWMperiod) / 4 + ((wX - wZ) / (int32_t)262144);
+ wTimePhB = wTimePhA + wZ / 131072;
+ wTimePhC = wTimePhB - wX / 131072;
+ pHandle->lowDuty = wTimePhA;
+ pHandle->midDuty = wTimePhB;
+ pHandle->highDuty = wTimePhC;
+ }
+ }
+
+ pHandle->CntPhA = (uint16_t)wTimePhA;
+ pHandle->CntPhB = (uint16_t)wTimePhB;
+ pHandle->CntPhC = (uint16_t)wTimePhC;
+
+}
\ No newline at end of file
diff --git a/src/Foc/svpwm.h b/src/Foc/svpwm.h
new file mode 100644
index 0000000..13dd56c
--- /dev/null
+++ b/src/Foc/svpwm.h
@@ -0,0 +1,18 @@
+/**
+ * @file svpwm.h
+ * @author jimingqing
+ * @date 2023-05-30
+ * @version 0.0.1
+ * @copyright XiazhiTech Copyright (c) 2023
+ *
+ * @brief
+ */
+
+#include "stm32f407xx.h"
+
+#define SECTOR_1 0u
+#define SECTOR_2 1u
+#define SECTOR_3 2u
+#define SECTOR_4 3u
+#define SECTOR_5 4u
+#define SECTOR_6 5u
\ No newline at end of file
diff --git a/src/Foc/type.h b/src/Foc/type.h
new file mode 100644
index 0000000..e3057d3
--- /dev/null
+++ b/src/Foc/type.h
@@ -0,0 +1,26 @@
+#include "stm32f407xx.h"
+
+/**
+ * @brief Two components q, d type definition
+ */
+typedef struct
+{
+ int16_t q;
+ int16_t d;
+} qd_t;
+/**
+ * @brief Two components a,b type definition
+ */
+typedef struct
+{
+ int16_t a;
+ int16_t b;
+} ab_t;
+/**
+ * @brief Two components alpha, beta type definition
+ */
+typedef struct
+{
+ int16_t alpha;
+ int16_t beta;
+} alphabeta_t;
\ No newline at end of file