code update

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

Applications/app/app.c

@@ -16,6 +16,7 @@ void app_start(void){
 	set_log_level(MOD_SYSTEM, L_debug);
 	can_message_init();
 	foc_init();
+	log_start_task();
 	co_task_create(_app_low_task, NULL, 512);
 	co_task_schedule();
 }
@@ -26,13 +27,15 @@ static void _can_report_info(void) {
 	can_report_phase_current(0x45, F2I(s->Ia * 1000), F2I(s->Ib * 1000), F2I(s->Ic * 1000));
 }
 
+
 static void _app_low_task(void *args) {
 	while(1) {
 		wdog_reload();
 		_can_report_info();
 		sys_debug("foc exec time %d, intval %d, max %d, error %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time, g_meas_foc.exec_max_error_time, g_meas_foc.intval_time_error);
-		//sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
-		co_task_delay(500);
+		sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
+		sys_debug("vbus voltage: %f\n", foc_get_vbus_voltage());
+		co_task_delay(1000);
 	}
 }
 

Applications/bsp/mc_hall_gpio.h

@@ -2,6 +2,9 @@
 #define _MC_HALL_H__
 #include "bsp/bsp.h"
 
+//51ms, 77.6
+//52.2ms 
+//25.2
 #define HALL_GPOI_CLK RCU_GPIOB
 #define HALL_1_PIN GPIO_PIN_6
 #define HALL_1_GROUP GPIOB

Applications/bsp/pwm.c

@@ -95,8 +95,8 @@ static void _init_pwm_timer(void) {
     timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE;
     timer_ocintpara.ocpolarity   = TIMER_OC_POLARITY_LOW;
     timer_ocintpara.ocnpolarity  = TIMER_OCN_POLARITY_LOW;
-    timer_ocintpara.ocidlestate  = TIMER_OC_IDLE_STATE_LOW;
-    timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
+    timer_ocintpara.ocidlestate  = TIMER_OC_IDLE_STATE_HIGH;
+    timer_ocintpara.ocnidlestate = TIMER_OC_IDLE_STATE_HIGH;
 
     timer_channel_output_config(timer,TIMER_CH_0,&timer_ocintpara);
     timer_channel_output_pulse_value_config(timer,TIMER_CH_0,half_period);

Applications/bsp/timer_count32.c

@@ -1,6 +1,6 @@
 #include "bsp/bsp.h"
 #include "timer_count32.h"
-
+#include "os/co_task.h"
 static void init_master_timer(void);
 static void init_slave_timer(void);
 
@@ -68,6 +68,7 @@ static void init_slave_timer(void) {
 }
 
 static __inline__ u32 _timer_count32_get(void) {
+#if 0
 	u16 high;
 	u16 low;
 	do {
@@ -75,6 +76,9 @@ static __inline__ u32 _timer_count32_get(void) {
 		low = TIMER_CNT(MASTER);
 	}while((high != TIMER_CNT(SLAVE)) /*|| (low != TIMER_CNT(MASTER))*/);
 	return (high<<16) | low;
+#else
+	return cpu_counts_abs();
+#endif
 }
 
 

Applications/bsp/timer_count32.h

@@ -20,7 +20,7 @@ void time_measure_start(measure_time_t *m);
 void time_measure_end(measure_time_t *m);
 
 #define COUNT_CLK (10 * 1000000)
-#define COUNT_2_US(c) (c/10)
+#define COUNT_2_US(c) (c/120)
 
 static __inline__ u32 timer_count32_getus(u32 now, u32 count) {
 	u32 delta = now - count;

Applications/foc/foc_cmd.c

@@ -41,22 +41,7 @@ static void foc_cmd_task(void *args) {
 	}
 }
 
-static void do_hall_calibrate(u8 can_addr, float vq) {
-	sys_debug("cali hall phase, %d\n", g_foc.state);
-	if (g_foc.state == IDLE) {
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 1);
-		pwm_turn_on_low_side();
-		delay_ms(10);
-		foc_pwm_start(true);
-		pwm_enable_channel();
-		adc_start_insert_convert();
-		int result = hall_sensor_calibrate(vq, NULL);
-		sys_debug("hall phase cali %d\n", result);
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Hall_Phase_Cali_Result), result);
-	}else {
-		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 0);
-	}
-}
+
 
 static void process_foc_command(foc_cmd_body_t *command) {
 	sys_debug("command %d\n", command->cmd);
@@ -73,9 +58,6 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)f);
 			break;
 		}
