开环hall OK，但是后陆续后优化hall跳变

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

Applications/app/app.c

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

Applications/bsp/bsp.h

@@ -24,7 +24,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 (20000u)
+#define FOC_PWM_FS (16000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 

Applications/bsp/mc_hall_gpio.c

@@ -15,7 +15,7 @@ void mc_hall_init(void){
 	_gpio_irq_enable();
 }
 
-#if 0
+#if 1
 int get_hall_stat(int samples) {
 	int h1 = 0, h2 = 0, h3 = 0;
 	int tres = (samples + 1) / 2;

Applications/bsp/timer_count32.c

@@ -68,7 +68,7 @@ static void init_slave_timer(void) {
 }
 
 static __inline__ u32 _timer_count32_get(void) {
-#if 1
+#if 0
 	u16 high;
 	u16 low;
 	do {
@@ -77,7 +77,7 @@ static __inline__ u32 _timer_count32_get(void) {
 	}while((high != TIMER_CNT(SLAVE)) /*|| (low != TIMER_CNT(MASTER))*/);
 	return (high<<16) | low;
 #else
-	return cpu_counts_abs();
+	return task_ticks_abs()/12;
 #endif
 }
 

Applications/foc/commands.c

@@ -49,7 +49,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			bool success;
 			foc_start_cmd_t *scmd = (foc_start_cmd_t *)command->data;
 			if (scmd->start_stop == Foc_Start) {
-				success = mc_start(TRQ_MODE);
+				success = mc_start(OPEN_MODE);
 			}else if (scmd->start_stop == Foc_Stop) {
 				success = mc_stop();
 			}

Applications/foc/core/PMSM_FOC_Core.c

@@ -119,7 +119,7 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.in.n_modulation = S16Q14(0.95f);
 	_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
 	_gFOC_Ctrl.out.f_vdqRation = S16Q14(0.9f);
-	_gFOC_Ctrl.in.s_manualAngle = 0xFFFF;
+	_gFOC_Ctrl.in.s_manualAngle = 0x3D00;
 
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[0], 1);
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[1], 1);
@@ -129,12 +129,15 @@ void PMSM_FOC_CoreInit(void) {
 	PMSM_FOC_Reset_PID();
 }
 
+s16 test_motangle = 0;
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
-	if (_gFOC_Ctrl.in.s_manualAngle != 0xFFFF) {
+	if (_gFOC_Ctrl.in.s_manualAngle != 0x3D00) {
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
+		test_motangle = hall_sensor_get_theta();
 	}else {
 		_gFOC_Ctrl.in.s_motAngle = hall_sensor_get_theta();
 	}
+
 	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
 	//sample current
 	phase_current_get(_gFOC_Ctrl.in.s_iABC);
@@ -179,8 +182,8 @@ void PMSM_FOC_Schedule(void) {
 
 	if (_g_ctl_count++ % 10 == 0) {
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-		plot_2data16(hall_sensor_get_theta()>>5, _gFOC_Ctrl.in.s_motAngle>>5);
-		//plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_iABC[2]);
+		plot_2data16(test_motangle>>5, _gFOC_Ctrl.in.s_motAngle>>5);
+		//plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_motAngle>>5);
 	}
 	phase_current_point(&_gFOC_Ctrl.out);
 
@@ -234,6 +237,7 @@ void PMSM_FOC_Start(u8 nCtrlMode) {
 		return;
 	}
 	PMSM_FOC_CoreInit();
+	_gFOC_Ctrl.in.n_ctlMode = nCtrlMode;
 	_gFOC_Ctrl.in.b_motEnable = true;
 }
 

Applications/foc/motor/hall.c

@@ -13,13 +13,13 @@
 
 //#define USE_DETECTED_ANGLE 1
 
-#define HALL_READ_TIMES 3
+#define HALL_READ_TIMES 9
 
 static u32 _hall_detect_task(void *args);
 static void _hall_init_el_angle(void);
 
 
-#define HALL_PLACE_OFFSET (315 << 19) //(345) //315
+#define HALL_PLACE_OFFSET (230 << 19) //(345) //315
 /* 
 4,5,1,3,2,6,4
 */
@@ -77,7 +77,7 @@ static bool __inline _hall_data_empty(void) {
 
 static void hall_sensor_default(void) {
 	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
-	_sensor_hander.phase_offset = 0;//HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
 	memcpy((char *)_sensor_hander.angle_table, (char *)mc_config_get()->hall_table, sizeof(_sensor_hander.angle_table));
 	_hall_init_el_angle();
 	hall_debug_log();
@@ -112,16 +112,19 @@ s16q5_t hall_sensor_get_theta(void){
 	_sensor_hander.estimate_time_ticks = us_now;
 	s32q19_t angle_step = _sensor_hander.estimate_el_speed * us_2_s(us_delta);
 	_sensor_hander.estimate_delta_angle += angle_step;
-
+	if (_sensor_hander.estimate_delta_angle >= _sensor_hander.next_delta_angle) {
+		_sensor_hander.estimate_delta_angle = _sensor_hander.next_delta_angle;
+	}
+	s32q19_t el_angle = 0;
 	if (_sensor_hander.direction == POSITIVE) {
-		_sensor_hander.estimate_el_angle += angle_step;
+		el_angle = _sensor_hander.estimate_el_angle + _sensor_hander.estimate_delta_angle;
 	}else {
-		_sensor_hander.estimate_el_angle -= angle_step;
+		el_angle = _sensor_hander.estimate_el_angle - _sensor_hander.estimate_delta_angle;
 	}
 
-	rand_angle(_sensor_hander.estimate_el_angle);
+	rand_angle(el_angle);
 
-	return (_sensor_hander.estimate_el_angle >> 14);
+	return (el_angle >> 14);
 }
 
 
@@ -257,7 +260,9 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 			_sensor_hander.sensor_error ++;
 			return 0xFFFFFFFF;
 	}
-	rand_angle(theta_now);
+	if (theta_now != 0xFFFFFFFF) {
+		rand_angle(theta_now);
+	}
 	return theta_now;
 }
 
@@ -303,7 +308,6 @@ static __inline u8 _next_hall(u8 hall_now) {
 		default: //not reached here
 			return STATE_1;
 	}
-
 } 
 
 
