hall angle/offset detect

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

Applications/app/nv_storage.c

@@ -14,7 +14,7 @@ mc_config_t *mc_config_get(void) {
 	return &g_config;
 }
 
-void store_hall_table(u16 *hall_table) {
+void store_hall_table(s32 *hall_table) {
 	memcpy((char *)g_config.hall_table, (char *)hall_table, sizeof(g_config.hall_table));
 	store_config();
 }

Applications/app/nv_storage.h

@@ -11,7 +11,7 @@ typedef struct {
 
 typedef struct {
 	s16 hall_offset; /* hall 和A相之间的电角度偏移量 */
-	u16 hall_table[8];
+	s32 hall_table[8];
 	op_cali_t phase_op[3]; /* 三相电流采集的校准系数 */
 	u16 crc16;
 }mc_config_t;
@@ -21,7 +21,7 @@ mc_config_t *mc_config_get(void);
 void config_set_hall_offset(s16 offset);
 void store_config(void);
 void restore_config(void);
-void store_hall_table(u16 *hall_table);
+void store_hall_table(s32 *hall_table);
 void store_hall_offset(s16 offset);
 
 #endif /* _NV_Storage_H__ */

Applications/bsp/bsp.h

@@ -24,10 +24,12 @@
 #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 (20000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
+#define FOC_CTRL_US (1000000u/FOC_PWM_FS)
+
 #define ADC_TRIG_CONV_LATENCY_CYCLES 12.5f
 #define ADC_SAMPLING_CYCLES 7.5f
 

Applications/bsp/timer_count32.c

@@ -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 task_ticks_abs()/12;
+	return task_ticks_abs()/120;
 #endif
 }
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -154,6 +154,7 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 }
 
 static int _g_ctl_count = 0;
+extern int get_hall_stat(int samples);
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
 	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABC;
@@ -182,8 +183,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(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);
+		//plot_3data16(test_motangle>>5, _gFOC_Ctrl.in.s_motAngle>>5, get_hall_stat(9)*60);
+		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);
 

Applications/foc/motor/hall.c

@@ -19,12 +19,12 @@ static u32 _hall_detect_task(void *args);
 static void _hall_init_el_angle(void);
 
 
-#define HALL_PLACE_OFFSET (230 << 19) //(345) //315
+#define HALL_PLACE_OFFSET (220 << 19) //(345) //315
 /* 
 4,5,1,3,2,6,4
 */
 
-static hall_sensor_t _sensor_hander;
+hall_t _sensor_hander;
 
 measure_time_t g_meas_hall = {.exec_max_time = 6,};
 
@@ -48,6 +48,15 @@ static void __inline _hall_put_sample(u32 ticks, s32q19_t angle) {
 	}
 }
 
