foc 框架

Signed-off-by: huhui <huhui@sharkgulf.com>

Application/FOC/PI_Controller.c

@@ -0,0 +1,6 @@
+#include "pi_controller.h"
+
+float pi_control(PI_ctrl_t *pi, float error){
+	return 0.0f;
+}
+

Application/FOC/PI_Controller.h

@@ -0,0 +1,13 @@
+#ifndef _PI_CONTROLLER_H__
+#define _PI_CONTROLLER_H__
+typedef struct _pi {
+	float Kp_gain;
+	float Ki_gain;
+	float limit_high_output;
+	float limit_low_output;
+}PI_ctrl_t;
+
+float pi_control(PI_ctrl_t *pi, float error);
+
+#endif  /* _PI_CONTROLLER_H__ */
+

Application/FOC/foc.c

@@ -1,16 +1,17 @@
 #include <string.h>
+#include "libs/task.h"
 #include "hal/pwm.h"
 #include "hal/hal.h"
 #include "foc/foc.h"
 #include "foc/park_clark.h"
 #include "foc/svpwm.h"
-#include "libs/task.h"
+#include "foc/foc_task.h"
 
 static void charge_cap_timer_handler(timer_t *t);
 static u32 foc_main_task_handler(void);
 
 static timer_t charge_cap_timer = {.handler = charge_cap_timer_handler};
