优化hall角度，通过观察电流找到最佳hall和U相的偏移角度

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

Application/FOC/foc.c

@@ -29,7 +29,7 @@ void foc_init(void) {
 	
 	task_start(foc_measure_task, 0);
 
-	//hall_sensor_calibrate(2.0f, mFOC.hall_table);
+	//hall_sensor_calibrate(8.0f, mFOC.hall_table);
 }
 
 static void foc_defulat_value(void){
@@ -45,6 +45,7 @@ void foc_clear(void) {
 	PWM_Stop();
 	mFOC.mosGate = false;
 	foc_defulat_value();
+	hall_sensor_init();
 }
 
 FOCState FOC_STM_State(void){
@@ -154,7 +155,7 @@ void foc_overide_set_vdq(float d, float q){
 static u32 foc_measure_task(void){
 	vbus_sample_voltage();
 	ntc_sensor_sample();
-	mFOC.vbus = vbus_get_voltage();
+	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
 	wdog_reload();
 	return 1;
 }

Application/FOC/foc_task.c

@@ -87,9 +87,9 @@ static void __inline Debug_Log(motor_foc_t *foc){
 #if 1
 	static int count;
 	if (count++ % 5== 0) {
-		//printf("$%d %d %d %d;", foc->motor_s.angle, (int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
-		//	(int)(foc->current_samp.Ic * 1000.0f));
-		printf("$%d;", (int)hall_sensor_get_speed());
+		printf("$%d %d %d;",(int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
+			(int)(foc->current_samp.Ic * 1000.0f));
+		//printf("$%d;", (int)hall_sensor_get_speed());
 	}
 #endif	
 }

Application/FOC/hall_sensor.c

@@ -7,6 +7,15 @@
 
 #define HALL_READ_TIMES 3
 /* 
+* 测量HALL_PLACE_OFFSET通用方式就是ST推荐的通过外力带动电机，
+* 测量电机U相反电动势和hall 1的上升沿之间的差值
+* 这里使用的是先通过hall_sensor_calibrate测量hall1，2，3，4，5，6
+* 对应的角度(偏差比较大),然后启动电机，让HALL_PLACE_OFFSET
+* 从0开始增加，每增加1度观察电机电流(看直流电源),
+* 找到一个电机平稳转动并且电流最小的角度作为HALL_PLACE_OFFSET
+*/
+#define HALL_PLACE_OFFSET 207.0f
+/* 
 100
 101
 001
@@ -16,8 +25,7 @@
 4,5,1,3,2,6,4
 */
 static u16 _hall_table[] = {0xFFFF, 292/*1*/, 54/*2*/, 1/*3*/, 180/*4*/, 229/*5*/, 115/*6*/, 0xFFFF};
-//static u16 _hall_table[] = {0xFFFF, 292/*1*/, 65/*2*/, 7/*3*/, 185/*4*/, 250/*5*/, 116/*6*/, 0xFFFF};
-
+//static u16 _hall_table[] = {0xFFFF, 257/*1*/, 36/*2*/, 344/*3*/, 159/*4*/, 222/*5*/, 88/*6*/, 0xFFFF};
 static hall_t _hall;
 static hall_sample_t h_samples;
 
@@ -60,6 +68,9 @@ static float __inline _hall_avg_speed(void){
 void hall_sensor_init(void) {
 	memset(&_hall, 0, sizeof(_hall));
 	read_hall(_hall.state, _hall.theta);
+#ifdef HALL_PLACE_OFFSET	
+	_hall.phase_offset = HALL_PLACE_OFFSET;
+#endif
 }
 
 float hall_sensor_get_theta(void){
@@ -101,7 +112,7 @@ int hall_sensor_calibrate(float current, u16 *hall_table){
 	memset(sin_hall, 0, sizeof(sin_hall));
 	memset(cos_hall, 0, sizeof(cos_hall));
 	memset(hall_iterations, 0, sizeof(hall_iterations));
-
+	task_udelay(50 * 1000);
 	// Forwards
 	for (int i = 0;i < 3;i++) {
 		for (int j = 0;j < 360;j++) {
@@ -148,12 +159,107 @@ int hall_sensor_calibrate(float current, u16 *hall_table){
 	return fails == 2;	
 }
 
-
+#ifdef HALL_PLACE_OFFSET
 void hall_sensor_handler(void) {
 	u8 state_now = get_hall_stat(HALL_READ_TIMES);
 	float theta_now = _hall_table[state_now];
 	u8 state_prev = _hall.state;
+
+	if (!_hall.working) {		
+		if(theta_now != 0xFFFF) {
+			_hall.working = true;
+			_hall.state = state_now;
+			_hall.theta = theta_now;
+			_hall.ticks = task_ticks_abs();
+		}
+		return;
+	}
+
+	switch (state_now) {
+		case STATE_1:
+			if (state_prev == STATE_5) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset + 60.0f;
+			}else if (state_prev == STATE_3) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset + 120.0f;
+			}
+			break;
+		case STATE_2:
+			if (state_prev == STATE_3) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset + 180.0f;
+			}else if (state_prev == STATE_6) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset + 240.0f;
+			}
+			break;
+		case STATE_3:
+			if (state_prev == STATE_1) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset + 120.0f;
+			}else if (state_prev == STATE_2) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset + 180.0f;
+			}
+			break;
+		case STATE_4:
+			if (state_prev == STATE_6) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset + 300.0f;
+			}else if (state_prev == STATE_5) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset;
+			}
+			break;
+		case STATE_5:
+			if (state_prev == STATE_4) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset;
+			}else if (state_prev == STATE_1) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset + 60.0f;
+			}
+			break;
+		case STATE_6:
+			if (state_prev == STATE_2) {
+				_hall.direction = POSITIVE;
+				theta_now = _hall.phase_offset + 240.0f;
+			}else if (state_prev == STATE_4) {
+				_hall.direction = NEGATIVE;
+				theta_now = _hall.phase_offset + 300.0f;
+			}
+			break;
+		default:
+			return;
+	}
+
+
+	float delta_time = tick_2_s(delta_ticks(_hall.ticks));
+	if (delta_time == 0.0f) { //may be errors ???
+		return;
+	}
+	float delta_theta = 60.0f;
+	_hall_put_sample(delta_theta, delta_ticks(_hall.ticks));
+	if (!h_samples.full) {
+		_hall.degree_per_s = delta_theta / delta_time;
+	}else {
+		_hall.degree_per_s = _hall_avg_speed();
+	}
+	_hall.ticks = task_ticks_abs();
+	_hall.theta = theta_now;
+	_hall.state = state_now;
+	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
+	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
+}
+
+#else
+void hall_sensor_handler1(void) {
+	u8 state_now = get_hall_stat(HALL_READ_TIMES);
+	float theta_now = _hall_table[state_now];
+	u8 state_prev = _hall.state;
 	float theta_prev = _hall.theta;
+
 	if (!_hall.working) {		
 		if(theta_now != 0xFFFF) {
 			_hall.working = true;
@@ -164,7 +270,7 @@ void hall_sensor_handler(void) {
 		return;
 	}
 	//printf("hall %d, %d\n", state_now, state_prev);
-
+	//{0xFFFF, 257/*1*/, 36/*2*/, 344/*3*/, 159/*4*/, 222/*5*/, 88/*6*/, 0xFFFF};
 	float delta_theta = 360.0f;
 	switch (state_now) {
 		case STATE_1:
@@ -245,5 +351,5 @@ void hall_sensor_handler(void) {
 	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
 	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
 }
-
+#endif
 

Application/FOC/hall_sensor.h

@@ -26,6 +26,7 @@ typedef struct {
 	bool  working;
 	s8    direction;
 	float degree_per_s; //ÿÃë¶È£¬ ext: 10¶È/s
+	float phase_offset;
 }hall_t;
 
 #define SAMPLE_MAX_COUNT 12

Application/FOC/ramp_ctrl.c

@@ -1,6 +1,6 @@
 #include "ramp_ctrl.h"
 
-#define RAMP_INTVAL 1 //ms
+#define RAMP_INTVAL 5 //ms
 void ramp_timer_handler(timer_t *timer);
 
 void ramp_ctrl_init(ramp_t *ramp, float start, float final, u32 duration_ms){

Application/FOC/vbus_sensor.c

@@ -4,7 +4,7 @@
 static vbus_t _vbus;
 void vbus_sensor_init(void){
 	_vbus.voltage_avg = 0;
-	_vbus.avg_count = 16;
+	_vbus.avg_count = 5;
 	HAL_ADC1_ChanConfig(VBUS_SENSOR_ADC_CHANNEL);
 	vbus_sample_voltage();
 }

Application/Math/fast_math.h

@@ -10,6 +10,19 @@
 void fast_sincos(float angle, float *sin, float *cos);
 void fast_norm_angle(float *angle);
 void normal_sincosf(float angle, float *sin, float *cos);
+/**
+ * A simple low pass filter.
+ *
+ * @param value
+ * The filtered value.
+ *
+ * @param sample
+ * Next sample.
+ *
+ * @param filter_constant
+ * Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
+ */
+#define LowPass_Filter(value, sample, filter_constant)	(value -= (filter_constant) * ((value) - (sample)))
 
 #endif /* _Fast_Math_H__ */