更新hall代码，从at32_mc copy过来

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

Applications/foc/motor/hall.c

@@ -1,488 +1,363 @@
-#include <string.h>
-#include "bsp/bsp_driver.h"
-#include "os/os_task.h"
-#include "libs/utils.h"
-#include "libs/logger.h"
-#include "math/fast_math.h"
-#include "foc/motor/hall.h"
-#include "app/nv_storage.h"
-#include "libs/time_measure.h"
-#include "app/nv_storage.h"
-#include "libs/logger.h"
-
-//#define USE_DETECTED_ANGLE 1
-
-#define HALL_READ_TIMES 9
-
-static u32 _hall_detect_task(void *args);
-static void _hall_init_el_angle(void);
-
-
-//#define HALL_PLACE_OFFSET (360-25)//(230) //(345) //315
-#define HALL_PLACE_OFFSET (230)
-
-/* 
-4,5,1,3,2,6,4
-*/
-
-hall_t _sensor_hander;
-
-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];}
-#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
-
-static void __inline _hall_put_sample(u32 ticks, float angle) {
-	hall_sample_t *s = &_sensor_hander.samples;
-	s->ticks_sum -= s->ticks[s->index];
-	s->angles_sum -= s->angles[s->index];
-	s->ticks[s->index] = ticks;
-	s->angles[s->index] = angle;
-	s->ticks_sum += s->ticks[s->index];
-	s->angles_sum += s->angles[s->index];
-	s->index += 1;
-	if (s->index >= SAMPLE_MAX_COUNT) {
-		s->full = true;
-		s->index = 0;
-	}
-}
-
-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 float __inline _hall_angle_speed(void){
-	hall_sample_t *s = &_sensor_hander.samples;
-	if (s->ticks_sum == 0) {
-		return 0.0f;
-	}
-	
-	if (!s->full) {
-		return s->angles[s->index - 1] / us_2_s(s->ticks[s->index-1]);
-	}else {
-		return s->angles_sum / us_2_s(s->ticks_sum);
-	}
-}
-
-void hall_debug_log(void) {
-	sys_debug("angle dir %d\n", _sensor_hander.direction);
-}
-
-/*
-static bool __inline _hall_data_empty(void) {
-	hall_sample_t *s = &_sensor_hander.samples;
-	if ((!s->full) && (s->index == 0)){
-		return true;
-	}
-	return false;
-}
-*/
-
-static void hall_sensor_default(s8 direction) {
-	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
-	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
-	for (int i = 0; i < 8; i++) {
-		_sensor_hander.angle_table[i] = (nv_get_motor_params()->hall_table[i]);
-	}
-	_sensor_hander.manual_angle = 0x3FF;
-	_sensor_hander.direction = direction;
-	_sensor_hander.running_dir = _sensor_hander.direction;
-	_hall_init_el_angle();
-}
-
-void hall_sensor_init(void) {
-	hall_sensor_default(POSITIVE);
-	mc_hall_init();
-	shark_task_create(_hall_detect_task, NULL);
-}
-
-void hall_set_direction(s8 direction) {
-	hall_sensor_default(direction);
-}
-
-void hall_sensor_clear(void) {
-	hall_set_direction(POSITIVE);
-}
-
-static u32 _hall_detect_task(void *args) {
-	if (_sensor_hander.el_speed != 0) {
-		u32 ticks_now = task_ticks_abs();
-		if (ticks_now > _sensor_hander.hall_ticks) {
-			if (task_delta_ticks(ticks_now) > 2000*1000) {
-				hall_sensor_clear();
-			}
-		}
-	}
-	return 0;
-}
-
-#if 1
-int hall_e_count = 0;
-float hall_sensor_get_theta(bool detect_err){
-	hall_e_count++;
-	float 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;
-
-	float el_angle = _sensor_hander.estimate_delta_angle;
-	if (el_angle > _sensor_hander.next_delta_angle) {
-		el_angle = _sensor_hander.next_delta_angle;
-	}
-	if (_sensor_hander.direction == POSITIVE) {
-		el_angle = _sensor_hander.estimate_el_angle + el_angle;
-	}else {
-		el_angle = _sensor_hander.estimate_el_angle - el_angle;
-	}
-	norm_angle_deg(el_angle);
-	if (hall_e_count%5 == 0) {
-		plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
-	}
-	return (el_angle);
-}
-#else
-static int test_loop = 0;
-float hall_sensor_get_theta(void){
-	float angle_add = _sensor_hander.delta_angle_ts;
-	float comp_angle = 0;
-	if (_sensor_hander.comp_count > 0) {
-		_sensor_hander.comp_count--;
-		comp_angle = _sensor_hander.angle_comp_ts;
-	}
-
-	if (_sensor_hander.direction == POSITIVE) {
-		_sensor_hander.estimate_delta_angle += angle_add;
-		_sensor_hander.estimate_el_angle += (angle_add + comp_angle);
-	}else {
-		_sensor_hander.estimate_delta_angle -= angle_add;
-		_sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
-	}
-	norm_angle_deg(_sensor_hander.estimate_el_angle);
-	test_loop ++;
-	if (test_loop % 20 == 0) {
-		//plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
-	}
-	return ( _sensor_hander.estimate_el_angle);
-}
-
-#endif
-
-float hall_sensor_get_speed(void) {
-	return (_sensor_hander.rpm) * _sensor_hander.running_dir;
-}
-
-int hall_offset_increase(int inc) {
-	inc = inc << 19;
-	if (_sensor_hander.phase_offset + inc >= PHASE_360_DEGREE) {
-		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - PHASE_360_DEGREE;
-	}else {
-		_sensor_hander.phase_offset += inc;
-	}
-	return _sensor_hander.phase_offset;
-}
-
-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] = (0x3FF);
-			fails++;
-		}
-	}
-	if (fails == 2) {
-		nv_save_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
-	if (_sensor_hander.hall_stat == 0 || _sensor_hander.hall_stat == 7) {
-		_sensor_hander.sensor_error ++;
-		return;
-	}
-	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + _sensor_hander.angle_table[_sensor_hander.hall_stat];
-#else
-	s32 sector_center = PHASE_60_DEGREE/2;
-  	switch ( _sensor_hander.hall_stat )
-  	{
-    case STATE_5:
-      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + sector_center;
-      	break;
-    case STATE_1:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + sector_center;
-      	break;
-    case STATE_3:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + sector_center;
-      	break;
-    case STATE_2:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + sector_center;
-      	break;
-    case STATE_6:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + sector_center;
-      	break;
-    case STATE_4:
-		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + sector_center;
-      	break;
-    default:
-      	/* Bad hall sensor configutarion so update the speed reliability */
-      	_sensor_hander.sensor_error ++;
-      	return;
- 	}
-#endif
-	_sensor_hander.sensor_error = 0;
-  	/* Initialize the measured angle */
-	_sensor_hander.measured_el_angle += PHASE_60_DEGREE;
-	norm_angle_deg(_sensor_hander.measured_el_angle);
-  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
-	_sensor_hander.hall_ticks = task_ticks_abs();
-}
-
-static __inline__ float _get_angle(u8 state, float added) {
-#ifdef USE_DETECTED_ANGLE
-	return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
-#else
-	return _sensor_hander.phase_offset + added;
-#endif
-}
-/* 4,5,1,3,2,6,4 */
-static float _hall_position(u8 state_now, u8 state_prev) {
-	float theta_now =  0xFFFFFFFF;
-	switch (state_now) {
-		case STATE_1:
-			if (state_prev == STATE_5) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
-			}else if (state_prev == STATE_3) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
-			}
-			break;
-		case STATE_2:
-			if (state_prev == STATE_3) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
-			}else if (state_prev == STATE_6) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
-			}
-			break;
-		case STATE_3:
-			if (state_prev == STATE_1) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
-			}else if (state_prev == STATE_2) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
-			}
-			break;
-		case STATE_4:
-			if (state_prev == STATE_6) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
-			}else if (state_prev == STATE_5) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
-			}
-			break;
-		case STATE_5:
-			if (state_prev == STATE_4) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
-			}else if (state_prev == STATE_1) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
-			}
-			break;
-		case STATE_6:
-			if (state_prev == STATE_2) {
-				_sensor_hander.running_dir = POSITIVE;
-				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
-			}else if (state_prev == STATE_4) {
-				_sensor_hander.running_dir = NEGATIVE;
-				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
-			}
-			break;
-		default:
-			_sensor_hander.sensor_error ++;
-			return 0xFFFFFFFF;
-	}
-	if (theta_now != 0xFFFFFFFF) {
-		norm_angle_deg(theta_now);
-	}
-	return theta_now;
-}
-
-#ifdef USE_DETECTED_ANGLE
-static __inline u8 _next_hall(u8 hall_now) {
-	switch (hall_now) {
-		case STATE_1:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_3;
-			}else {
-				return STATE_5;
-			}
-		case STATE_2:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_6;
-			}else {
-				return STATE_3;
-			}
-		case STATE_3:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_2;
-			}else {
-				return STATE_1;
-			}
-		case STATE_4:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_5;
-			}else {
-				return STATE_6;
-			}
-		case STATE_5:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_1;
-			}else {
-				return STATE_4;
-			}
-		case STATE_6:
-			if (_sensor_hander.direction == POSITIVE) {
-				return STATE_4;
-			}else {
-				return STATE_2;
-			}
-		default: //not reached here
-			return STATE_1;
-	}
-} 
-
-
-static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
-	s32 delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
-	if (_sensor_hander.direction == POSITIVE) {
-		if (delta_angle < 0) { //process cross 360 degree
-			delta_angle += PHASE_360_DEGREE;
-		}
-	}else if (_sensor_hander.direction == NEGATIVE) {
-		if (delta_angle > 0) { //process cross 360 degree
-			delta_angle -= PHASE_360_DEGREE;
-		}
-		delta_angle = -delta_angle;
-	}
-	return delta_angle;
-}
-#endif
-
-void HALL_IRQHandler(void) {
-	time_measure_start(&g_meas_hall);
-	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
-	u8 hall_stat_prev = _sensor_hander.hall_stat;
-	u32 hall_ticks_now = task_ticks_abs();	
-
-	/*获取当前转子角度*/
-	float theta_now = _hall_position(hall_stat_now, hall_stat_prev);
-	if (theta_now == 0xFFFFFFFF) {
-		return;
-	}
-	//plot_2data16(hall_stat_now*60, _sensor_hander.manual_angle);
-	//获取两次中断的时间间隔，估计速度
-	u32 delta_us = task_delta_ticks(hall_ticks_now);
-	if (delta_us == 0) {
-		return;
-	}
-	_sensor_hander.hall_ticks = hall_ticks_now;
-	//获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
-#ifdef USE_DETECTED_ANGLE
-	float delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
-	float next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
-#else
-	float delta_angle = PHASE_60_DEGREE;
-	float next_delta_angle = delta_angle;
-#endif
-	float delta_time = us_2_s(delta_us);
-	float prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
-	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
-	float delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
-	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);
-	u32 mask = cpu_enter_critical();
-	if (_sensor_hander.samples.full) {
-		//float 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);
-		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
-		delta_us = _hall_get_angle_ticks();
-		//_sensor_hander.comp_count = 50;
-		//_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
-		_sensor_hander.estimate_el_angle = theta_now;
-	}else {
-		_sensor_hander.comp_count = 0;
-		_sensor_hander.angle_comp_ts = 0;
-		_sensor_hander.estimate_el_angle = theta_now;
-	}
-	_sensor_hander.estimate_delta_angle = 0;
-	_sensor_hander.delta_angle_ts = (next_delta_angle/(us_2_s(delta_us)/FOC_CTRL_US));
-	_sensor_hander.next_delta_angle = next_delta_angle;
-	_sensor_hander.measured_el_angle = theta_now;
-
-	cpu_exit_critical(mask);
-	_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) * nv_get_motor_params()->poles; //s32q5
-	//plot_3data16(_sensor_hander.rpm >> 5, _sensor_hander.el_speed>>5, delta_us);
-	time_measure_end(&g_meas_hall);
-
-	//plot_3data16(delta_us/10, hall_stat_prev * 10, _sensor_hander.hall_stat * 10);
-}
-
-
+#include <string.h>
+#include "bsp/bsp_driver.h"
+#include "os/os_task.h"
+#include "libs/utils.h"
+#include "libs/logger.h"
+#include "math/fast_math.h"
+#include "foc/motor/hall.h"
+#include "foc/mc_config.h"
+#include "app/nv_storage.h"
+#include "libs/logger.h"
+
+#define HALL_READ_TIMES 5
+#define SMOOTH_COUNT 0
+#define HALL_DETECT_OFFSET 0
+/* 
+1,5,4,6,2,3
+0,1,2,3,4,5
+//////
+2,6,4,5,1,3
+0,1,2,3,4,5
+////
+2,3,1,5,4,6
+0,1,2,3,4,5
+*/
+static s8 hall_2_pos[] = {7,2,0,1,4,3,5,7};
+static hall_t g_hall;
+
+#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
+#define HALL_MAX_TIME 1000000UL
+
+static u32 g_delta_cnt = 0;
+static u32 g_trns_cnt = 0;
+static u32 g_min_delta;
+
+#define COUNT_2_US(c) (c/(SYSTEM_CLOCK/1000000))
+
+#if HALL_DETECT_OFFSET > 0
+static float hall_off[6];
+static u32	 hall_off_cnt[6];
+
+static void hall_offset_dec_init(void) {
+	for (int i = 0; i < 6; i++) {
+		hall_off[i] = 0;
+		hall_off_cnt[i] = 0;
+	}
+}
+float mc_get_mot_angle(void);
+static __INLINE void hall_offset_update(void) {
+	int pos_int = (int)g_hall.low_res_pos;
+	float hall_pos = g_hall.low_res_pos * PHASE_60_DEGREE;
+	float delta_pos = hall_pos - (mc_get_mot_angle() + 90.0f);
+	norm_angle_deg(delta_pos);
+	hall_off[pos_int] = delta_pos;
+	hall_off_cnt[pos_int] += 1;
+}
+static void hall_offset_log(void) {
+	if (hall_off_cnt[0] < 20) {
+		return;
+	}
+	s16 value[6];
+	u32 mask = cpu_enter_critical();
+	for (int i = 0; i < 6; i++) {
+		value[i] = (s16)(hall_off[i]/(float)hall_off_cnt[i]);
+		hall_off_cnt[i] = 0;
+	}
+	cpu_exit_critical(mask);
+	sys_debug("off:%d,%d,%d,%d,%d,%d\n", value[0], value[1], value[2], value[3], value[4], value[5]);
+}
+
+#else
+static void hall_offset_dec_init(void) {
+}
+static __INLINE void hall_offset_update(void) {
+}
+static void hall_offset_log(void) {
+}
+#endif
+static __INLINE u32 hall_delta_us(u32 count) {
+	u32 now = task_ticks_abs();
+	u32 delta = now - count;
+	if (now < count) { //wrap
+		delta = 0xFFFFFFFF - count + now + 1;
+	}
+	return COUNT_2_US(delta);
+}
+
+
+static u8 __INLINE hall_read_state(void) {
+	u8 hall_a = 0, hall_b = 0, hall_c = 0;
+	for (int i = 0; i < HALL_READ_TIMES; i++) {
+		hall_a += gpio_hall_a_value();
+		hall_b += gpio_hall_b_value();
+		hall_c += gpio_hall_c_value();
+	}
+	u8 state = 0;
+	if (hall_a > (HALL_READ_TIMES/2 + 1)) {
+		state = 1;
+	}
+	if (hall_b > (HALL_READ_TIMES/2 + 1)) {
+		state = state | (1<<1);
+	}
+	if (hall_c > (HALL_READ_TIMES/2 + 1)) {
+		state = state | (1 << 2);
+	}
+	return state;
+}
+
+static void hall_init_low_pos(void) {
+	u8 state = hall_read_state();
+	s16 pos = hall_2_pos[state];
+	if (pos == 7) {
+		g_hall.sig_errors ++;
+		return;
+	}
+	g_hall.state = state;
+	if (g_hall.dir == POSITIVE) {
+		g_hall.low_res_pos = pos + 0.5f;
+	}else {
+		g_hall.low_res_pos = pos - 0.5f;
+	}
+}
+
+
+static void __INLINE hall_put_sample(u32 ticks) {
+	hsample_t *s = &g_hall.samples;
+	g_hall.last_delta_ticks = ticks;
+	s->ticks_sum -= s->ticks[s->index];
+	s->ticks[s->index] = ticks;
+	s->ticks_sum += s->ticks[s->index];
+	s->index += 1;
+	if (s->index >= SAMPLE_MAX_COUNT) {
+		s->index = 0;
+	}
+	if (s->filled < SAMPLE_MAX_COUNT) {
+		s->filled++;
+	}
+}
+
+
+static float __INLINE hall_elec_angle_vel(void){
+	hsample_t *s = &g_hall.samples;
+	if (s->filled < SAMPLE_MAX_COUNT) {
+		return (PHASE_60_DEGREE) / us_2_s(g_hall.last_delta_ticks);
+	}
+	if (s->ticks_sum == 0) {
+		return 0.0f;
+	}
+	return (PHASE_60_DEGREE * SAMPLE_MAX_COUNT) / us_2_s(s->ticks_sum);
+}
+
+void hall_debug_log(void) {
+	sys_debug("angle dir %d, stat %d, lowres %f, err %d,%d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors);
+	sys_debug("D: %d,%d,%d\n", g_delta_cnt, g_trns_cnt, g_min_delta);
+	sys_debug("RPM: %f\n", hall_get_velocity());
+	hall_offset_log();
+}
+
+void hall_init(void) {
+	g_hall.phase_offset = mc_conf()->m.encoder_offset;
+	g_hall.mot_poles = mc_conf()->m.poles;
+	g_hall.b_trns_det = false;
+	g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
+	g_hall.angle_smooth_step = 0;
+	g_hall.samples.ticks_sum = 0;
+	g_hall.position = 0;
+	g_hall.samples.index = 0;
+	g_hall.samples.filled = 0;
+	g_hall.elec_angle_vel = 0;
+	g_hall.dir_set = g_hall.prev_dir = g_hall.dir = POSITIVE;
+	g_min_delta = 1000000 * 100;
+	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
+		g_hall.samples.ticks[i] = HALL_MAX_TIME;
+	}
+	g_hall.samples.ticks_sum = HALL_MAX_TIME * SAMPLE_MAX_COUNT;
+
+	if (!g_hall.inited) {
+		g_hall.inited = true;
+#ifdef HALL_TIMER
+		hall_tmr_init();
+#else
+		gpio_hall_init();
+#endif
+	}
+	hall_offset_dec_init();
+	hall_init_low_pos();
+}
+
+void  hall_set_direction(s8 dir) {
+	if (dir == g_hall.dir_set) {
+		return;
+	}
+	g_hall.dir_set = g_hall.dir = g_hall.prev_dir = dir;
+	hall_init_low_pos();
+}
+
+#if SMOOTH_COUNT > 0
+static float get_angle_diff(float a1, float a2) {
+	float diff = a1 - a2;
+	float abs_diff = ABS(diff);
+	if (abs_diff >= PHASE_180_DEGREE) {
+		return (PHASE_360_DEGREE - abs_diff);
+	}else {
+		return diff;
+	}
+}
+#endif
+
+static __INLINE bool hall_update_low_pos(void) {
+	u8 state = hall_read_state();
+	s16 pos = hall_2_pos[state];
+	if (pos == 7) {
+		g_hall.sig_errors ++;
+		return false;
+	}
+	s16 pos_prev = hall_2_pos[g_hall.state];
+	s16 delta_pos = pos - pos_prev;
+	if (delta_pos != 0) {
+		s8  prev_dir = g_hall.dir;
+		if (delta_pos == 1 || delta_pos == -5) {
+			g_hall.dir = POSITIVE;
+			g_hall.prev_dir = prev_dir;
+		}else if (delta_pos == -1 || delta_pos == 5){
+			g_hall.dir = NEGATIVE;
+			g_hall.prev_dir = prev_dir;
+		}else {
+			if (g_hall.samples.filled > 0) {
+				g_hall.sig_errors ++;
+				return false;
+			}
+		}
+		g_hall.edge_ticks = task_ticks_abs();
+	}else {
+		g_hall.sig_errors ++;
+		return false;
+	}	
+	g_hall.state = state;
+	
+	g_hall.low_res_pos = pos;
+
+	return true;
+}
+
+hall_t *hall_get(void) {
+	return &g_hall;
+}
+
+float hall_get_elec_angle(void) {
+	float phase_offset = g_hall.phase_offset;
+	if (g_hall.dir == NEGATIVE) {
+		phase_offset = PHASE_60_DEGREE + phase_offset;
+	}
+	float angle = g_hall.elec_angle + phase_offset;
+	norm_angle_deg(angle);
+	return angle;
+}
+
+float hall_update_elec_angle(void) {
+	float delta_ticks = (float)hall_delta_us(g_hall.edge_ticks);//上次hall变换到目前的时间
+	float low_res = g_hall.low_res_pos;
+	float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
+	if (delta_pos > 1.0f) {
+		delta_pos = 1.0f;
+	}else if (delta_pos < -1.0f) {
+		delta_pos = -1.0f;
+	}
+	float high_res_pos = delta_pos  + low_res;
+	float elec_angle = high_res_pos * PHASE_60_DEGREE;
+	float elec_smooth_angle;
+#if SMOOTH_COUNT>0
+	float delta_angle = delta_pos * PHASE_60_DEGREE;
+	if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
+		elec_smooth_angle = g_hall.elec_angle_edge + g_hall.angle_smooth_step * g_hall.angle_smooth_cnt + delta_angle;
+		g_hall.angle_smooth_cnt++;
+		if (g_hall.angle_smooth_step >= 0) {
+			elec_smooth_angle = min(elec_smooth_angle, elec_angle);
+		}else {
+			elec_smooth_angle = MAX(elec_smooth_angle, elec_angle);
+		}
+	}else {
+		elec_smooth_angle = elec_angle;
+	}
+#else
+	elec_smooth_angle = elec_angle;
+#endif
+	norm_angle_deg(elec_smooth_angle);
+	g_hall.elec_angle = elec_smooth_angle;
+	g_hall.position += g_hall.elec_angle_vel * FOC_CTRL_US / g_hall.mot_poles;
+	if ((g_hall.samples.filled > 1) && (delta_ticks / g_hall.last_delta_ticks >= 1.4f)) {
+		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.95f;
+		if (g_hall.elec_angle_vel < 30) {
+			g_hall.elec_angle_vel = 0;
+			g_hall.samples.filled = 0;
+			g_hall.samples.index = 0;
+			g_hall.dir = g_hall.prev_dir = g_hall.dir_set;
+			hall_init_low_pos();
+		}
+		float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
+		g_hall.velocity_filted = velocity_raw;
+	}
+	return hall_get_elec_angle();
+}
+
+float hall_get_vel_counts(void) {
+	return g_hall.elec_angle_vel/PHASE_60_DEGREE * g_hall.dir;
+}
+
+float hall_get_velocity(void) {
+	return g_hall.velocity_filted;
+}
+
+float hall_get_position(void) {
+	return g_hall.position;
+}
+
+static __INLINE void hall_calc_mot_velocity(u32 delta_cnt) {
+	if (g_hall.samples.filled == 0) {
+		hall_put_sample(HALL_MAX_TIME/10);
+		return;
+	}
+	if (delta_cnt < g_min_delta) {
+		g_min_delta = delta_cnt;
+	}
+	hall_put_sample(delta_cnt);
+	g_hall.elec_angle_vel = hall_elec_angle_vel();
+	float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
+	LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.5F);
+}
+
+
+float hall_offset_detect(float *off) {
+	return 0.0f;
+}
+
+void HALL_IRQHandler(void) {
+	g_delta_cnt++;
+	u32 mask = cpu_enter_critical();
+	u32 delta_cnt = hall_delta_us(g_hall.edge_ticks);
+	if (delta_cnt < 200) {
+		g_hall.noise_errors++;
+		goto hall_end;
+	}
+	if (!hall_update_low_pos()) {
+		goto hall_end;
+	}
+	hall_offset_update();
+	g_hall.elec_angle_edge = g_hall.elec_angle;
+#if SMOOTH_COUNT>0
+	g_hall.delta_angle_edge = get_angle_diff(g_hall.low_res_pos * PHASE_60_DEGREE, g_hall.elec_angle_edge);
+	if (ABS(g_hall.delta_angle_edge) >= 2.0f) {
+		g_hall.angle_smooth_step = g_hall.delta_angle_edge/SMOOTH_COUNT;
+		g_hall.angle_smooth_cnt = 1;
+	}else {
+		g_hall.angle_smooth_step = 0;
+		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
+	}
+#endif
+	hall_calc_mot_velocity(delta_cnt);
+hall_end:
+	cpu_exit_critical(mask);
+	return;
+}
+
+