-		case Foc_Cali_Hall_Phase:
-			do_hall_calibrate(command->can_src, decode_u32((u8 *)command->data));
-			break;
 		case Foc_Cali_Hall_Offset:
 		{
 			int offset = 0;

Applications/foc/foc_core.c

@@ -230,8 +230,8 @@ void mc_phase_current_irq(void) {
 	time_measure_start(&g_meas_foc);
 	FOC_Fast_Task(&g_foc);
 	time_measure_end(&g_meas_foc);
-	if (g_meas_foc.intval_time < 1 || g_meas_foc.intval_time > 34) {
-		g_meas_foc.intval_time_error ++;
+	if (g_meas_foc.intval_time < 33 || g_meas_foc.intval_time > 33) {
+		//log_int_value(1, g_meas_foc.intval_time);
 	}
 }
 

Applications/foc/hall_sensor.c

@@ -15,18 +15,11 @@
 #define HALL_READ_TIMES 3
 
 static void _hall_detect_task(void *args);
+static void _hall_init_el_angle(void);
 
-#define PWM_T  (0.00003333f)
+#define PWM_T  (0.000033f)
 
-/* 
-* 测量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 210
+#define HALL_PLACE_OFFSET (199)
 /* 
 100
 101
@@ -36,10 +29,8 @@ static void _hall_detect_task(void *args);
 110
 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[] = {THETA_NONE, 121/*1*/, 240/*2*/, 190/*3*/, 13/*4*/, 58/*5*/, 306/*6*/, THETA_NONE};
-static hall_t _hall;
 
+static hall_sensor_t _sensor_hander;
 
 measure_time_t g_meas_hall = {.exec_max_time = 6,};
 
@@ -48,7 +39,7 @@ measure_time_t g_meas_hall = {.exec_max_time = 6,};
 
 
 static void __inline _hall_put_sample(u32 ticks) {
-	hall_sample_t *s = &_hall.samples;
+	hall_sample_t *s = &_sensor_hander.samples;
 	s->ticks_sum -= s->ticks[s->index];
 	s->ticks[s->index] = ticks;
 	s->ticks_sum += s->ticks[s->index];
@@ -61,7 +52,7 @@ static void __inline _hall_put_sample(u32 ticks) {
 }
 
 static float __inline _hall_avg_speed(void){
-	hall_sample_t *s = &_hall.samples;
+	hall_sample_t *s = &_sensor_hander.samples;
 	if (s->ticks_sum == 0.0f) {
 		return 0.0f;
 	}
@@ -69,27 +60,26 @@ static float __inline _hall_avg_speed(void){
 }
 
 static void hall_sensor_default(void) {
-	memset(&_hall, 0, sizeof(_hall));
-	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
+	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	_hall_init_el_angle();
 }
 
 void hall_sensor_init(void) {
 	mc_hall_init();
 	hall_sensor_default();
-	read_hall(_hall.state, _hall.measured_el_angle);
 	co_task_create(_hall_detect_task, NULL, 512);
 }
 
 void hall_sensor_clear(void) {
 	hall_sensor_default();
-	read_hall(_hall.state, _hall.measured_el_angle);
 }
 
 static void _hall_detect_task(void *args) {
 	while(1) {
-		if (_hall.working) {
+		if (_sensor_hander.el_speed != 0) {
 			u32 ticks_now = timer_count32_get();
-			u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
+			u32 delta_us = timer_count32_getus(ticks_now, _sensor_hander.hall_ticks);
 			if (delta_us >= (1200*1000)) {
 				hall_sensor_clear();
 			}
@@ -98,143 +88,86 @@ static void _hall_detect_task(void *args) {
 	}
 }
 
-
-u16 *hall_phase_angle(void) {
-	return _hall_table;
-}
-
 float hall_sensor_get_theta(void){
-	if (_hall.is_override_angle) {
-		return _hall.override_el_angle;
+	if (_sensor_hander.is_override_angle) {
+		return _sensor_hander.override_el_angle;
 	}
-	if (!_hall.working) {
-		read_hall(_hall.state, _hall.measured_el_angle);
-		return _hall.measured_el_angle;
+
+	float angle_step = _sensor_hander.el_speed * PWM_T;
+	float compensated_step = angle_step + _sensor_hander.el_speed_compensate * PWM_T * 0.05f;
+	_sensor_hander.estimate_delta_angle += compensated_step;
+#if 1
+	if (_sensor_hander.direction == POSITIVE) {
+		_sensor_hander.estimate_el_angle += compensated_step;
+	}else {
+		_sensor_hander.estimate_el_angle -= compensated_step;
 	}
-	os_disable_irq();
-	float angle_step = _hall.el_speed_avg * PWM_T;
-	float compensated_step = angle_step + _hall.el_speed_compensate * PWM_T;
-	if (_hall.estimate_delta_angle <= PHASE_60_DEGREE) {
-		_hall.estimate_delta_angle += compensated_step;
-		if (_hall.direction == POSITIVE) {
-			_hall.estimate_el_angle += compensated_step;
-			if (_hall.estimate_el_angle > PHASE_360_DEGREE) {
-				_hall.estimate_el_angle -= PHASE_360_DEGREE;
-			}
+#else
+	if (_sensor_hander.estimate_delta_angle >= PHASE_60_DEGREE) {
+		if (_sensor_hander.direction == POSITIVE) {
+			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle + PHASE_60_DEGREE;
 		}else {
-			_hall.estimate_el_angle -= compensated_step;
-			if (_hall.estimate_el_angle < 0) {
-			_hall.estimate_el_angle += PHASE_360_DEGREE;
-			}
+			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle - PHASE_60_DEGREE;
 		}
 	}
-	os_enable_irq();
-	return _hall.estimate_el_angle;
+#endif
+	return _sensor_hander.estimate_el_angle;
 }
 
-/*
-两个hall中断之间估计的走过的角度减去60度，就是估计偏差，通过偏差修正当前的速度,
-如果估计的比实际的小，当前速度需要加上一个修正值，如果比实际大，就要减去一个修正值
-*/
-static __inline__ void _hall_obsever_correct(float delta_s) {
-	float delta_angle = 0;
-	if (_hall.estimate_angle_aliment) {
-		delta_angle = (float)PHASE_60_DEGREE - _hall.estimate_delta_angle;
-		_hall.el_speed_compensate = delta_angle / delta_s;
-		_hall.estimate_el_angle = _hall.measured_el_angle;
-	}else {
-		_hall.estimate_el_angle = _hall.measured_el_angle;
-		_hall.el_speed_compensate = 0.0f;
-	}
-	_hall.estimate_delta_angle = 0.0f;
-}
 
 void hall_sensor_set_theta(bool override, float theta){
-	_hall.is_override_angle = override;
-	_hall.override_el_angle = theta;
+	_sensor_hander.is_override_angle = override;
+	_sensor_hander.override_el_angle = theta;
 }
 
 float hall_sensor_get_speed(void) {
-	_hall.rpm = _hall.el_speed / 360.0f * 60.0f;
-	return _hall.rpm;
+	_sensor_hander.rpm = _sensor_hander.el_speed / 360.0f * 60.0f;
+	return _sensor_hander.rpm;
 }
 
 float hall_sensor_avg_speed(void) {
-	return _hall.el_speed_avg / 360 * 60.0f;
+	return _sensor_hander.el_speed_avg / 360 * 60.0f;
 }
 
 int hall_offset_increase(int inc) {
-	if (_hall.phase_offset + inc >= 360) {
-		_hall.phase_offset = _hall.phase_offset + inc - 360;
+	if (_sensor_hander.phase_offset + inc >= 360) {
+		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - 360;
 	}else {
-		_hall.phase_offset += inc;
+		_sensor_hander.phase_offset += inc;
 	}
-	return _hall.phase_offset;
+	return _sensor_hander.phase_offset;
 }
 
-int hall_sensor_calibrate(float voltage, u16 *hall_table){
-	if (hall_table == NULL) {
-		hall_table = _hall_table;
-	}
-	foc_set_controller_mode(FOC_MODE_OPEN_LOOP);
-	hall_sensor_set_theta(true, 0.0f);
-	foc_set_dq_command(0.0f, 0.0f);
-	foc_pwm_start(true);
-	for (int i = 0;i < 1000;i++) {
-		foc_set_dq_command((float)i * voltage / 1000.0f, 0.0f);
-		delay_ms(1);
-	}
-	float sin_hall[8];
-	float cos_hall[8];
-	int hall_iterations[8];
-	memset(sin_hall, 0, sizeof(sin_hall));
-	memset(cos_hall, 0, sizeof(cos_hall));
-	memset(hall_iterations, 0, sizeof(hall_iterations));
-	delay_ms(2 * 1000);
-	// Forwards
-	for (int i = 0;i < 5;i++) {
-		for (int j = 0;j < 360;j++) {
-			hall_sensor_set_theta(true, j);
-			delay_ms(5);
-
-			int hall = get_hall_stat(7);
-			float s, c;
-			normal_sincosf(degree_2_pi(j), &s, &c);
-			sin_hall[hall] += s;
-			cos_hall[hall] += c;
-			hall_iterations[hall]++;
-		}
-	}
-	delay_ms(2 * 1000);
-	// Reverse
-	for (int i = 0;i < 5;i++) {
-		for (int j = 360;j >= 0;j--) {
-			hall_sensor_set_theta(true, j);
-			delay_ms(5);
-
-			int hall = get_hall_stat(7);
-			float s, c;
-			normal_sincosf(degree_2_pi(j), &s, &c);
-			sin_hall[hall] += s;
-			cos_hall[hall] += c;
-			hall_iterations[hall]++;
-		}
-	}
-	foc_pwm_start(false);
-	hall_sensor_set_theta(false, 0.0f);
-	foc_set_dq_command(0.0f, 0.0f);
-	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);
-			hall_table[i] = (u16)ang;
-		} else {
-			hall_table[i] = THETA_NONE;
-			fails++;
-		}
-	}
-	return fails == 2;	
+static void _hall_init_el_angle(void) {
+	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
+  	switch ( _sensor_hander.hall_stat )
+  	{
+    case STATE_5:
+      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE/2;
+      	break;
+    case STATE_1:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_3:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_2:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_120_DEGREE - PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_6:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE - PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_4:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE / 2;
+      	break;
+    default:
+      	/* Bad hall sensor configutarion so update the speed reliability */
+      	_sensor_hander.sensor_error = true;
+      	break;
+ 	}
+  	/* Initialize the measured angle */
+  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
+	_sensor_hander.hall_ticks = timer_count32_get();
 }
 
 static s32 _hall_position(u8 state_now, u8 state_prev) {
@@ -242,113 +175,108 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 	switch (state_now) {
 		case STATE_1:
 			if (state_prev == STATE_5) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset + PHASE_60_DEGREE;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
 			}else if (state_prev == STATE_3) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset + PHASE_120_DEGREE;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
 			}
 			break;
 		case STATE_2:
 			if (state_prev == STATE_3) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset + PHASE_180_DEGREE;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
 			}else if (state_prev == STATE_6) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset + PHASE_240_DEGREE;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
 			}
 			break;
 		case STATE_3:
 			if (state_prev == STATE_1) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset + PHASE_120_DEGREE;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
 			}else if (state_prev == STATE_2) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset + PHASE_180_DEGREE;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
 			}
 			break;
 		case STATE_4:
 			if (state_prev == STATE_6) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset + PHASE_300_DEGREE;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
 			}else if (state_prev == STATE_5) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset;
 			}
 			break;
 		case STATE_5:
 			if (state_prev == STATE_4) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset;
 			}else if (state_prev == STATE_1) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset + PHASE_60_DEGREE;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
 			}
 			break;
 		case STATE_6:
 			if (state_prev == STATE_2) {
-				_hall.direction = POSITIVE;
-				theta_now = _hall.phase_offset + PHASE_240_DEGREE;
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
 			}else if (state_prev == STATE_4) {
-				_hall.direction = NEGATIVE;
-				theta_now = _hall.phase_offset + PHASE_300_DEGREE;
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
 			}
 			break;
 		default:
 			return 0xFFFFFFFF;
 	}
