霍尔超时使用co_task

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

Applications/bsp/mc_hall_gpio.c

@@ -10,6 +10,7 @@ void mc_hall_init(void){
 	_gpio_irq_enable();
 }
 
+#if 0
 int get_hall_stat(int samples) {
 	int h1 = 0, h2 = 0, h3 = 0;
 	int tres = (samples + 1) / 2;
@@ -25,7 +26,16 @@ int get_hall_stat(int samples) {
 	return (h1 > tres) | ((h2 > tres) << 1) | ((h3 > tres) << 2);
 #endif
 }
+#else
+int get_hall_stat(int samples) {
+#if HALL_PLACE==DEGREES_60	
+	return (((READ_HALL2())^1) << 2) | ((READ_HALL3()) << 1) | (READ_HALL1());
+#else
+	return (READ_HALL1()) | ((READ_HALL2()) << 1) | ((READ_HALL3()) << 2);
+#endif
+}
 
+#endif
 static void _gpio_irq_enable(void){
 	gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOB, GPIO_PIN_SOURCE_6);
 	exti_init(EXTI_6, EXTI_INTERRUPT, EXTI_TRIG_BOTH);

Applications/bsp/timer_count32.c

@@ -10,9 +10,6 @@ static void init_slave_timer(void);
 #define SLAVE  TIMER2
 #define SLAVE_CK RCU_TIMER2
 
-#define COUNT_CLK (10 * 1000000)
-#define COUNT_2_US(c) (c/10)
-
 void timer_count32_init(void) {
 
 	init_master_timer();
@@ -98,6 +95,7 @@ u32 timer_count32_delta_us(u32 count, u32 *p_update) {
 	return COUNT_2_US(delta);
 }
 
+
 void time_measure_start(measure_time_t *m){
 	m->intval_time = timer_count32_delta_us(m->intval_count, &m->intval_count);
 	m->exec_count = timer_count32_get();

Applications/bsp/timer_count32.h

@@ -18,5 +18,16 @@ u32 timer_count32_delta_us(u32 prev_count, u32 *p_update);
 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)
+
+static __inline__ u32 timer_count32_getus(u32 now, u32 count) {
+	u32 delta = now - count;
+	if (now < count) { //wrap
+		delta = 0xFFFFFFFF - count + now + 1;
+	}
+	return COUNT_2_US(delta);
+}
+
 #endif	/* _TIMER_COUNT32_H__ */
 

Applications/foc/foc_api.c

@@ -69,7 +69,7 @@ void foc_clear(void) {
 	pwm_stop();
 	g_foc.mosfec_gate = false;
 	foc_defulat_value();
-	hall_sensor_init();
+	hall_sensor_clear();
 }
 
 u32 foc_get_speed(void) {

Applications/foc/hall_sensor.c

@@ -4,6 +4,7 @@
 #include "os/co_task.h"
 #include "os/timer.h"
 #include "libs/utils.h"
+#include "libs/logger.h"
 #include "math/fast_math.h"
 #include "hall_sensor.h"
 #include "foc/foc_api.h"
@@ -11,7 +12,8 @@
 #include "bsp/timer_count32.h"
 
 #define HALL_READ_TIMES 3
-static void _hall_timeout_handler(timer_t *timer);
+static void _hall_detect_task(void *args);
+
 
 /* 
 * 测量HALL_PLACE_OFFSET通用方式就是ST推荐的通过外力带动电机，
@@ -21,7 +23,7 @@ static void _hall_timeout_handler(timer_t *timer);
 * 从0开始增加，每增加1度观察电机电流(看直流电源),
 * 找到一个电机平稳转动并且电流最小的角度作为HALL_PLACE_OFFSET
 */
-#define HALL_PLACE_OFFSET 210.0f
+#define HALL_PLACE_OFFSET 210
 /* 
 100
 101
@@ -35,7 +37,6 @@ static void _hall_timeout_handler(timer_t *timer);
 static u16 _hall_table[] = {0xFFFF, 121/*1*/, 240/*2*/, 190/*3*/, 13/*4*/, 58/*5*/, 306/*6*/, 0xFFFF};
 static hall_t _hall;
 static hall_sample_t h_samples;
-static timer_t _hall_detect_timer = TIMER_INIT(_hall_detect_timer, _hall_timeout_handler);
 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];}
