as5407P编码器调试

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

Applications/app/app.c

@@ -31,7 +31,8 @@ void fetch_jtag_cmd(void) {
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 3) {
-		PMSM_FOC_SetOpenVdq(0, (jtag_data));
+		float vq = (float)jtag_data/10.0f;
+		PMSM_FOC_SetOpenVdq(0, (vq));
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 4) {
 		foc_cmd.cmd = Foc_Cali_Hall_Phase;

Applications/app/key_process.c

@@ -14,7 +14,7 @@
 static u8 key_value[3];
 static float foc_current = 0.0f;
 static u8 ctrl_mode = CTRL_MODE_OPEN;
-static float max_speed = 1500;
+static float max_speed = 6000;
 static u32 key_task(void *p) {
 	foc_cmd_body_t foc_cmd;
 	u8 cmd_data[16];

Applications/bsp/bsp.c

@@ -13,7 +13,8 @@ void bsp_init(void){
 	gd32_bkp_init();
 	dbg_periph_enable(DBG_TIMER0_HOLD);
 	dbg_periph_enable(DBG_TIMER1_HOLD);
-	dbg_periph_enable(DBG_TIMER2_HOLD);
+	//dbg_periph_enable(DBG_TIMER2_HOLD);
+	//dbg_periph_enable(DBG_TIMER3_HOLD);
 	systick_open();
 	task_ticks_enable();
 	timer_count32_init();

Applications/bsp/bsp.h

@@ -16,7 +16,6 @@
 //#define GD32_FOC_DEMO
 #define USER_ITMER_BRAKE 0
 
-
 #define SYSTEM_CLOCK (120000000u) //system clk 120M Hz
 #define TIM_CLOCK (SYSTEM_CLOCK) /*SystemClock_Config��TIM1��clk��sys PLL �������̶�2����PLLƵ��*/
 #define TIM_CLOCK_MHz (120u)
@@ -24,7 +23,7 @@
 #define ADC_CLOCK_MHz (30u)
 #define NS_PER_TCLK (8u) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) (((ns)/NS_PER_TCLK) + 1u) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (16000u)
+#define FOC_PWM_FS (10000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
@@ -70,7 +69,9 @@
 #define CAN_IRQ_PRIORITY       6 
 #define RTC_IRQ_PRIORITY       7 
 #define UART_IRQ_PRIORITY      6 
-
+#define ENC_TIMER_IRQ_PRIORITY 2
+#define ENC_PWM_IRQ_PRIORITY 2
+#define ENC_I_EXIT_IRQ_PRIORITY 2
 //#define ENABLE_PWM_UP_IRQ 1
 
 #define THREE_SHUNTS_SAMPLE 1
@@ -80,6 +81,8 @@
 
 //#define ENABLE_AUX_TIMER 1
 
+#define NO_SAMPLE_IDC //如果硬件没有采集母线电流，定义一下
+
 #ifdef GD32_FOC_DEMO
 #define ADC_TO_CURR_ceof (0.004f)
 #define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*16/4096.0f)
@@ -88,6 +91,10 @@
 #define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*45/4096.0f)
 #define THROTTLE_VOL_CEOF (1)
 #endif
+
+#define USE_ENCODER_ABI
+//#define USE_ENCODER_HALL
+
 void bsp_init(void);
 void wdog_reload(void);
 void system_reboot(void);

Applications/bsp/enc_intf.c

@@ -0,0 +1,158 @@
+#include "bsp/enc_intf.h"
+#include "libs/logger.h"
+
+static void _io_init(void) {
+	rcu_periph_clock_enable(ENC_A_RCU);
+	rcu_periph_clock_enable(ENC_B_RCU);
+	rcu_periph_clock_enable(ENC_PWM_RCU);
+	rcu_periph_clock_enable(ENC_I_RCU);
+	gpio_init(ENC_A_GROUP, ENC_A_MODE, GPIO_OSPEED_50MHZ, ENC_A_PIN);
+	gpio_init(ENC_B_GROUP, ENC_B_MODE, GPIO_OSPEED_50MHZ, ENC_B_PIN);
+	gpio_init(ENC_PWM_GROUP, ENC_PWM_MODE, GPIO_OSPEED_50MHZ, ENC_PWM_PIN);
+	gpio_init(ENC_I_GROUP, ENC_I_MODE, GPIO_OSPEED_50MHZ, ENC_I_PIN);
+}
+
+static void _io_init_irq(void) {
+	gpio_exti_source_select(ENC_I_EXIT_SRC_GROUP, ENC_I_EXIT_SRC_PIN);
+	exti_init(ENC_I_EXTI, EXTI_INTERRUPT, EXTI_TRIG_RISING);
+	exti_interrupt_flag_clear(ENC_I_EXTI);
+	exti_interrupt_enable(ENC_I_EXTI);
+
+	nvic_irq_enable(ENC_I_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0U);
+}
+
+void enc_intf_init(u32 rate) {
+	_io_init();
+	enc_intf_pwm_counter();
+	enc_intf_quadrature_init(rate);
+	_io_init_irq();
+}
+
+void enc_intf_quadrature_init(u32 rate) {
+	timer_parameter_struct timer_initpara;
+	timer_ic_parameter_struct timer_icinitpara;
+
+	u32 timer = ENC_TIMER;
+
+	rcu_periph_clock_enable(ENC_TIMER_RCU);
+
+	timer_deinit(timer);
+
+	/* TIMER configuration */
+	timer_struct_para_init(&timer_initpara);
+	timer_initpara.prescaler		= 0;//TIM_CLOCK/PWM_TIME_CLK - 1;
+	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
+	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
+	timer_initpara.period		   = rate-1;
+	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
+	timer_initpara.repetitioncounter = 0;
+	timer_init(timer,&timer_initpara);
+
+    /* initialize TIMER channel input parameter struct */
+    timer_channel_input_struct_para_init(&timer_icinitpara);
+    /* TIMER2 CH0 input capture configuration */
+    timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
+    timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
+    timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
+    timer_icinitpara.icfilter    = 0x0;
+    timer_input_capture_config(timer,TIMER_CH_0,&timer_icinitpara);
+	
+	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
+	timer_input_capture_config(timer,TIMER_CH_1,&timer_icinitpara);
+
+	timer_quadrature_decoder_mode_config(timer ,TIMER_ENCODER_MODE2,TIMER_IC_POLARITY_RISING, TIMER_IC_POLARITY_RISING);
+	
+	/* auto-reload preload enable */
+	timer_auto_reload_shadow_enable(timer);	
+
+	
+	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);
+
+	//timer_interrupt_enable(timer, TIMER_INT_UP);
+	//nvic_irq_enable(ENC_TIMER_IRQ, ENC_TIMER_IRQ_PRIORITY, 0);
+
+	/* TIMER2 counter enable */
+	timer_enable(timer);
+}
+
+void enc_intf_pwm_counter(void) {
+	timer_ic_parameter_struct timer_icinitpara;
+	timer_parameter_struct timer_initpara;
+	u32 timer = ENC_PWM_TIMER;
+
+	rcu_periph_clock_enable(ENC_PWM_TIMER_RCU);
+
+	timer_deinit(timer);
+
+	/* TIMER configuration */
+	timer_struct_para_init(&timer_initpara);
+	timer_initpara.prescaler		= TIM_CLOCK/PWM_TIME_CLK - 1;
+	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
+	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
+	timer_initpara.period		   = 65535;
+	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
+	timer_initpara.repetitioncounter = 0;
+	timer_init(timer,&timer_initpara);
+
+	timer_channel_input_struct_para_init(&timer_icinitpara);
+	/* TIMER CH0 PWM input capture configuration */
+	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
+	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
+	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
+	timer_icinitpara.icfilter	 = 0x0;
+	timer_input_pwm_capture_config(timer, ENC_PWM_TIMER_CHAN, &timer_icinitpara);
+
+	/* slave mode selection: TIMER */
+	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_CI0FE0);
+	timer_slave_mode_select(timer, TIMER_SLAVE_MODE_RESTART);
+
+	/* select the master slave mode */
+	timer_master_slave_mode_config(timer, TIMER_MASTER_SLAVE_MODE_ENABLE);
+
+    /* auto-reload preload enable */
+    timer_auto_reload_shadow_enable(timer);
+    /* clear channel 0 interrupt bit */
+    timer_interrupt_flag_clear(timer, ENC_PWM_TIMER_INT_FLG);
+    /* channel 0 interrupt enable */
+    timer_interrupt_enable(timer, ENC_PWM_TIMER_IRQ_CH);
+	nvic_irq_enable(ENC_PWM_TIMER_IRQ, ENC_PWM_IRQ_PRIORITY, 0);
+    /* TIMER2 counter enable */
+    timer_enable(timer);
+}
+
+__weak void ENC_TIMER_Overflow(void) {
+
+}
+
+void ENC_TIMER_IRQHandler(void) {
+	if (SET == timer_interrupt_flag_get(ENC_TIMER, TIMER_INT_FLAG_UP)) {
+		timer_interrupt_flag_clear(ENC_TIMER, TIMER_INT_FLAG_UP);
+		ENC_TIMER_Overflow();
+	}
+}
+
+
+__weak void ENC_ABI_IRQHandler(void) {
+
+}
+
+void ABI_I_IRQHandler(void) {
+	ENC_ABI_IRQHandler();
+}
+
+__weak void ENC_PWM_Duty_Handler(float t, float d) {
+
+}
+
+void ENC_PWM_IRQHandler(void) {
+    if(SET == timer_interrupt_flag_get(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG)){
+        /* clear channel 0 interrupt bit */
+        timer_interrupt_flag_clear(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG);
+        /* read channel 0 capture value */
+        u32 ic0value = TIMER_CH0CV(ENC_PWM_TIMER)+1;
+		u32 ic1value = TIMER_CH1CV(ENC_PWM_TIMER)+1;
+		float p_calc = ENC_PWM_Calc_P(ic0value);
+		float d_calc = ENC_PWM_Calc_P(ic1value);
+		ENC_PWM_Duty_Handler(p_calc, d_calc);
+    }
+}

Applications/bsp/enc_intf.h

@@ -0,0 +1,72 @@
+#ifndef _ENC_INTF_H__
+#define _ENC_INTF_H__
+#include "os/os_types.h"
+#include "bsp/bsp.h"
+
+#define PWM_TIME_CLK 10000000U
+
+#ifdef GD32_FOC_DEMO
+#define ENC_A_GROUP GPIOA
+#define ENC_A_PIN GPIO_PIN_6
+#define ENC_A_RCU RCU_GPIOA
+#define ENC_A_MODE GPIO_MODE_IN_FLOATING
+
+#define ENC_B_GROUP GPIOA
+#define ENC_B_PIN GPIO_PIN_7
+#define ENC_B_RCU RCU_GPIOA
+#define ENC_B_MODE GPIO_MODE_IN_FLOATING
+
+#define ENC_PWM_GROUP GPIOB
+#define ENC_PWM_PIN GPIO_PIN_6
+#define ENC_PWM_RCU RCU_GPIOB
+#define ENC_PWM_MODE GPIO_MODE_IN_FLOATING
+
+#define ENC_I_GROUP GPIOC     /*测量编码器的ABI的I信号，360度同步一次*/
+#define ENC_I_PIN GPIO_PIN_10
+#define ENC_I_RCU RCU_GPIOC
+#define ENC_I_MODE GPIO_MODE_IPU
+#define ENC_I_IRQ  EXTI10_15_IRQn
+#define ENC_I_EXTI EXTI_10
+#define ENC_I_EXIT_SRC_GROUP GPIO_PORT_SOURCE_GPIOC
+#define ENC_I_EXIT_SRC_PIN GPIO_PIN_SOURCE_10
+
+#define ENC_TIMER TIMER2  /* 测量编码器的ABI信号的AB信号 */
+#define ENC_TIMER_RCU RCU_TIMER2
+#define ENC_TIMER_IRQ TIMER2_IRQn
+#define ENC_TIMER_IRQHandler TIMER2_IRQHandler
+
+#define ENC_PWM_TIMER TIMER3    /* 测量绝对编码器PWM输出的占空比，获取转子angle*/
+#define ENC_PWM_TIMER_RCU RCU_TIMER3
+#define ENC_PWM_TIMER_IRQ TIMER3_IRQn
+#define ENC_PWM_TIMER_CHAN  TIMER_CH_0
+#define ENC_PWM_TIMER_IRQ_CH TIMER_INT_CH0
+#define ENC_PWM_TIMER_INT_FLG TIMER_INT_FLAG_CH0
+#define ENC_PWM_IRQHandler TIMER3_IRQHandler
+
+#else
+#define ENC_A_GROUP GPIOB
+#define ENC_A_PIN GPIO_PIN_8
+#define ENC_B_GROUP GPIOB
+#define ENC_B_PIN GPIO_PIN_7
+#define ENC_PWM_GROUP GPIOB
+#define ENC_PWM_PIN GPIO_PIN_6
+#endif
+
+#define ENC_DIR_UP 1
+#define ENC_DIR_DOWN 2
+
+#define ENC_PWM_Calc_P(t) ((float)t / (float)PWM_TIME_CLK)
+
+#define ENC_COUNT TIMER_CNT(ENC_TIMER)
+
+#define ENC_Direction() ((TIMER_CTL0(ENC_TIMER) & TIMER_CTL0_DIR)?ENC_DIR_DOWN:ENC_DIR_UP)
+
+#define ENC_OverFlow() ((TIMER_INTF(ENC_TIMER) & TIMER_FLAG_UP)?true:false)
+#define ENC_ClearUpFlags() (TIMER_INTF(ENC_TIMER) = (~(uint32_t)TIMER_FLAG_UP))
+
+void enc_intf_quadrature_init(u32 rate);
+void enc_intf_pwm_counter(void);
+void enc_intf_init(u32 rate);
+
+#endif /*_ENC_INTF_H__*/
+

Applications/bsp/gpio.c

@@ -17,7 +17,7 @@ const static gpio_pin_config_t _pins[] = {
 #ifdef GD32_FOC_DEMO
 	[IR2136S_Enable_pin] = {GPIOA, GPIO_PIN_12, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
 	[LED1_Enable_pin] = {GPIOD, GPIO_PIN_2, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
-	[LED2_Enable_pin] = {GPIOB, GPIO_PIN_9, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
+	[LED2_Enable_pin] = {0, GPIO_PIN_9, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
 	[LED3_Enable_pin] = {GPIOC, GPIO_PIN_5, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
 	[KEY_Start_pin] = {GPIOB, GPIO_PIN_5, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, 0},
 	[KEY_Stop_pin] = {GPIOB, GPIO_PIN_11, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, 0},
@@ -35,8 +35,10 @@ void gpio_pin_init(void){
 	for (int i = 0; i < ARRAY_SIZE(_pins); i++){
 		if (_pins[i].init_value != -1){
 			gpio_bit_write(_pins[i].group, _pins[i].pin, (bit_status)_pins[i].init_value);
-		}		
-		gpio_init(_pins[i].group, _pins[i].mode, _pins[i].speed, _pins[i].pin);
+		}
+		if (_pins[i].group != 0) {
+			gpio_init(_pins[i].group, _pins[i].mode, _pins[i].speed, _pins[i].pin);
+		}
 	}	
 }
 
@@ -59,7 +61,7 @@ void gpio_led1_enable(bool enable) {
 
 void gpio_led2_enable(bool enable) {
 #ifdef GD32_FOC_DEMO
-	gpio_bit_write(_pins[LED2_Enable_pin].group, _pins[LED2_Enable_pin].pin, (enable)?SET:RESET);
+	//gpio_bit_write(_pins[LED2_Enable_pin].group, _pins[LED2_Enable_pin].pin, (enable)?SET:RESET);
 #endif
 }
 

Applications/bsp/mc_hall_gpio.h

@@ -6,23 +6,21 @@
 //52.2ms 
 //25.2
 #ifdef GD32_FOC_DEMO
-#define HALL_GPOI_CLK RCU_GPIOB
-#define HALL_1_PIN GPIO_PIN_6
 #define HALL_1_GROUP GPIOA
-#define HALL_2_PIN GPIO_PIN_7
+#define HALL_1_PIN GPIO_PIN_6
 #define HALL_2_GROUP GPIOA
-#define HALL_3_PIN GPIO_PIN_0
+#define HALL_2_PIN GPIO_PIN_7
 #define HALL_3_GROUP GPIOB
+#define HALL_3_PIN GPIO_PIN_0
 #else
-#define HALL_GPOI_CLK RCU_GPIOB
-#define HALL_1_PIN GPIO_PIN_8
 #define HALL_1_GROUP GPIOB
-#define HALL_2_PIN GPIO_PIN_7
+#define HALL_1_PIN GPIO_PIN_8
 #define HALL_2_GROUP GPIOB
-#define HALL_3_PIN GPIO_PIN_6
+#define HALL_2_PIN GPIO_PIN_7
 #define HALL_3_GROUP GPIOB
-
+#define HALL_3_PIN GPIO_PIN_6
 #endif
+
 #define DEGREES_120 0u
 #define DEGREES_60 1u
 

Applications/bsp/mc_irqs.c

@@ -89,6 +89,10 @@ __weak void HALL_IRQHandler(void) {
 
 }
 
+__weak void ABI_I_IRQHandler(void) {
+
+}
+
 void ADC0_1_IRQHandler(void)
 {
 	//adc_disable_ext_trigger();
@@ -146,7 +150,6 @@ void EXTI4_IRQHandler(void)
 	}	
 }
 
-
 void EXTI5_9_IRQHandler(void){
 	if(RESET != exti_interrupt_flag_get(EXTI_5)){
 		exti_interrupt_flag_clear(EXTI_5);
@@ -174,6 +177,9 @@ void EXTI5_9_IRQHandler(void){
 void EXTI10_15_IRQHandler(void){
 	if(RESET != exti_interrupt_flag_get(EXTI_10)){
 		exti_interrupt_flag_clear(EXTI_10);
+#ifdef GD32_FOC_DEMO
+		ABI_I_IRQHandler();
+#endif
 	}
 	if(RESET != exti_interrupt_flag_get(EXTI_11)){
 		exti_interrupt_flag_clear(EXTI_11);

Applications/foc/commands.c

@@ -81,7 +81,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Cali_Hall_Phase:
 		{
 			s16 vd = decode_s16((u8 *)command->data);
-			mc_hall_calibrate((vd));
+			mc_encoder_calibrate((vd));
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:

Applications/foc/core/PI_Controller.h

@@ -62,6 +62,44 @@ static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
 	return (MATH_sat(out, pi->min, pi->max));
 }
 
+static __INLINE float _fmod(float v, s32 m) {
+	int v_i = (int)v;
+	int m_i = v_i % m;
+	return (v-v_i) + m_i;
+}
+
+typedef struct {
+	float observer;
+	float kp;
+	float ki;
+	float Ui;
+	float out;
+	s32   max_wp;
+	bool  ob_wp;
+	float DT;
+}PLL_t;
+
+static __INLINE void PLL_Reset(PLL_t *pll) {
+	pll->observer = 0.0f;
+	pll->out = 0.0f;
+	pll->ob_wp = false;
+}
+/*
+Transfer func:
+ki*s/(s*s + kp*s + ki)
+ki = (kp/2) * (kp/2)
+Wn = kp/2 ==> kp = 2 * Wn, Wn为PLL的带宽
+*/
+static __INLINE float PLL_run(PLL_t *pll, float sample, float comp) {
+	float observer = pll->observer - comp;
+	if (comp < 0) {
+		observer = -comp - pll->observer;
+	}
+	float delta = sample - observer;
+	pll->observer = observer + (pll->out + pll->kp * delta) * pll->DT;
+	pll->out += pll->ki * delta * pll->DT;
+	return pll->out;
+}
 
 #endif
 #endif	/*_PI_Contrller_H__*/

Applications/foc/core/PMSM_FOC_Core.c

@@ -5,7 +5,7 @@
 #include "math/fix_math.h"
 #include "math/fast_math.h"
 #include "foc/motor/current.h"
-#include "foc/motor/hall.h"
+#include "foc/motor/motor.h"
 #include "foc/core/svpwm.h"
 #include "foc/core/torque_lut.h"
 #include "foc/samples.h"
@@ -151,13 +151,13 @@ void PMSM_FOC_CoreInit(void) {
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	if (!_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
-		_gFOC_Ctrl.in.s_hallAngle = hall_sensor_get_theta();
+		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
 	}else {
-		_gFOC_Ctrl.in.s_hallAngle = hall_sensor_get_theta();
+		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_hallAngle;
 	}
 
-	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed() / _gFOC_Ctrl.params.n_poles;
+	_gFOC_Ctrl.in.s_motRPM = motor_encoder_get_speed() / _gFOC_Ctrl.params.n_poles;
 	_gFOC_Ctrl.in.s_vDC = get_vbus_float();
 	//sample current
 	phase_current_get(_gFOC_Ctrl.in.s_iABC);
@@ -231,9 +231,10 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases);
 
+#ifdef NO_SAMPLE_IDC
 	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.d, _gFOC_Ctrl.out.s_RealIdq.d, 0.01f);
 	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.q, _gFOC_Ctrl.out.s_RealIdq.q, 0.01f);
-
+#endif
 	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
 		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);

Applications/foc/foc_config.h

@@ -15,7 +15,7 @@
 #define MIN_CRUISE_RPM 	   1000     /* 能启动定速巡航的最小速度 */
 #define MAX_TORQUE         30
 #ifdef GD32_FOC_DEMO
-#define MAX_vDC (24)   /* 母线最大电压 V*/
+#define MAX_vDC (16)   /* 母线最大电压 V*/
 #else
 #define MAX_vDC (50)   /* 母线最大电压 V*/
 #endif

Applications/foc/motor/encoder.c

@@ -0,0 +1,160 @@
+#include "bsp/bsp.h"
+#include "bsp/enc_intf.h"
+#include "bsp/timer_count32.h"
+#include "foc/motor/encoder.h"
+#include "foc/motor/motor_param.h"
+#include "libs/logger.h"
+
+#define ANGLE_OFFSET (-50.0f)//133.0f
+/* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
+#define DIR_ADJUGE_MAX_CNT 10
+#define PLL_BANDWIDTH 1000
+static encoder_t g_encoder;
+
+#define USE_PWM_ONLY
+
+static __INLINE void encoder_pll_update_gain(void) {
+	if (g_encoder.pll_bandwidth_shadow != g_encoder.pll_bandwidth) {
+		g_encoder.pll_bandwidth = g_encoder.pll_bandwidth_shadow;
+		g_encoder.est_pll.kp = 2.0f * g_encoder.pll_bandwidth;
+		g_encoder.est_pll.ki = 0.25f * g_encoder.est_pll.kp * g_encoder.est_pll.kp;		
+	}
+}
+
+static void _init_pll(void) {
+	g_encoder.est_pll.DT = FOC_CTRL_US;
+	g_encoder.est_pll.max_wp = g_encoder.cpr;
+	g_encoder.pll_bandwidth = 0;
+	g_encoder.pll_bandwidth_shadow = PLL_BANDWIDTH;
+	encoder_pll_update_gain();
+	PLL_Reset(&g_encoder.est_pll);
+}
+
+
+void encoder_init(void) {
+	encoder_init_clear(POSITIVE);
+	enc_intf_init(ENC_MAX_RES);
+}
+
+void encoder_set_direction(s8 direction) {
+	encoder_init_clear(direction);
+}
+
+void encoder_set_bandwidth(float bandwidth) {
+	g_encoder.pll_bandwidth_shadow = bandwidth;
+}
+
+void encoder_init_clear(s8 diretcion) {
+	_init_pll();
+	g_encoder.cpr = ENC_MAX_RES;
+	g_encoder.enc_offset = ANGLE_OFFSET;
+	g_encoder.b_index_found = false;
+	g_encoder.direction = diretcion;
+	g_encoder.abi_angle = 0.0f;
+	g_encoder.pwm_angle = 0.0f;
+	g_encoder.est_angle_counts = 0;
+	g_encoder.est_vel_counts = 0;
+	g_encoder.interpolation = 0.0f;
+}
+
+static __INLINE void encoder_run_pll(float cnt) {
+	float pll_comp = 0.0f;
+	if (g_encoder.b_timer_ov) {
+		if(ENC_Direction() == ENC_DIR_DOWN){
+			pll_comp = -((float)g_encoder.cpr);
+		}else {
+			pll_comp = g_encoder.cpr;
+		}
+		g_encoder.b_timer_ov = false;
+	}
+	encoder_pll_update_gain();
+	g_encoder.est_vel_counts = PLL_run(&g_encoder.est_pll, cnt, pll_comp);
+	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
+}
+static int _count = 0;
+float encoder_get_theta(void) {
+	if (!g_encoder.b_index_found) {
+		return g_encoder.pwm_angle;
+	}
+	
+	u32 cnt = ENC_COUNT;
+	__NOP();__NOP();__NOP();__NOP();
+	if (ENC_OverFlow()) {
+		cnt = ENC_COUNT;
+		g_encoder.b_timer_ov = true;
+		ENC_ClearUpFlags();
+	}
+	if (cnt == g_encoder.last_cnt) {
+		g_encoder.interpolation += g_encoder.est_vel_counts * FOC_CTRL_US;
+		if (g_encoder.interpolation > 1.0f) {
+			g_encoder.interpolation = 1.0f;
+		}else if (g_encoder.interpolation < -1.0f) {
+			g_encoder.interpolation = -1.0f;
+		}
+	}else {
+		g_encoder.interpolation = 0.0f;
+	}
+	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * MOTOR_POLES + g_encoder.enc_offset;
+	rand_angle(g_encoder.abi_angle);
+
+	encoder_run_pll((float)(cnt));
+
+	if (_count++ % 5 == 0) {
+		//plot_2data16(cnt, (s16)(g_encoder.est_vel_counts/((float)g_encoder.cpr) * 60.0f));
+	}
+
+	g_encoder.last_cnt = cnt;
+	g_encoder.last_us = timer_count32_get();
+	return g_encoder.abi_angle;
+}
+
+float encoder_get_speed(void) {
+	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f * MOTOR_POLES;
+}
+
+void encoder_detect_offset(float angle){
+	//plot_3data16(angle, g_encoder.pwm_angle, g_encoder.abi_angle);
+}
+
+
+/*I 信号的中断处理，一圈一个中断*/
+void ENC_ABI_IRQHandler(void) {
+	g_encoder.b_index_cnt = ENC_COUNT;	
+	if (!g_encoder.b_index_found){
+		ENC_COUNT = g_encoder.pwm_count;
+		g_encoder.last_cnt = 0;
+		g_encoder.b_index_found = true;
+	}else {
+		if ((g_encoder.b_index_cnt > 100) && (g_encoder.b_index_cnt < (g_encoder.cpr - 100))) {
+			//g_encoder.b_index_found = false;
+		}
+		//g_encoder.last_cnt = g_encoder.b_index_cnt;
+	}
+}
+
+/* 编码器AB信号读书溢出处理 */
+void ENC_TIMER_Overflow(void) {
+	g_encoder.b_timer_ov = true;
+}
+
+
+/*PWM 信号捕获一个周期的处理 */
+void ENC_PWM_Duty_Handler(float t, float d) {
+	float duty = d/t;
+	if (duty < ENC_PWM_Min_P || duty > 1.0f) {
+		return;
+	}
+	float Nr = ENC_Duty_2_Pluse_Nr(duty);
+	if (Nr < 0) {
+		return;
+	}
+	g_encoder.pwm_count = (u32)Nr;
+	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * MOTOR_POLES + g_encoder.enc_offset;
+	
+	rand_angle(g_encoder.pwm_angle);
+
+	//plot_1data16(g_encoder.est_vel_counts);
+	//plot_1data16(g_encoder.pwm_angle);
+	//plot_3data16(g_encoder.last_cnt, g_encoder.est_angle_counts, g_encoder.est_vel_counts);
+}
+

Applications/foc/motor/encoder.h

@@ -0,0 +1,53 @@
+#ifndef _Encoder_H__
+#define _Encoder_H__
+#include "bsp/enc_intf.h"
+#include "foc/core/PI_Controller.h"
+typedef struct {
+	bool  b_index_found; //I 对齐
+	float enc_offset;
+	float pwm_angle;
+	u32   pwm_count;
+	float abi_angle;
+	float interpolation;
+	float pll_bandwidth_shadow;
+	float pll_bandwidth;
+	PLL_t est_pll;
+	bool  b_timer_ov;
+	u32   cpr;
+	u32   last_cnt;
+	u32   b_index_cnt;
+	u32   last_us;
+	s8    enc_dir; //encoder count dir UP or DOWN
+	s8    direction; //motor running dir, 逆时针 正，顺时针负
+	u16   p_dir_cnt;
+	u16   n_dir_cnt;
+	float est_vel_counts; //每秒多少个计数
+	float est_angle_counts;
+	float rpm;
+}encoder_t;
+
+#define ENC_MAX_RES  4096
+/*min. 490 Hz, max 603 Hz*/
+#define ENC_PWM_Max_P (1.0f/490.0f)
+#define ENC_PWM_Min_P (1.0f/603.0f)
+
+#define ENC_PWM_MAX_RES 4119.0F
+#define ENC_PWM_INIT_WIDTH 12.0F //PWM 起始宽度
+#define ENC_PWM_ERROR_WIDTH 4.0f //PWM 指示错误的宽度
+#define ENC_PWM_END_WIDTH   0.0F
+#define ENC_PWM_Error_P (ENC_PWM_INIT_WIDTH/ENC_PWM_MAX_RES)
+#define ENC_PWM_MIN_duty ((ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)/ENC_PWM_MAX_RES)
+
+#define ENC_Duty_2_Pluse_Nr(duty) (duty * ENC_PWM_MAX_RES - (ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)) //通过占空比计算有几个脉冲
+#define ENC_Pluse_Nr_2_angle(Nr) (360.0f/(float)ENC_MAX_RES * (Nr))
+
+
+void encoder_init(void);
+void encoder_init_clear(s8 direction);
+float encoder_get_theta(void);
+float encoder_get_speed(void);
+void encoder_detect_offset(float angle);
+void encoder_set_direction(s8 direction);
+
+#endif /* _Encoder_H__ */
+

Applications/foc/motor/hall.c

@@ -33,8 +33,6 @@ measure_time_t g_meas_hall = {.exec_max_time = 6,};
 //#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
 #define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
 
-#define rand_angle(a) {if (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;else if (a < 0) a +=PHASE_360_DEGREE;};
-
 static void __inline _hall_put_sample(u32 ticks, float angle) {
 	hall_sample_t *s = &_sensor_hander.samples;
 	s->ticks_sum -= s->ticks[s->index];
@@ -86,24 +84,30 @@ static bool __inline _hall_data_empty(void) {
 }
 */
 
-static void hall_sensor_default(void) {
+static void hall_sensor_default(s8 direction) {
 	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
 	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
 	for (int i = 0; i < 8; i++) {
 		_sensor_hander.angle_table[i] = (mc_config_get()->hall_table[i]);
 	}
 	_sensor_hander.manual_angle = 0x3FF;
+	_sensor_hander.direction = direction;
+	_sensor_hander.running_dir = _sensor_hander.direction;
 	_hall_init_el_angle();
 }
 
 void hall_sensor_init(void) {
-	hall_sensor_default();
+	hall_sensor_default(POSITIVE);
 	mc_hall_init();
 	shark_task_create(_hall_detect_task, NULL);
 }
 
-void hall_sensor_clear(void) {
-	hall_sensor_default();
+void hall_set_direction(s8 direction) {
+	hall_sensor_default(direction);
+}
+
+void hall_sensor_clear(s8 direction) {
+	hall_set_direction(direction);
 }
 
 static u32 _hall_detect_task(void *args) {
@@ -111,7 +115,7 @@ static u32 _hall_detect_task(void *args) {
 		u32 ticks_now = timer_count32_get();
 		if (ticks_now > _sensor_hander.hall_ticks) {
 			if (timer_count32_delta(ticks_now, _sensor_hander.hall_ticks) > 2000*1000) {
-				//hall_sensor_clear();
+				hall_sensor_clear(_sensor_hander.direction);
 			}
 		}
 	}
@@ -119,7 +123,9 @@ static u32 _hall_detect_task(void *args) {
 }
 
 #if 1
+int hall_e_count = 0;
 float hall_sensor_get_theta(void){
+	hall_e_count++;
 	float angle_add = _sensor_hander.delta_angle_ts;
 	if (_sensor_hander.comp_count > 0) {
 		_sensor_hander.comp_count--;
@@ -136,9 +142,10 @@ float hall_sensor_get_theta(void){
 	}else {
 		el_angle = _sensor_hander.estimate_el_angle - el_angle;
 	}
-
 	rand_angle(el_angle);
-
+	if (hall_e_count%5 == 0) {
+		plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
+	}
 	return (el_angle);
 }
 #else
@@ -169,7 +176,7 @@ float hall_sensor_get_theta(void){
 #endif
 
 float hall_sensor_get_speed(void) {
-	return (_sensor_hander.rpm);
+	return (_sensor_hander.rpm) * _sensor_hander.running_dir;
 }
 
 int hall_offset_increase(int inc) {
@@ -271,6 +278,7 @@ static void _hall_init_el_angle(void) {
 #endif
 	_sensor_hander.sensor_error = 0;
   	/* Initialize the measured angle */
+	_sensor_hander.measured_el_angle += PHASE_60_DEGREE;
 	rand_angle(_sensor_hander.measured_el_angle);
   	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
 	_sensor_hander.hall_ticks = timer_count32_get();
@@ -289,55 +297,55 @@ static float _hall_position(u8 state_now, u8 state_prev) {
 	switch (state_now) {
 		case STATE_1:
 			if (state_prev == STATE_5) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
 			}else if (state_prev == STATE_3) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
 			}
 			break;
 		case STATE_2:
 			if (state_prev == STATE_3) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
 			}else if (state_prev == STATE_6) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
 			}
 			break;
 		case STATE_3:
 			if (state_prev == STATE_1) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
 			}else if (state_prev == STATE_2) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
 			}
 			break;
 		case STATE_4:
 			if (state_prev == STATE_6) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
 			}else if (state_prev == STATE_5) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
 			}
 			break;
 		case STATE_5:
 			if (state_prev == STATE_4) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
 			}else if (state_prev == STATE_1) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
 			}
 			break;
 		case STATE_6:
 			if (state_prev == STATE_2) {
-				_sensor_hander.direction = POSITIVE;
+				_sensor_hander.running_dir = POSITIVE;
 				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
 			}else if (state_prev == STATE_4) {
-				_sensor_hander.direction = NEGATIVE;
+				_sensor_hander.running_dir = NEGATIVE;
 				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
 			}
 			break;

Applications/foc/motor/hall.h

@@ -4,26 +4,7 @@
 #include "bsp/bsp.h"
 #include "math/fix_math.h"
 
-#define NEGATIVE          (int8_t)-1
-#define POSITIVE          (int8_t)1
 
-//S32Q19 格式
-#define PHASE_0_DEGREE (0.0f)
-#if 0
-#define PHASE_60_DEGREE (983040*32)
-#define PHASE_120_DEGREE (1966080*32)
-#define PHASE_180_DEGREE (2949120*32)
-#define PHASE_240_DEGREE (3932160*32)
-#define PHASE_300_DEGREE (4915200*32)
-#define PHASE_360_DEGREE (5898240*32)
-#else
-#define PHASE_60_DEGREE (60.0f)
-#define PHASE_120_DEGREE (120.0f)
-#define PHASE_180_DEGREE (180.0f)
-#define PHASE_240_DEGREE (240.0f)
-#define PHASE_300_DEGREE (300.0f)
-#define PHASE_360_DEGREE (360.0f)
-#endif
 #define STATE_0 (uint8_t)0
 #define STATE_1 (uint8_t)1
 #define STATE_2 (uint8_t)2
@@ -60,6 +41,7 @@ typedef struct {
 	u8    			hall_stat;
 	u32   			hall_ticks;	
 	s8    			direction;
+	s8              running_dir;
 	float   		phase_offset;
 	hall_sample_t 	samples;
 	u32  			sensor_error;
@@ -68,7 +50,7 @@ typedef struct {
 }hall_t;
 
 void hall_sensor_init(void);
-void hall_sensor_clear(void);
+void hall_sensor_clear(s8 direction);
 float hall_sensor_get_theta(void); //return degree
 float hall_sensor_get_speed(void); //return rpm;
 int hall_offset_increase(int inc);
@@ -77,6 +59,7 @@ bool hall_detect_angle_finish(void);
 void hall_detect_angle(s16 angle);
 bool hall_detect_offset_finish(void);
 void hall_detect_offset(s16 angle);
+void hall_set_direction(s8 direction);
 
 #endif /* _HALL_SENSOR_H__ */
 

Applications/foc/motor/motor.c

@@ -12,17 +12,18 @@
 #include "foc/commands.h"
 #include "libs/logger.h"
 #include "bsp/sched_timer.h"
-#include "foc/motor/hall.h"
 #include "foc/core/e_ctrl.h"
 #include "foc/motor/motor_param.h"
 
-static motor_t motor;
+static motor_t motor = {
+	.s_direction = POSITIVE,
+};
 
 void mc_init(void) {
 	adc_init();
 	pwm_3phase_init();
 	samples_init();
-	hall_sensor_init();
+	motor_encoder_init();
 	foc_command_init();
 	PMSM_FOC_CoreInit();
 	sched_timer_enable(SPD_CTRL_MS);
@@ -49,7 +50,7 @@ bool mc_start(u8 mode) {
 		return false;
 	}
 	eCtrl_init(1000, 1000);
-	hall_sensor_clear();
+	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
 	task_udelay(500);
@@ -93,7 +94,7 @@ void mc_use_throttle(void) {
 }
 
 
-void mc_hall_calibrate(s16 vd) {
+void mc_encoder_calibrate(s16 vd) {
 	if (PMSM_FOC_Is_Start()) {
 		return;
 	}
@@ -107,12 +108,12 @@ void mc_hall_calibrate(s16 vd) {
 	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
 	delay_ms(3000);
-	for (int i = 0; i < 50; i++) {
+	for (int i = 0; i < 50000000; i++) {
 		for (s16 angle = 0; angle < 360; angle++) {
 			PMSM_FOC_Set_Angle(angle);
-			if (i > 10 && i < 40) {
-				hall_detect_offset(angle);
-			}
+			
+			motor_encoder_offset(angle);
+			
 			delay_ms(1);
 		}
 	}
@@ -122,7 +123,7 @@ void mc_hall_calibrate(s16 vd) {
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
-	hall_detect_offset_finish();
+	motor_encoder_offset_finish();
 }
 
 bool mc_lock_motor(bool lock) {

Applications/foc/motor/motor.h

@@ -2,6 +2,8 @@
 #define _MOTOR_H__
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
+#include "foc/motor/hall.h"
+#include "foc/motor/encoder.h"
 
 typedef struct {
 	bool   b_start;
@@ -9,17 +11,87 @@ typedef struct {
 	bool   b_ignor_throttle;
 	s16    s_testAngle;
 	s32    s_targetFix;
+	s8     s_direction;
 }motor_t;
 
 void mc_init(void);
 bool mc_start(u8 mode);
 bool mc_stop(void);
-void mc_hall_calibrate(s16 vd);
+void mc_encoder_calibrate(s16 vd);
 bool mc_throttle_released(void);
 bool mc_lock_motor(bool lock);
 void mc_set_spd_torque(s32 target);
 void mc_use_throttle(void);
 
+static __INLINE float motor_encoder_get_angle(void) {
+#ifdef USE_ENCODER_HALL
+	return hall_sensor_get_theta();
+#elif defined (USE_ENCODER_ABI)
+	return encoder_get_theta();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE float motor_encoder_get_speed(void) {
+#ifdef USE_ENCODER_HALL
+	return hall_sensor_get_speed();
+#elif defined (USE_ENCODER_ABI)
+	return encoder_get_speed();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+
+static __INLINE void motor_encoder_init(void) {
+#ifdef USE_ENCODER_HALL
+		hall_sensor_init();
+#elif defined (USE_ENCODER_ABI)
+		encoder_init();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE void motor_encoder_start(s8 direction) {
+#ifdef USE_ENCODER_HALL
+		hall_sensor_clear(direction);
+#elif defined (USE_ENCODER_ABI)
+		encoder_init_clear(direction);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE void motor_encoder_offset(float angle) {
+#ifdef USE_ENCODER_HALL
+		hall_detect_offset(angle);
+#elif defined (USE_ENCODER_ABI)
+		encoder_detect_offset(angle);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE void motor_encoder_offset_finish(void) {
+#ifdef USE_ENCODER_HALL
+		hall_detect_offset_finish();
+#elif defined (USE_ENCODER_ABI)
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE void motor_encoder_set_direction(s8 dir) {
+#ifdef USE_ENCODER_HALL
+			hall_set_direction(dir);
+#elif defined (USE_ENCODER_ABI)
+			encoder_set_direction(dir);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
 
 #endif /* _MOTOR_H__ */
 

Applications/foc/motor/motor_param.h

@@ -1,15 +1,29 @@
 #ifndef _MOTOR_PARAM_H__
 #define _MOTOR_PARAM_H__
 
-#if 1
+#if 0
 #define MOTOR_R   0.33f
 #define MOTOR_Ld   0.00136f
 #define MOTOR_Lq   0.00136f
 #define MOTOR_POLES     2
 #else
+//编码器电机 3505
+#define MOTOR_R 0.08f
+#define MOTOR_Ld 0.000032f
+#define MOTOR_Lq 0.000032f
+#define MOTOR_POLES  10
+/*
 #define MOTOR_R   0.67f//0.33F
 #define MOTOR_Ld   0.00149f//0.00136F
 #define MOTOR_Lq   0.00149f//0.00136F
 #define MOTOR_POLES     2
+*/
 #endif
+/*
+编码器电机 3505
+#define MOTOR_R 0.08f
+#define MOTOR_Ld 0.000032f
+#define MOTOR_Lq 0.000032f
+#define MOTOR_POLES  10
+*/
 #endif /* _MOTOR_PARAM_H__ */

Applications/math/fast_math.h

@@ -34,6 +34,9 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 #define pi_2_degree(d) ((float)(d) * 180.0f / M_PI)
 
 #define INVALID_ANGLE 0x3DFF
+
+
+
 /**
  * A simple low pass filter.
  *

Applications/os/os_types.h

@@ -33,6 +33,20 @@ typedef unsigned char	uint8;
 typedef unsigned short	uint16;
 typedef unsigned int	uint32;
 
+#define NEGATIVE          ((int8_t)-1)
+#define POSITIVE          ((int8_t)1)
+
+#define PHASE_0_DEGREE (0.0f)
+
+#define PHASE_60_DEGREE (60.0f)
+#define PHASE_120_DEGREE (120.0f)
+#define PHASE_180_DEGREE (180.0f)
+#define PHASE_240_DEGREE (240.0f)
+#define PHASE_300_DEGREE (300.0f)
+#define PHASE_360_DEGREE (360.0f)
+
+#define rand_angle(a) {while (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;while (a < 0) a +=PHASE_360_DEGREE;};
+
 extern void *pvPortMalloc( size_t xWantedSize );
 #define os_alloc pvPortMalloc
 extern void vPortFree(void *);

Project/GD32_DEMO.uvoptx

@@ -120,8 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name> ?
-?</Name>
+          <Name>m</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -211,7 +210,28 @@
           <WinNumber>1</WinNumber>
           <ItemText>g_eCtrl,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_encoder,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
+      <MemoryWindow1>
+        <Mm>
+          <WinNumber>1</WinNumber>
+          <SubType>8</SubType>
+          <ItemText>uart_moden_dma_buff</ItemText>
+          <AccSizeX>0</AccSizeX>
+        </Mm>
+      </MemoryWindow1>
+      <MemoryWindow2>
+        <Mm>
+          <WinNumber>2</WinNumber>
+          <SubType>8</SubType>
+          <ItemText>0x20000632</ItemText>
+          <AccSizeX>0</AccSizeX>
+        </Mm>
+      </MemoryWindow2>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>
@@ -256,9 +276,13 @@
       <pMultCmdsp></pMultCmdsp>
       <SystemViewers>
         <Entry>
-          <Name>System Viewer\DMA0</Name>
+          <Name>System Viewer\TIMER2</Name>
           <WinId>35903</WinId>
         </Entry>
+        <Entry>
+          <Name>System Viewer\TIMER3</Name>
+          <WinId>35905</WinId>
+        </Entry>
       </SystemViewers>
     </TargetOption>
   </Target>
@@ -453,6 +477,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>3</GroupNumber>
+      <FileNumber>15</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\foc\motor\encoder.c</PathWithFileName>
+      <FilenameWithoutPath>encoder.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -463,7 +499,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -475,7 +511,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -487,7 +523,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -499,7 +535,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -519,7 +555,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -531,7 +567,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -543,7 +579,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>4</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -563,7 +599,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -575,7 +611,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -587,7 +623,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -599,7 +635,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -611,7 +647,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -623,7 +659,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -635,7 +671,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -647,7 +683,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -659,7 +695,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -671,7 +707,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -683,7 +719,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -695,7 +731,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -707,7 +743,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -719,7 +755,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -731,7 +767,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -741,6 +777,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>6</GroupNumber>
+      <FileNumber>38</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\bsp\enc_intf.c</PathWithFileName>
+      <FilenameWithoutPath>enc_intf.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -751,7 +799,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -763,7 +811,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -775,7 +823,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -787,7 +835,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -799,7 +847,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -811,7 +859,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -831,7 +879,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -843,7 +891,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -855,7 +903,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -875,7 +923,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -887,7 +935,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -899,7 +947,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -911,7 +959,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -923,7 +971,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -935,7 +983,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -947,7 +995,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -959,7 +1007,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -971,7 +1019,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -983,7 +1031,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -995,7 +1043,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1007,7 +1055,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1019,7 +1067,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1031,7 +1079,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1043,7 +1091,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1055,7 +1103,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1075,7 +1123,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1087,7 +1135,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/GD32_DEMO.uvprojx

@@ -463,6 +463,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\motor\hall.c</FilePath>
             </File>
+            <File>
+              <FileName>encoder.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\motor\encoder.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -588,6 +593,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\bsp\sched_timer.c</FilePath>
             </File>
+            <File>
+              <FileName>enc_intf.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\bsp\enc_intf.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

Simulink/init_model.m

@@ -17,6 +17,7 @@ f_adc_curr_ceof = 0.0942;
 f_lpf_idq  = 0.4; %Phase Current LowPass filter
 f_lpf_vdq    = 0.01; %DC link current limit LowPass filter
 
+pll_w = 100;
 %Simulink provider Motor parameters
 n_polePairs  = 4;        % [-] Number of motor pole pairs
 PM           = 0.1688;   % Permanent magnet flux linkage,