-	if (theta_now >= 360) {
-		theta_now -= 360;
-	}
 	return theta_now;
 }
 
 void hall_sensor_handler(void) {
 	time_measure_start(&g_meas_hall);
-	u8 state_now = get_hall_stat(HALL_READ_TIMES);
-	u8 state_prev = _hall.state;
-	u32 ticks_now = timer_count32_get();	
-	if (!_hall.working) {		
-		if(_hall_table[state_now] != 0xFFFF) {
-			_hall.working = true;
-			_hall.state = state_now;
-			_hall.ticks = ticks_now;
-			_hall.measured_el_angle = _hall_table[state_now];
-			_hall.estimate_el_angle = _hall.measured_el_angle;
-		}
-		return;
-	}
-	s32 theta_now = _hall_position(state_now, state_prev);
+	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
+	u8 hall_stat_prev = _sensor_hander.hall_stat;
+	u32 hall_ticks_now = timer_count32_get();	
+
+	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
 	if (theta_now == 0xFFFFFFFF) {
-		sys_debug("hall error\n");
 		return;
 	}
 
-	u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
+	u32 delta_us = timer_count32_getus(hall_ticks_now, _sensor_hander.hall_ticks);
 	if (delta_us == 0) {
-		sys_debug("hall time error\n");
 		return;
 	}
 
-	os_disable_irq();
-	
 	float delta_time = us_2_s(delta_us);
 	_hall_put_sample(delta_us);
-	if (_hall.samples.full) {
-		_hall.el_speed_avg = _hall_avg_speed();
-		_hall.estimate_angle_aliment = true;
+	if (_sensor_hander.samples.full) {
+		_sensor_hander.el_speed_avg = _hall_avg_speed();
+		///_sensor_hander.estimate_angle_aliment = true;
 	}else {
-		_hall.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
+		_sensor_hander.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
 	}
-	_hall.el_speed = _hall.el_speed_avg;
-	_hall.measured_el_angle = theta_now;	
-	_hall.state = state_now;
-	_hall.ticks = ticks_now;
 
-	_hall_obsever_correct(delta_time);
+	os_disable_irq();
+	
+	_sensor_hander.measured_el_angle = theta_now;	
+	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
 
-	os_enable_irq();
+	if (_sensor_hander.estimate_angle_aliment) {
+		float delta_angle = (float)PHASE_60_DEGREE - _sensor_hander.estimate_delta_angle;
+		_sensor_hander.el_speed_compensate = delta_angle / delta_time;
+	}else {
+		_sensor_hander.el_speed_compensate = 0.0f;
+	}
+	_sensor_hander.estimate_delta_angle = 0.0f;
+	_sensor_hander.el_speed = _sensor_hander.el_speed_avg;
 
+	os_enable_irq();
+	
+	_sensor_hander.hall_stat = hall_stat_now;
+	_sensor_hander.hall_ticks = hall_ticks_now;
+	
 	time_measure_end(&g_meas_hall);
 }
 

Applications/foc/hall_sensor.h

@@ -42,15 +42,16 @@ typedef struct {
 	float el_speed_compensate; //电角速度补偿
 	//float el_angle_mod;
 	float rpm;        		//当前的电速度, 单位：RPM
-	u8    state;
-	u32   ticks;
+	u8    hall_stat;
+	u32   hall_ticks;
 	bool  working;
 	s8    direction;
-	u32   phase_offset;
+	s32   phase_offset;
 	bool  is_override_angle;
 	float override_el_angle;
 	hall_sample_t samples;
-}hall_t;
+	bool  sensor_error;
+}hall_sensor_t;
 
 
 