+static u32 __inline__ _hall_get_angle_ticks(void) {
+	hall_sample_t *s = &_sensor_hander.samples;
+	if (!s->full) {
+		return s->ticks[s->index-1];
+	}else {
+		return s->ticks_sum/SAMPLE_MAX_COUNT;
+	}
+}
+
 static s32q5_t __inline _hall_angle_speed(void){
 	hall_sample_t *s = &_sensor_hander.samples;
 	if (s->ticks_sum == 0) {
@@ -78,7 +87,10 @@ static bool __inline _hall_data_empty(void) {
 static void hall_sensor_default(void) {
 	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
 	_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));
+	for (int i = 0; i < 8; i++) {
+		_sensor_hander.angle_table[i] = S32Q19(mc_config_get()->hall_table[i]);
+	}
+	_sensor_hander.manual_angle = 0x3FF;
 	_hall_init_el_angle();
 	hall_debug_log();
 }
@@ -107,19 +119,21 @@ static u32 _hall_detect_task(void *args) {
 }
 
 s16q5_t hall_sensor_get_theta(void){
-	u32 us_now = timer_count32_get();
-	u32 us_delta = timer_count32_delta(us_now, _sensor_hander.estimate_time_ticks);
-	_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 angle_add = _sensor_hander.delta_angle_ts;
+	if (_sensor_hander.comp_count > 0) {
+		_sensor_hander.comp_count--;
+		angle_add += _sensor_hander.angle_comp_ts;
+	}
+	_sensor_hander.estimate_delta_angle += angle_add;
+
+	s32q19_t el_angle = _sensor_hander.estimate_delta_angle;
+	if (el_angle > _sensor_hander.next_delta_angle) {
+		el_angle = _sensor_hander.next_delta_angle;
 	}
-	s32q19_t el_angle = 0;
 	if (_sensor_hander.direction == POSITIVE) {
-		el_angle = _sensor_hander.estimate_el_angle + _sensor_hander.estimate_delta_angle;
+		el_angle = _sensor_hander.estimate_el_angle + el_angle;
 	}else {
-		el_angle = _sensor_hander.estimate_el_angle - _sensor_hander.estimate_delta_angle;
+		el_angle = _sensor_hander.estimate_el_angle - el_angle;
 	}
 
 	rand_angle(el_angle);
@@ -146,7 +160,53 @@ s32 *hall_get_table(void) {
 	return _sensor_hander.angle_table;
 }
 
+static float sin_hall[8];
+static float cos_hall[8];
+static int hall_iterations[8];
+void hall_detect_angle(s16 angle) {
+	_sensor_hander.manual_angle = (angle);
+	int hall = get_hall_stat(HALL_READ_TIMES);
+	float s, c;
+	normal_sincosf(degree_2_pi(angle), &s, &c);
+	sin_hall[hall] += s;
+	cos_hall[hall] += c;
+	hall_iterations[hall]++;
+}
+
+bool hall_detect_angle_finish(void) {
+	int fails = 0;
+	for(int i = 0;i < 8;i++) {
+		if (hall_iterations[i] > 30) {
+			float ang = pi_2_degree(atan2f(sin_hall[i], cos_hall[i]));
+			fast_norm_angle(&ang);
+			_sensor_hander.angle_table[i] = (ang);
+		} else {
+			_sensor_hander.angle_table[i] = S32Q19(0x3FF);
+			fails++;
+		}
+	}
+	if (fails == 2) {
+		store_hall_table(_sensor_hander.angle_table);
+	}
+	memset(sin_hall, 0, sizeof(sin_hall));
+	memset(cos_hall, 0, sizeof(cos_hall));
+	memset(hall_iterations, 0, sizeof(hall_iterations));	
+	return fails == 2;
+}
 
+void hall_detect_offset(s16 angle) {
+	_sensor_hander.manual_angle = (angle);
+}
+
+bool hall_detect_offset_finish(void) {
+	int fails = 0;
+	for(int i = 0;i < 8;i++) {
+		if (hall_iterations[i] > 20) {
+			_sensor_hander.angle_table[i] = _sensor_hander.angle_table[i] / hall_iterations[i];
+		}
+	}
+	return (fails == 2);
+}
 static void _hall_init_el_angle(void) {
 	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
 #ifdef USE_DETECTED_ANGLE
@@ -188,7 +248,6 @@ static void _hall_init_el_angle(void) {
 	rand_angle(_sensor_hander.measured_el_angle);
   	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
 	_sensor_hander.hall_ticks = timer_count32_get();
-	_sensor_hander.estimate_time_ticks = timer_count32_get();
 }
 
 static __inline__ s32 _get_angle(u8 state, s32 added) {
@@ -327,7 +386,6 @@ static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
 }
 #endif
 
-extern s16 _g_hall_angle;
 void HALL_IRQHandler(void) {
 	time_measure_start(&g_meas_hall);
 	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
@@ -339,6 +397,7 @@ void HALL_IRQHandler(void) {
 	if (theta_now == 0xFFFFFFFF) {
 		return;
 	}
+	//plot_2data16(hall_stat_now*60, _sensor_hander.manual_angle);
 	//获取两次中断的时间间隔，估计速度
 	u32 delta_us = timer_count32_delta(hall_ticks_now, _sensor_hander.hall_ticks);
 	if (delta_us == 0) {
@@ -356,41 +415,34 @@ void HALL_IRQHandler(void) {
 	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%,认为不稳定，需要使用即时速度估计转子位置
+	if (delta_el_speed*100/prev_imme_el_speed >= 40) { //即时速度增加40%,认为不稳定，需要使用即时速度估计转子位置
 		_sensor_hander.trns_detect = true;
 	}else {
 		_sensor_hander.trns_detect = false;
 	}
 	_hall_put_sample(delta_us, delta_angle);
 	os_disable_irq();
-	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
-	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 estimate_delta_angle =  _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
+		//plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.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;
-		}
+		_sensor_hander.comp_count = 0;//(s32)(delta_us/FOC_CTRL_US)/2;
+		_sensor_hander.angle_comp_ts = 0;//estimate_delta_angle/_sensor_hander.comp_count;
+		_sensor_hander.estimate_el_angle = theta_now;
 	}else {
-		est_el_speed = _sensor_hander.el_speed;
+		_sensor_hander.comp_count = 0;
+		_sensor_hander.angle_comp_ts = 0;
+		_sensor_hander.estimate_el_angle = theta_now;
 	}
-#else
-	est_el_speed = _sensor_hander.el_speed;
-#endif
-	//s32q5
-	_sensor_hander.estimate_el_speed = est_el_speed;
+	_sensor_hander.estimate_delta_angle = 0;
+	_sensor_hander.delta_angle_ts = next_delta_angle/(delta_us/FOC_CTRL_US);
 	_sensor_hander.next_delta_angle = next_delta_angle;
 	_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.el_speed = _hall_angle_speed();	//s32q5
 	_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 2
+#define SAMPLE_MAX_COUNT 16
 
 typedef struct {
 	u32        	ticks[SAMPLE_MAX_COUNT];
@@ -38,24 +38,26 @@ typedef struct {
 }hall_sample_t;
 
 typedef struct {
-	s32q19_t estimate_el_angle; //60度区间内的估计电角度
-	s32q19_t estimate_delta_angle;	
-	u32   estimate_time_ticks;	
-	s32q19_t   measured_el_angle; //hall测量到的电角度
-	s32q19_t next_delta_angle;
-	s32q5_t estimate_el_speed;
-	s32q5_t immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
-	s32q5_t el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
-	s32q5_t  rpm;        		//当前的电速度, 单位：RPM
-	bool  trns_detect;     //速度变化超过阈值
-	u8    hall_stat;
-	u32   hall_ticks;
-	s8    direction;
-	s32q19_t   phase_offset;
-	hall_sample_t samples;
-	u32  sensor_error;
-	s32q19_t  angle_table[8];
-}hall_sensor_t;
+	s32q19_t 		estimate_el_angle; //60度区间内的估计电角度
+	s32q19_t 		estimate_delta_angle;		
+	s32q19_t   		measured_el_angle; //hall测量到的电角度
+	s32q19_t 		next_delta_angle;
+	s32q19_t        delta_angle_ts;    //每个控制周期角度的增加量，通过上次的速度计算得到
+	s32q19_t        angle_comp_ts;     //每个控制周期角度增加的补偿量，主要用在加减速，hall角度和60度有偏差的情况
+	int             comp_count;
+	s32q5_t 		immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
+	s32q5_t 		el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
+	s32q5_t  		rpm;        		//当前的电速度, 单位：RPM
+	bool  			trns_detect;     //速度变化超过阈值
+	u8    			hall_stat;
+	u32   			hall_ticks;	
+	s8    			direction;
+	s32q19_t   		phase_offset;
+	hall_sample_t 	samples;
+	u32  			sensor_error;
+	s16q5_t         manual_angle;
+	s32q19_t  		angle_table[8];
+}hall_t;
 
 void hall_sensor_init(void);
 void hall_sensor_clear(void);
@@ -63,6 +65,10 @@ s16q5_t hall_sensor_get_theta(void); //return degree
 s32q5_t hall_sensor_get_speed(void); //return rpm;
 int hall_offset_increase(int inc);
 s32 *hall_get_table(void);
+bool hall_detect_angle_finish(void);
+void hall_detect_angle(s16 angle);
+bool hall_detect_offset_finish(void);
+void hall_detect_offset(s16 angle);
 
 #endif /* _HALL_SENSOR_H__ */
 

Applications/foc/motor/motor.c

@@ -76,7 +76,7 @@ bool mc_stop(void) {
 	gpio_led2_enable(false);
 	return true;
 }
-s16 _g_hall_angle = 0;
+
 void mc_hall_calibrate(s16 vd) {
 	if (PMSM_FOC_Is_Start()) {
 		return;
@@ -94,8 +94,8 @@ void mc_hall_calibrate(s16 vd) {
 	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(5);
+			hall_detect_offset(angle);
+			delay_ms(1);
 		}
 	}
 	delay_ms(500);
@@ -104,6 +104,7 @@ void mc_hall_calibrate(s16 vd) {
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
+	hall_detect_offset_finish();
 }
 
 bool mc_lock_motor(bool lock) {

Applications/math/fast_math.h

@@ -1,5 +1,6 @@
 #ifndef _Fast_Math_H__
 #define _Fast_Math_H__
+#include <arm_math.h>
 
 // Constants
 #define ONE_BY_SQRT3			(0.57735026919f) // 1/sqrt(3)
@@ -17,8 +18,21 @@
 #define SQ(x) ((x)*(x))
 #endif
 void fast_sincos(float angle, float *sin, float *cos);
-void fast_norm_angle(float *angle);
-void normal_sincosf(float angle, float *sin, float *cos);
+static void fast_norm_angle(float *angle) {
+	*angle = fmodf(*angle, 360.0f);
+
+	if (*angle < 0.0f) {
+		*angle += 360.0f;
+	}
+}
+
+static void normal_sincosf(float angle, float *sin, float *cos) {
+	*sin = arm_sin_f32(angle);
+	*cos = arm_cos_f32(angle);
+}
+#define degree_2_pi(d) ((float)d * M_PI / 180.0f)
+#define pi_2_degree(d) ((float)d * 180.0f / M_PI)
+
 
 /**
  * A simple low pass filter.

Applications/math/fix_math.h

@@ -29,6 +29,10 @@ typedef int32_t   s32q19_t;
 #define S32Q14(A) (signed int)((A) * 16384.0F)
 #define S32Q14toF(A) ((float)(A)/16384.0f)
 
+#define S32Q19(A) (signed int)((A) * (16384.0F * 32.0F))
+#define S32Q19toF(A) ((float)(A)/(16384.0F * 32.0F))
+
+
 #define S32Q14_MUL(a, b) (((a)*(b)) >>14)
 
 #define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)

Project/GD32_DEMO.uvoptx

@@ -184,6 +184,21 @@
           <WinNumber>1</WinNumber>
           <ItemText>g_meas_foc,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>_sensor_hander,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>hall_iterations,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_config,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>