@@ -45,23 +46,21 @@ static float __inline _delta_seconds(u32 prev) {
 	return (float)timer_count32_delta_us(prev, NULL)/1000000.0f;
 }
 
-static void __inline _hall_put_sample(float angle, u32 ticks) {
+static void __inline _hall_put_sample(u32 ticks) {
 	hall_sample_t *s = &h_samples;
 	s->index += 1;
 	if (s->index >= SAMPLE_MAX_COUNT) {
 		s->full = true;
 		s->index = 0;
 	}
-	s->angle[s->index] = angle;
 	s->ticks[s->index] = ticks;
 }
 
 static float __inline _hall_avg_speed(void){
 	hall_sample_t *s = &h_samples;
-	float t_angle = 0.0f;
+	float t_angle = (float)(PHASE_60_DEGREE * SAMPLE_MAX_COUNT);
 	u32   t_ticks = 0;
 	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
-		t_angle += s->angle[i];
 		t_ticks += s->ticks[i];
 	}
 	if (t_ticks == 0.0f) {
@@ -75,15 +74,29 @@ void hall_sensor_init(void) {
 	memset(&_hall, 0, sizeof(_hall));
 	read_hall(_hall.state, _hall.theta);
 	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	co_task_create(_hall_detect_task, NULL, 512);
 }
 
-void _hall_timeout_handler(timer_t *timer){
+void hall_sensor_clear(void) {
+	memset(&_hall, 0, sizeof(_hall));
+	read_hall(_hall.state, _hall.theta);
+	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+}
 
-	_hall.angle_speed = _hall.angle_speed_avg = 0;
-	_hall.rpm = 0;
-	h_samples.index = 0;
+static void _hall_detect_task(void *args) {
+	while(1) {
+		if (_hall.working) {
+			u32 ticks_now = timer_count32_get();
+			u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
+			if (delta_us >= (1200*1000)) {
+				hall_sensor_clear();
+			}
+		}
+		co_task_delay(100);
+	}
 }
 
+
 u16 *hall_phase_angle(void) {
 	return _hall_table;
 }
@@ -96,7 +109,7 @@ float hall_sensor_get_theta(void){
 		read_hall(_hall.state, _hall.theta);
 		return _hall.theta;
 	}
-	_hall.est_theta = _delta_seconds(_hall.ticks) * _hall.angle_speed + _hall.theta;
+	_hall.est_theta = _delta_seconds(_hall.ticks) * _hall.angle_speed * _hall.direction + _hall.theta;
 	float est_delta = _hall.est_theta - _hall.theta;
 	if (est_delta > PHASE_60_DEGREE) {
 		_hall.est_theta = _hall.theta + PHASE_60_DEGREE;
@@ -126,8 +139,8 @@ float hall_sensor_avg_speed(void) {
 }
 
 int hall_offset_increase(int inc) {
-	if (_hall.phase_offset + inc >= 360.0f) {
-		_hall.phase_offset = _hall.phase_offset + inc - 360.0f;
+	if (_hall.phase_offset + inc >= 360) {
+		_hall.phase_offset = _hall.phase_offset + inc - 360;
 	}else {
 		_hall.phase_offset += inc;
 	}
@@ -199,24 +212,8 @@ int hall_sensor_calibrate(float voltage, u16 *hall_table){
 	return fails == 2;	
 }
 
-
-void hall_sensor_handler(void) {
-	
-	time_measure_start(&g_meas_hall);
-	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 = timer_count32_get();
-		}
-		return;
-	}
-	
+static s32 _hall_position(u8 state_now, u8 state_prev) {
+	s32 theta_now =  0xFFFFFFFF;
 	switch (state_now) {
 		case STATE_1:
 			if (state_prev == STATE_5) {
@@ -273,38 +270,63 @@ void hall_sensor_handler(void) {
 			}
 			break;
 		default:
-			return;
+			return 0xFFFFFFFF;
 	}
-
-	if (theta_now >= 360.0f) {
-		theta_now -= 360.0f;
+	if (theta_now >= 360) {
+		theta_now -= 360;
 	}
+	return theta_now;
+}
+
+void hall_sensor_handler(void) {
 	
-	float delta_time = _delta_seconds(_hall.ticks);
-	if (delta_time == 0.0f) { //may be errors ???
+	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();
+
+	_hall.state = get_hall_stat(HALL_READ_TIMES);	
+	if (!_hall.working) {		
+		if(_hall_table[state_now] != 0xFFFF) {
+			_hall.working = true;
+			_hall.state = state_now;
+			_hall.theta = _hall_table[state_now];
+			_hall.ticks = ticks_now;
+		}
+		return;
+	}
+	s32 theta_now = _hall_position(_hall.state, state_prev);
+	if (theta_now == 0xFFFFFFFF) {
+		_hall.state = state_prev;
 		return;
 	}
-	time_measure_end(&g_meas_hall);
-	float delta_theta = (_hall.direction == POSITIVE)?60.0f : -60.0f;
-	_hall_put_sample(delta_theta, timer_count32_delta_us(_hall.ticks, NULL));
 
+	u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
+	if (delta_us == 0) {
+		return;
+	}
 	os_disable_irq();
-	_hall.angle_speed = delta_theta / delta_time;
+	
+	float delta_time = us_2_s(delta_us);
+	
+	_hall_put_sample(delta_us);
+
+	_hall.angle_speed = (float)PHASE_60_DEGREE / delta_time;
 
 	if (!h_samples.full) {
 		_hall.angle_speed_avg = _hall.angle_speed;
 	}else {		
 		_hall.angle_speed_avg = _hall_avg_speed();
 	}
-	_hall.ticks = timer_count32_get();
+	
 	_hall.theta = theta_now;	
 	_hall.state = state_now;
+	_hall.ticks = ticks_now;
 	os_enable_irq();
 	
 	_hall.rpm = _hall.angle_speed / 360.0f * 60.0f;
-	
-	timer_post(&_hall_detect_timer, 1500);
-	
+
+	time_measure_end(&g_meas_hall);
 }
 
 

Applications/foc/hall_sensor.h

@@ -6,12 +6,12 @@
 #define NEGATIVE          (int8_t)-1
 #define POSITIVE          (int8_t)1
 
-#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 PHASE_60_DEGREE (60)
+#define PHASE_120_DEGREE (120)
+#define PHASE_180_DEGREE (180)
+#define PHASE_240_DEGREE (240)
+#define PHASE_300_DEGREE (300)
+#define PHASE_360_DEGREE (360)
 
 #define STATE_0 (uint8_t)0
 #define STATE_1 (uint8_t)1
@@ -25,7 +25,7 @@
 #define THETA_NONE        (float)0xFFFF
 typedef struct {
 	bool  alignmnet;
-	float theta;
+	s32   theta;
 	float est_theta;
 	float rpm;        		//当前的电速度, 单位：RPM
 	float angle_speed; 		//当前的电角速度, 单位：rad/s
@@ -34,7 +34,7 @@ typedef struct {
 	u32   ticks;
 	bool  working;
 	s8    direction;
-	float phase_offset;
+	u32   phase_offset;
 	bool  is_override_theta;
 	float override_theta;
 }hall_t;
@@ -42,13 +42,13 @@ typedef struct {
 #define SAMPLE_MAX_COUNT 6
 
 typedef struct {
-	float angle[SAMPLE_MAX_COUNT];
 	u32   ticks[SAMPLE_MAX_COUNT];
 	u32   index;
 	bool  full;
 }hall_sample_t;
 
 void hall_sensor_init(void);
+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);

Applications/os/cpu.c

@@ -12,7 +12,7 @@ u32 cpu_counts_abs(void)
 	return DWT->CYCCNT;
 }
 
-static u32 cpu_counts_rel(u32 start)
+static __inline__ u32 cpu_counts_rel(u32 start)
 {
 	u32 ticks = DWT->CYCCNT;