@@ -330,8 +334,6 @@ void HALL_IRQHandler(void) {
 	u8 hall_stat_prev = _sensor_hander.hall_stat;
 	u32 hall_ticks_now = timer_count32_get();	
 
-	//plot_2data16(hall_stat_now*50, _g_hall_angle);
-
 	/*获取当前转子角度*/
 	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
 	if (theta_now == 0xFFFFFFFF) {
@@ -351,7 +353,7 @@ void HALL_IRQHandler(void) {
 	s32 next_delta_angle = delta_angle;
 #endif
 	s32 delta_time = us_2_s(delta_us);
-	s32 prev_imme_el_speed = _sensor_hander.immediately_el_speed;
+	s32 prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
 	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
 	s32 delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
 	if (delta_el_speed*100/prev_imme_el_speed >= 20) { //即时速度增加10%,认为不稳定，需要使用即时速度估计转子位置
@@ -362,21 +364,35 @@ void HALL_IRQHandler(void) {
 	_hall_put_sample(delta_us, delta_angle);
 	os_disable_irq();
 	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
-	_sensor_hander.estimate_delta_angle = _sensor_hander.estimate_delta_angle - delta_angle;
-	/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
-	s32q14_t est_el_speed = ((next_delta_angle - _sensor_hander.estimate_delta_angle)<<9)/delta_angle * _sensor_hander.el_speed;
-	if (_sensor_hander.trns_detect) { //s32q14
-		est_el_speed = ((next_delta_angle - _sensor_hander.estimate_delta_angle)<<9)/delta_angle * _sensor_hander.immediately_el_speed;
+	s32q14_t est_el_speed = 0;
+#if 0	
+	
+	if (_sensor_hander.samples.full) {
+		s32 estimate_delta_angle = _sensor_hander.estimate_delta_angle - _sensor_hander.next_delta_angle;
+		//plot_1data16(estimate_delta_angle>>19);
+		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
+		s32 delta_ration = estimate_delta_angle/(_sensor_hander.next_delta_angle >> 5); //q5
+		est_el_speed = (delta_ration * _sensor_hander.el_speed)>>5 + _sensor_hander.el_speed;
+		if (_sensor_hander.trns_detect) { //s32q14
+			est_el_speed = (delta_ration * _sensor_hander.immediately_el_speed)>>5 + _sensor_hander.immediately_el_speed;
+		}
+	}else {
+		est_el_speed = _sensor_hander.el_speed;
 	}
+#else
+	est_el_speed = _sensor_hander.el_speed;
+#endif
 	//s32q5
-	_sensor_hander.estimate_el_speed = est_el_speed >> 9;
+	_sensor_hander.estimate_el_speed = est_el_speed;
 	_sensor_hander.next_delta_angle = next_delta_angle;
-	//_sensor_hander.measured_el_angle = theta_now;
+	_sensor_hander.measured_el_angle = theta_now;
+	_sensor_hander.estimate_el_angle = theta_now;
+	_sensor_hander.estimate_delta_angle = 0;
 	os_enable_irq();
 	_sensor_hander.hall_stat = hall_stat_now;
 	_sensor_hander.hall_ticks = hall_ticks_now;
 	_sensor_hander.rpm = _sensor_hander.el_speed / 360 * 60; //s32q5
-	
+	//plot_3data16(_sensor_hander.rpm >> 5, (_sensor_hander.immediately_el_speed/6) >> 5, (_sensor_hander.estimate_el_speed/6)>>5);
 	time_measure_end(&g_meas_hall);
 }
 

Applications/foc/motor/hall.h

@@ -26,7 +26,7 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
-#define SAMPLE_MAX_COUNT 3
+#define SAMPLE_MAX_COUNT 2
 
 typedef struct {
 	u32        	ticks[SAMPLE_MAX_COUNT];

Applications/foc/motor/motor.c

@@ -43,6 +43,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
 		return false;
 	}
+	hall_sensor_clear();
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
 	task_udelay(500);
@@ -87,14 +88,14 @@ void mc_hall_calibrate(s16 vd) {
 	pwm_start();
 	adc_start_convert();
 	phase_current_wait_cali();
+	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
-	delay_ms(1000);
-
+	delay_ms(3000);
 	for (int i = 0; i < 50; i++) {
 		for (s16 angle = 0; angle < 360; angle++) {
 			PMSM_FOC_Set_Angle(angle);
 			_g_hall_angle = angle;
-			delay_ms(1);
+			delay_ms(5);
 		}
 	}
 	delay_ms(500);
@@ -183,7 +184,7 @@ void TIMER_UP_IRQHandler(void){
 	time_measure_start(&g_meas_timeup);
 }
 
-measure_time_t g_meas_foc = {.intval_max_time = 50,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
+measure_time_t g_meas_foc = {.exec_max_time = 20, .intval_max_time = 50,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
 #define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
 

Project/GD32_DEMO.uvoptx

@@ -120,7 +120,6 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>?</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>