-motor_foc_t m_foc;
+static motor_foc_t m_foc;
 u16 _hall_table[] = {0xFFFF, 292, 47, 1, 180, 227, 113, 0xFFFF};
 
 void foc_init(void) {
@@ -32,7 +33,7 @@ void foc_motor_spin(dq_t *dq_v, float angle) {
 	u8 sector = 0xFF;
 	Rev_Park(dq_v, degree_2_pi(angle), &alphabeta);
 	svpwm(&alphabeta, MAX_VBUS, FOC_PWM_period/2, &phase_time, &sector);
-	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C);
+	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, FOC_PWM_period/2);
 }
 
 int foc_hall_detect(float current, u16 *hall_table){
@@ -132,7 +133,7 @@ void hall_detect_handler(void) {
 }
 
 void current_sample_handler(void) {
-
+	foc_task(&m_foc);
 }
 
 void foc_start_pwm(bool start) {

Application/FOC/foc.h

@@ -2,10 +2,7 @@
 #define _FOC_H__
 #include "foc/foc_type.h"
 
-#define TIM_CLOCK (72000000L)
-#define FOC_FS (20 * 1000)
-#define FOC_PWM_period (TIM_CLOCK/FOC_FS)
-#define MAX_VBUS (12.f) //12v
+
 void foc_init(void);
 void set_dq_voltage(float d_v, float q_v);
 void foc_motor_spin(dq_t *dq_v, float angle);

Application/FOC/foc_task.c

@@ -0,0 +1,32 @@
+#include "hal/hal.h"
+#include "hal/pwm.h"
+#include "foc_task.h"
+#include "phase_current.h"
+#include "park_clark.h"
+#include "svpwm.h"
+
+void foc_task(motor_foc_t *foc){
+	current_samp_t *c_sample = &foc->current_samp;
+	alpha_beta_t sample_ab, pwm_ab;
+	dq_t         sample_dq, v_dq;
+	phase_time_t         phase_time;
+	get_phase_current(c_sample);
+	Clark(c_sample->ia, c_sample->ib, c_sample->ic, &sample_ab);
+	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
+
+	if (foc->mode == FOC_MODE_I_DQ || foc->mode == FOC_MODE_FULL_PI) {
+		v_dq.d = pi_control(&foc->PI_id, foc->dq_ref.d - sample_dq.d);
+		v_dq.q = pi_control(&foc->PI_iq, foc->dq_ref.q - sample_dq.q);
+	}else {
+		v_dq.d = foc->dq_ref.d;
+		v_dq.q = foc->dq_ref.q;
+	}
+	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
+
+	svpwm(&pwm_ab, foc->vbus, FOC_PWM_period/2, &phase_time, &foc->sector);
+	
+	u32 sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
+	
+	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
+}
+

Application/FOC/foc_task.h

@@ -0,0 +1,6 @@
+#ifndef _FOC_TASK_H__
+#define _FOC_TASK_H__
+#include "foc_type.h"
+void foc_task(motor_foc_t *foc);
+#endif /* _FOC_TASK_H__ */
+

Application/FOC/foc_type.h

@@ -2,6 +2,7 @@
 #define _FOC_TYPE_H__
 #include "libs/types.h"
 #include "math/fast_math.h"
+#include "pi_controller.h"
 
 typedef struct _alphabeta {
 	float alpha;
@@ -21,6 +22,11 @@ typedef struct _motor_p {
 	int  flux_linkage; //磁链
 }motor_param_t;
 
+typedef struct _motor_s {
+	float theta;//转子电角度
+	float rpm; //转速
+}motor_stat_t;
+
 typedef struct _phase_time {
 	u32 A;
 	u32 B;
@@ -34,23 +40,53 @@ typedef enum {
 	READY_TO_RUN = 3
 }foc_state_t;
 
+typedef enum {
+	FOC_MODE_OPEN_LOOP, //开环
+	FOC_MODE_I_DQ,      //只电流环
+	FOC_MODE_SPEED,     //只速度环
+	FOC_MODE_FULL_PI         //速度，电流环都有
+}foc_mode_t;
+
+typedef struct current_sample {
+	u32   adc_offset_a;
+	u32   adc_offset_b;
+	u32   adc_offset_c;
+	float ia;
+	float ib;
+	float ic;
+	u8    sector;
+	u32   adc_inject_flags;
+}current_samp_t;
+
 typedef struct foc_s {
 	alpha_beta_t alpha_beta;
-	dq_t dq_ref;
-	int  rpm_ref;
+	current_samp_t current_samp; /* 三相电流采样 */
+	dq_t dq_ref; //如果是正常模式下，由速度环输出
+	int  rpm_ref; //用户设置的速度
 	dq_t dq_v;
 	u8   sector; //svpwm 扇区
-	float theta; //转子电角度
 	float vbus; //母线电压
-	int  rpm;
-	motor_param_t motor;
+	motor_param_t motor_p;
+	motor_stat_t  motor_s;
 	phase_time_t phase_time;
+	PI_ctrl_t    PI_id;
+	PI_ctrl_t    PI_iq;
+	PI_ctrl_t    PI_speed;
 	volatile foc_state_t state;
+	volatile foc_mode_t mode;
 	bool gate_output;
 	u16  hall_table[8];
 }motor_foc_t;
 
 #define degree_2_pi(d) ((float)d * M_PI / 180.0f)
 #define pi_2_degree(d) ((float)d * 180.0f / M_PI)
+
+#define SECTOR_1  0u
+#define SECTOR_2  1u
+#define SECTOR_3  2u
+#define SECTOR_4  3u
+#define SECTOR_5  4u
+#define SECTOR_6  5u
+
 #endif /* _FOC_TYPE_H__ */
 

Application/FOC/phase_current.c

@@ -0,0 +1,42 @@
+#include "hal/adc.h"
+#include "foc_type.h"
+
+static float __inline adc_to_current(u32 adc){
+	int i_adc = (int)adc;
+	if (i_adc > INT16_MAX){
+		i_adc = INT16_MAX;
+	}else if (i_adc < -INT16_MAX) {
+		i_adc = - INT16_MAX;
+	}
+	return (i_adc/4095.0f * 3.3f / 0.001f);
+}
+
+void get_phase_current(current_samp_t *cs){
+	u32 phase_current1, phase_current2;
+	HAL_ADC1_Inject_Read(cs->sector, &phase_current1, &phase_current2);
+	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
+		/* Current on Phase C is not accessible */
+      	/* Ia = PhaseAOffset - ADC converted value) */
+		cs->ia = adc_to_current(phase_current1 - cs->adc_offset_a);
+		cs->ib = adc_to_current(phase_current2 - cs->adc_offset_b);
+		cs->ic = -(cs->ia + cs->ib);
+	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
+		/* Current on Phase A is not accessible 	*/
+		/* Ib = PhaseBOffset - ADC converted value) */
+		cs->ib = adc_to_current(phase_current1 - cs->adc_offset_b);
+		cs->ic = adc_to_current(phase_current2 - cs->adc_offset_c);
+		cs->ia = -(cs->ib + cs->ic);
+	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_3) {
+		/* Current on Phase B is not accessible 	*/
+		/* Ia = PhaseAOffset - ADC converted value) */
+		cs->ia = adc_to_current(phase_current1 - cs->adc_offset_a);
+		cs->ic = adc_to_current(phase_current2 - cs->adc_offset_c);
+		cs->ib = -(cs->ia + cs->ic);
+	}
+}
+
+
+u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector){
+	cs->sector = sector;
+	return 0;
+}

Application/FOC/phase_current.h

@@ -0,0 +1,9 @@
+#ifndef _PHASE_CURRENT_H__
+#define _PHASE_CURRENT_H__
+#include "foc_type.h"
+
+void get_phase_current(current_samp_t *cs);
+u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector);
+
+#endif /* _PHASE_CURRENT_H__ */
+

Application/FOC/svpwm.c

@@ -1,7 +1,7 @@
 #include "foc/svpwm.h"
 #include "math/fast_math.h"
 
-void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase_time_t *phase_out, u8 *sector_out){
+void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
 	float alpha = alpha_beta->alpha / (3.0f/2.0f * vbus);
 	float beta  = alpha_beta->beta / (3.0f/2.0f * vbus);
 	uint32_t sector;
@@ -10,32 +10,32 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 		if (alpha >= 0.0f) {
 			//quadrant I
 			if (ONE_BY_SQRT3 * beta > alpha) {
-				sector = 2;
+				sector = SECTOR_2;
 			} else {
-				sector = 1;
+				sector = SECTOR_1;
 			}
 		} else {
 			//quadrant II
 			if (-ONE_BY_SQRT3 * beta > alpha) {
-				sector = 3;
+				sector = SECTOR_3;
 			} else {
-				sector = 2;
+				sector = SECTOR_2;
 			}
 		}
 	} else {
 		if (alpha >= 0.0f) {
 			//quadrant IV5
 			if (-ONE_BY_SQRT3 * beta > alpha) {
-				sector = 5;
+				sector = SECTOR_5;
 			} else {
-				sector = 6;
+				sector = SECTOR_6;
 			}
 		} else {
 			//quadrant III
 			if (ONE_BY_SQRT3 * beta > alpha) {
-				sector = 4;
+				sector = SECTOR_4;
 			} else {
-				sector = 5;
+				sector = SECTOR_5;
 			}
 		}
 	}