Applications/foc/motor/hall.h

@@ -3,6 +3,7 @@
 #include "os/os_types.h"
 #include "bsp/bsp.h"
 #include "math/fix_math.h"
+#include "foc/core/PI_Controller.h"
 
 
 #define STATE_0 (uint8_t)0
@@ -15,51 +16,49 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
-#define SAMPLE_MAX_COUNT 16
+#define SAMPLE_MAX_COUNT 6
 
 typedef struct {
 	u32       	ticks[SAMPLE_MAX_COUNT];
-	float   	angles[SAMPLE_MAX_COUNT];
 	u32   		ticks_sum;
-	float   	angles_sum;
 	u32   		index;
-	bool  		full;
-}hall_sample_t;
+	u32			filled;
+}hsample_t;
 
 typedef struct {
-	float 		estimate_el_angle; //60度区间内的估计电角度
-	float 		estimate_delta_angle;		
-	float   		measured_el_angle; //hall测量到的电角度
-	float 		next_delta_angle;
-	float        delta_angle_ts;    //每个控制周期角度的增加量，通过上次的速度计算得到
-	float        angle_comp_ts;     //每个控制周期角度增加的补偿量，主要用在加减速，hall角度和60度有偏差的情况
-	int             comp_count;
-	float 		immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
-	float 		el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
-	float  		rpm;        		//当前的电速度, 单位：RPM
-	bool  			trns_detect;     //速度变化超过阈值
-	u8    			hall_stat;
-	u32   			hall_ticks;	
-	s8    			direction;
-	s8              running_dir;
+	bool			inited;
 	float   		phase_offset;
-	hall_sample_t 	samples;
-	u32  			sensor_error;
-	float         manual_angle;
-	s32  		angle_table[8];
+	float			mot_poles;
+	u8				state; // state = A <<2+ B <<1 + C
+	float			low_res_pos; //每个霍尔中断更新一次，60度
+	s8				dir;
+	s8				prev_dir;
+	s8				dir_set;
+	u32				edge_ticks;
+	u32				last_delta_ticks;
+	float			elec_angle; //经过插值的高分辨率角度
+	float			elec_angle_vel;
+	float			elec_angle_edge; //霍尔中断的时候，上面插值高分辨率的角度所存
+	float			delta_angle_edge;
+	float			angle_smooth_step;
+	float			angle_smooth_cnt;
+	float			velocity_filted;
+	float			position;
+	hsample_t		samples;
+	bool			b_trns_det; //速度突变
+	u32				sig_errors;
+	u32				noise_errors;
 }hall_t;
 
-void hall_sensor_init(void);
-void hall_sensor_clear(void);
-float hall_sensor_get_theta(bool detect_err); //return degree
-float 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);
-void hall_set_direction(s8 direction);
+void hall_init(void);
+float hall_update_elec_angle(void);
+float hall_get_elec_angle(void);
+float hall_get_velocity(void);
+float hall_get_vel_counts(void);
+float hall_get_position(void);
+float hall_offset_detect(float *off);
+void  hall_set_direction(s8 dir);
+hall_t *hall_get(void);
 
 #endif /* _HALL_SENSOR_H__ */