@@ -60,9 +61,7 @@ void hall_sensor_clear(void);
 float hall_sensor_get_theta(void); //return degree
 float hall_sensor_get_speed(void); //return rpm
 float hall_sensor_avg_speed(void);
-int hall_sensor_calibrate(float voltage, u16 *hall_table);
 void hall_sensor_set_theta(bool override, float theta);
-u16 *hall_phase_angle(void);
 int hall_offset_increase(int inc);
 
 #endif /* _HALL_SENSOR_H__ */

Applications/foc/svpwm.c

@@ -461,11 +461,8 @@ void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_perio
 			}
 		}
 	}
-	//X = SQRT3 * beta * modu;
 	X = TWO_BY_SQRT3 * beta * modu;
-	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
 	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
-	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
 	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
 	switch(sector) {
 		case SECTOR_1: // 3

Applications/libs/logger.c

@@ -2,9 +2,33 @@
 #include <stdarg.h>
 #include "bsp/bsp.h"
 #include "logger.h"
-
+#include "os/co_task.h"
+#include "os/queue.h"
 
 static uint32_t level_data[2];
+static co_queue_t log_queue = NULL;
+
+static void log_co_task(void *args) {
+	log_int_t log_v;
+	while(1) {
+		if (queue_get(log_queue, &log_v)){
+			sys_debug("%d: %d\n", log_v.id, log_v.value);
+		}
+		co_task_yield();
+	}
+}
+
+void log_start_task(void) {
+	log_queue = queue_create(1000, sizeof(log_int_t));
+	co_task_create(log_co_task, NULL, 512);
+}
+
+void log_int_value(int id, int value) {
+	log_int_t log_v;
+	log_v.id = id;
+	log_v.value = value;
+	queue_put(log_queue, &log_v);
+}
 
 static int _get_level(int mod){
 	int index = mod_bit_start(mod)/(32+1);

Applications/libs/logger.h

@@ -2,6 +2,14 @@
 #define _Shark_Logger_h__
 #include <stdarg.h>
 
+typedef struct {
+	int id;
+	int value;
+}log_int_t;
+
+void log_start_task(void);
+void log_int_value(int id, int value) ;
+
 #define L_disable 0
 #define L_debug  3
 #define L_warning 2

Applications/os/co_task.h

@@ -25,6 +25,10 @@ void co_task_yield(void); //task 让出cpu
 void co_task_schedule(void); //task开始调度
 void *co_malloc(u32 size);
 void co_free(void *ptr);
+float cpu_count_times(u32 prev);
+float cpu_times_bew_counts(u32 now, u32 prev);
+
+#define cpu_count_2_time(c) ((float)c / (float)SystemCoreClock)
 
 #endif /*_OS_TASK_H__*/
 

Applications/os/cpu.c

@@ -35,6 +35,19 @@ void cpu_udelay(u32 delay)
 }
 
 
+float cpu_count_times(u32 prev) {
+	u32 delta = cpu_counts_rel(prev);
+	return (float)delta / (float)SystemCoreClock;
+}
+
+float cpu_times_bew_counts(u32 now, u32 prev) {
+	u32 delta = now - prev;
+	if (now < prev) {
+		delta = 0xFFFFFFFF - prev + now + 1;
+	}
+	return (float)delta / (float)SystemCoreClock;
+}
+
 u32 cpu_count_delta_us(u32 count) {
 	u32 cpu_c = cpu_counts_rel(count);
 	float us = (float)cpu_c/120;

Applications/os/heap_4.c

@@ -38,7 +38,7 @@
 
 #define portBYTE_ALIGNMENT			8
 #define portBYTE_ALIGNMENT_MASK 	0x0007
-#define configTOTAL_HEAP_SIZE    (32*1024)
+#define configTOTAL_HEAP_SIZE    (40*1024)
 #define configASSERT(x)
 /* Block sizes must not get too small. */
 #define heapMINIMUM_BLOCK_SIZE	( ( size_t ) ( xHeapStructSize << 1 ) )