@@ -43,25 +43,25 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 	uint32_t tA, tB, tC;
 	switch (sector) {
 		// sector 1-2
-	case 1: {
+	case SECTOR_1: {
 		// Vector on-times
-		uint32_t t1 = (alpha - ONE_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t2 = (TWO_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t1 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t2 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
 	
 		// PWM timings
-		tA = (PWW_half_period - t1 - t2) / 2;
+		tA = (PWM_half_period - t1 - t2) / 2;
 		tB = tA + t1;
 		tC = tB + t2;
 		break;
 	}
 	// sector 2-3
-	case 2: {
+	case SECTOR_2: {
 		// Vector on-times
-		uint32_t t2 = (alpha + ONE_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t3 = (-alpha + ONE_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t2 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t3 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
 	
 		// PWM timings
-		tB = (PWW_half_period - t2 - t3) / 2;
+		tB = (PWM_half_period - t2 - t3) / 2;
 		tA = tB + t3;
 		tC = tA + t2;
 	
@@ -69,13 +69,13 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 	}
 	
 	// sector 3-4
-	case 3: {
+	case SECTOR_3: {
 		// Vector on-times
-		uint32_t t3 = (TWO_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t4 = (-alpha - ONE_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t3 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t4 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
 	
 		// PWM timings
-		tB = (PWW_half_period - t3 - t4) / 2;
+		tB = (PWM_half_period - t3 - t4) / 2;
 		tC = tB + t3;
 		tA = tC + t4;
 	
@@ -83,13 +83,13 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 	}
 	
 	// sector 4-5
-	case 4: {
+	case SECTOR_4: {
 		// Vector on-times
-		uint32_t t4 = (-alpha + ONE_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t5 = (-TWO_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t4 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t5 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
 	
 		// PWM timings
-		tC = (PWW_half_period - t4 - t5) / 2;
+		tC = (PWM_half_period - t4 - t5) / 2;
 		tB = tC + t5;
 		tA = tB + t4;
 	
@@ -97,13 +97,13 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 	}
 	
 	// sector 5-6
-	case 5: {
+	case SECTOR_5: {
 		// Vector on-times
-		uint32_t t5 = (-alpha - ONE_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t6 = (alpha - ONE_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t5 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t6 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
 	
 		// PWM timings
-		tC = (PWW_half_period - t5 - t6) / 2;
+		tC = (PWM_half_period - t5 - t6) / 2;
 		tA = tC + t5;
 		tB = tA + t6;
 	
@@ -111,13 +111,13 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase
 	}
 	
 	// sector 6-1
-	case 6: {
+	case SECTOR_6: {
 		// Vector on-times
-		uint32_t t6 = (-TWO_BY_SQRT3 * beta) * PWW_half_period;
-		uint32_t t1 = (alpha + ONE_BY_SQRT3 * beta) * PWW_half_period;
+		uint32_t t6 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
+		uint32_t t1 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
 
 		// PWM timings
-		tA = (PWW_half_period - t6 - t1) / 2;
+		tA = (PWM_half_period - t6 - t1) / 2;
 		tC = tA + t1;
 		tB = tC + t6;
 	

Application/Hal/adc.c

@@ -0,0 +1,83 @@
+#include "hal/adc.h"
+
+static ADC_HandleTypeDef hadc1;
+
+void HAL_ADC1_Init(void){	
+	ADC_InjectionConfTypeDef sConfigInjected = {0};
+	ADC_ChannelConfTypeDef sConfig = {0};
+	
+	hadc1.Instance = ADC1;
+	hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV1;
+	hadc1.Init.Resolution = ADC_RESOLUTION_12B;
+	hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
+	hadc1.Init.ContinuousConvMode = DISABLE;
+	hadc1.Init.DiscontinuousConvMode = DISABLE;
+	hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+	hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+	hadc1.Init.DataAlign = ADC_DATAALIGN_LEFT;
+	hadc1.Init.NbrOfConversion = 2;
+	hadc1.Init.DMAContinuousRequests = DISABLE;
+	hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+	hadc1.Init.LowPowerAutoWait = DISABLE;
+	hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
+	if (HAL_ADC_Init(&hadc1) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+	/** Configure Injected Channel
+	*/
+	sConfigInjected.InjectedChannel = ADC_CHANNEL_1;
+	sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
+	sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
+	sConfigInjected.InjectedNbrOfConversion = 3;
+	sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_7CYCLES_5;
+	sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
+	sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
+	sConfigInjected.AutoInjectedConv = DISABLE;
+	sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
+	sConfigInjected.QueueInjectedContext = ENABLE;
+	sConfigInjected.InjectedOffset = 0;
+	sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
+	if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+	/** Configure Injected Channel
+	*/
+	sConfigInjected.InjectedChannel = ADC_CHANNEL_7;
+	sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
+	if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+	/** Configure Injected Channel
+	*/
+	sConfigInjected.InjectedChannel = ADC_CHANNEL_6;
+	sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
+	if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+	/** Configure Regular Channel
+	*/
+	sConfig.Channel = ADC_CHANNEL_2;
+	sConfig.Rank = ADC_REGULAR_RANK_1;
+	sConfig.SingleDiff = ADC_SINGLE_ENDED;
+	sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5;
+	sConfig.OffsetNumber = ADC_OFFSET_NONE;
+	sConfig.Offset = 0;
+	if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+	/** Configure Regular Channel
+	*/
+	sConfig.Channel = ADC_CHANNEL_8;
+	sConfig.Rank = ADC_REGULAR_RANK_2;
+	if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+	{
+	  Error_Handler();
+	}
+
+}
+

Application/Hal/adc.h

@@ -0,0 +1,55 @@
+#ifndef _ADC_H__
+#define _ADC_H__
+#include "hal/hal.h"
+#include "stm32f3xx_ll_adc.h"
+#include "libs/types.h"
+typedef struct _R3_f3_1_adc_config {
+  	ADC_TypeDef * ADCx;            /*!< First ADC peripheral to be used.*/
+  
+  	u32 volatile * ADCDataReg1[6]; /*!< Contains the Address of ADC read value for one phase
+                                         and all the 6 sectors */
+  	u32 volatile * ADCDataReg2[6]; /*!< Contains the Address of ADC read value for one phase
+                                         and all the 6 sectors */                                         
+  	uint32_t ADCConfig [6] ; /*!< values of JSQR for first ADC for 6 sectors */ 	
+}R3_F30x_8_ADC_Config_t;
+
+static const R3_F30x_8_ADC_Config_t adc_config = {
+	.ADCx = ADC1,
+	.ADCConfig = {
+				CURRENT_V_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_W_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+				CURRENT_U_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_W_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+				CURRENT_W_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_U_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+				CURRENT_V_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_U_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+				CURRENT_U_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_V_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+				CURRENT_W_ADC_CHANNAL<<ADC_JSQR_JSQ1_Pos | CURRENT_V_ADC_CHANNAL<<ADC_JSQR_JSQ2_Pos | 1<<ADC_JSQR_JL_Pos | (LL_ADC_INJ_TRIG_EXT_TIM1_TRGO & ~ADC_INJ_TRIG_EXT_EDGE_DEFAULT),
+	},
+		
+	.ADCDataReg1 = {
+				&ADC1->JDR1,//V
+				&ADC1->JDR1,//U
+				&ADC1->JDR2,//U  
+				&ADC1->JDR2,//U 
+				&ADC1->JDR1,//U
+				&ADC1->JDR2,//V
+	},
+	.ADCDataReg2 = {
+				&ADC1->JDR2, //W
+				&ADC1->JDR2, //W
+				&ADC1->JDR1, //W
+				&ADC1->JDR1, //V
+				&ADC1->JDR2, //V
+				&ADC1->JDR1, //W
+	},
+};
+
+void HAL_ADC1_Init(void);
+void __inline HAL_ADC1_Inject_Config(u8 sector, u32 inject_flags) {
+	adc_config.ADCx->JSQR = adc_config.ADCConfig[sector] | inject_flags;
+}
+
+void __inline HAL_ADC1_Inject_Read(u8 sector, u32 *v1, u32 *v2) {
+	*v1 = *adc_config.ADCDataReg1[sector];
+	*v2 = *adc_config.ADCDataReg2[sector];
+}
+
+#endif /* _ADC_H__ */

Application/Hal/hal.c

@@ -116,6 +116,43 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 
 }
 
+void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(hadc->Instance==ADC1)
+  {
+  /* USER CODE BEGIN ADC1_MspInit 0 */
+
+  /* USER CODE END ADC1_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_ADC1_CLK_ENABLE();
+
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**ADC1 GPIO Configuration
+    PC0     ------> ADC1_IN6
+    PC1     ------> ADC1_IN7
+    PC2     ------> ADC1_IN8
+    PA0     ------> ADC1_IN1
+    PA1     ------> ADC1_IN2
+    */
+    GPIO_InitStruct.Pin = CURR_AMPL_W_Pin|CURR_AMPL_V_Pin|TEMPERATURE_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = CURR_AMPL_U_Pin|BUS_VOLTAGE_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN ADC1_MspInit 1 */
+
+  /* USER CODE END ADC1_MspInit 1 */
+  }
+
+}
+
 
 int HALL_Read(int samples) {
 	samples = 1 + 2 * samples;

Application/Hal/hal.h

@@ -17,6 +17,18 @@
 #define PWM_EN_W_Pin GPIO_PIN_12
 #define PWM_EN_W_GPIO_Port GPIOC
 
+
+#define CURR_AMPL_W_Pin GPIO_PIN_0
+#define CURR_AMPL_W_GPIO_Port GPIOC
+#define CURR_AMPL_V_Pin GPIO_PIN_1
+#define CURR_AMPL_V_GPIO_Port GPIOC
+#define TEMPERATURE_Pin GPIO_PIN_2
+#define TEMPERATURE_GPIO_Port GPIOC
+#define CURR_AMPL_U_Pin GPIO_PIN_0
+#define CURR_AMPL_U_GPIO_Port GPIOA
+#define BUS_VOLTAGE_Pin GPIO_PIN_1
+#define BUS_VOLTAGE_GPIO_Port GPIOA
+
 #define M1_OCP_Pin GPIO_PIN_11
 #define M1_OCP_GPIO_Port GPIOA
 
@@ -27,10 +39,36 @@
 #define HALL_3_PIN GPIO_PIN_10
 #define HALL_3_GROUP GPIOB
 
+#define CURRENT_U_ADC_CHANNAL 6
+#define CURRENT_V_ADC_CHANNAL 7
+#define CURRENT_W_ADC_CHANNAL 1
+
+
 #define READ_HALL1() (HAL_GPIO_ReadPin(HALL_1_GROUP, HALL_1_PIN) == GPIO_PIN_SET ?1:0)
 #define READ_HALL2() (HAL_GPIO_ReadPin(HALL_2_GROUP, HALL_2_PIN) == GPIO_PIN_SET ?1:0)
 #define READ_HALL3() (HAL_GPIO_ReadPin(HALL_3_GROUP, HALL_3_PIN) == GPIO_PIN_SET ?1:0)
 
+#define TIM_CLOCK (72000000L * 2) /*SystemClock_Config中TIM1的clk从sys PLL 过来，固定2倍的PLL频率*/
+#define TIM_CLOCK_MHz (144)
+#define ADC_CLOCK (72000000L)
+#define ADC_CLOCK_MHz (72)
+#define NS_PER_TCLK (7) /* (1/144000000 * 1000000000) */
+#define NS_2_TCLK(ns) ((ns/NS_PER_TCLK) + 1)
+#define FOC_FS (20 * 1000)
+#define FOC_PWM_period (TIM_CLOCK/FOC_FS)
+#define MAX_VBUS (12.f) //12v
+
+#define ADC_TRIG_CONV_LATENCY_CYCLES 3.5f
+#define ADC_SAMPLING_CYCLES 7.5f
+
+
+#define HW_DEAD_TIME_NS  800
+#define HW_RISE_TIME_NS  50
+#define HW_NOISE_TIME_NS 50
+#define TDead NS_2_TCLK(HW_DEAD_TIME_NS)/* 死区时间 */ 
+#define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS 开关时间*/
+#define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS开关引入的开关噪声时间 */
+#define TADC   ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC 采样时间 */
 
 void Error_Handler(void);
 void SystemClock_Config(void);

Application/Hal/pwm.c

@@ -64,7 +64,12 @@ void HAL_PWM_Init(int fs) {
 	{
 	  Error_Handler();
 	}
-#if 1
+  	sConfigOC.OCMode = TIM_OCMODE_PWM2;
+  	sConfigOC.Pulse = (SystemCoreClock / fs) - 5;
+  	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;
@@ -80,7 +85,7 @@ void HAL_PWM_Init(int fs) {
 	{
 	  Error_Handler();
 	}
-#endif	
+
 	HAL_TIM1_Pin_Init();
 }
 
@@ -124,12 +129,13 @@ void PWM_TimerInit(void){
 }
 
 
-void PWM_UpdateDuty(uint32_t duty1, uint32_t duty2, uint32_t duty3) {
+void PWM_UpdateDuty(u32 duty1, u32 duty2, u32 duty3, u32 sample_point ) {
 	TIM_TypeDef * TIMx = htim1.Instance;
 	LL_TIM_DisableUpdateEvent(TIMx);
 	LL_TIM_OC_SetCompareCH1(TIMx, duty1);
 	LL_TIM_OC_SetCompareCH2(TIMx, duty2);
 	LL_TIM_OC_SetCompareCH3(TIMx, duty3);
+	LL_TIM_OC_SetCompareCH4(TIMx, sample_point);
 	/* wait for a new PWM period */
 	LL_TIM_ClearFlag_UPDATE( TIMx );
 	LL_TIM_EnableUpdateEvent(TIMx);

Application/Hal/pwm.h

@@ -3,7 +3,7 @@
 #include "libs/types.h"
 
 void HAL_PWM_Init(int fs);
-void PWM_UpdateDuty(uint32_t duty1, uint32_t duty2, uint32_t duty3);
+void PWM_UpdateDuty(u32 duty1, u32 duty2, u32 duty3, u32 sample_point);
 void PWM_Start(void);
 void PWM_Stop(void);
 void PWM_TimerInit(void);

Project/Motor_PMSM.uvoptx

@@ -154,16 +154,16 @@
           <Type>0</Type>
           <LineNumber>25</LineNumber>
           <EnabledFlag>1</EnabledFlag>
-          <Address>134229020</Address>
+          <Address>0</Address>
           <ByteObject>0</ByteObject>
           <HtxType>0</HtxType>
           <ManyObjects>0</ManyObjects>
           <SizeOfObject>0</SizeOfObject>
           <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
+          <BreakIfRCount>0</BreakIfRCount>
           <Filename>..\Application\FOC\foc.c</Filename>
           <ExecCommand></ExecCommand>
-          <Expression>\\Motor_PMSM\../Application/FOC/foc.c\25</Expression>
+          <Expression></Expression>
         </Bp>
         <Bp>
           <Number>1</Number>
@@ -297,6 +297,42 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>4</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\FOC\foc_task.c</PathWithFileName>
+      <FilenameWithoutPath>foc_task.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>5</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\FOC\PI_Controller.c</PathWithFileName>
+      <FilenameWithoutPath>PI_Controller.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>6</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\FOC\phase_current.c</PathWithFileName>
+      <FilenameWithoutPath>phase_current.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -307,7 +343,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>4</FileNumber>
+      <FileNumber>7</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -319,7 +355,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>5</FileNumber>
+      <FileNumber>8</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -339,7 +375,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>6</FileNumber>
+      <FileNumber>9</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -351,7 +387,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>7</FileNumber>
+      <FileNumber>10</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -363,7 +399,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>8</FileNumber>
+      <FileNumber>11</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -383,7 +419,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>9</FileNumber>
+      <FileNumber>12</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -395,7 +431,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>10</FileNumber>
+      <FileNumber>13</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -405,6 +441,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>14</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Application\Hal\adc.c</PathWithFileName>
+      <FilenameWithoutPath>adc.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -415,7 +463,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>11</FileNumber>
+      <FileNumber>15</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -427,7 +475,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>12</FileNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -439,7 +487,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>13</FileNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -451,7 +499,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -463,7 +511,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -475,7 +523,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -487,7 +535,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -499,7 +547,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -511,7 +559,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -523,7 +571,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -535,7 +583,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -547,7 +595,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -559,7 +607,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -571,7 +619,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -583,7 +631,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -595,7 +643,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -607,7 +655,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -619,7 +667,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -631,7 +679,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -643,7 +691,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -655,7 +703,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -667,7 +715,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -679,7 +727,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -691,7 +739,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -711,7 +759,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -723,7 +771,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/Motor_PMSM.uvprojx

@@ -403,6 +403,21 @@
               <FileType>1</FileType>
               <FilePath>..\Application\FOC\svpwm.c</FilePath>
             </File>
+            <File>
+              <FileName>foc_task.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\FOC\foc_task.c</FilePath>
+            </File>
+            <File>
+              <FileName>PI_Controller.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\FOC\PI_Controller.c</FilePath>
+            </File>
+            <File>
+              <FileName>phase_current.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\FOC\phase_current.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -453,6 +468,11 @@
               <FileType>1</FileType>
               <FilePath>..\Application\Hal\pwm.c</FilePath>
             </File>
+            <File>
+              <FileName>adc.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Application\Hal\adc.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>