update hall code

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

Applications/app/app.c

@@ -30,8 +30,8 @@ static void _app_low_task(void *args) {
 	while(1) {
 		wdog_reload();
 		_can_report_info();
-		sys_debug("foc exec time %d, intval %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time);
-		sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
+		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);
 	}
 }

Applications/bsp/timer_count32.h

@@ -10,6 +10,7 @@ typedef struct {
 	u32 exec_max_time;
 	u32 exec_max_error_time;
 	u32 exec_time_error;
+	u32 intval_time_error;
 }measure_time_t;
 
 void timer_count32_init(void);

Applications/foc/foc_cmd.c

@@ -10,6 +10,7 @@
 #include "foc/foc_core.h"
 #include "foc/hall_sensor.h"
 #include "foc/foc_cmd.h"
+#include "prot/can_foc_msg.h"
 
 extern motor_foc_t g_foc;
 
@@ -85,7 +86,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				offset = hall_offset_increase(-1);
 			}
 			sys_debug("cali hall offset %d\n", offset);
-			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset), 1);
+			can_response_hall_offset(command->can_src, offset);
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:

Applications/foc/foc_core.c

@@ -13,6 +13,7 @@
 #include "gas_sensor.h"
 #include "svpwm.h"
 #include "bsp/timer_count32.h"
+#include "libs/logger.h"
 
 motor_foc_t g_foc = {
 	.motor_param = {
@@ -229,6 +230,9 @@ 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 ++;
+	}
 }
 
 void foc_pwm_start(bool start) {

Applications/foc/hall_sensor.c

@@ -11,9 +11,12 @@
 #include "app/nv_storage.h"
 #include "bsp/timer_count32.h"
 
+
 #define HALL_READ_TIMES 3
+
 static void _hall_detect_task(void *args);
 
+#define PWM_T  (0.00003333f)
 
 /* 
 * 测量HALL_PLACE_OFFSET通用方式就是ST推荐的通过外力带动电机，
@@ -34,53 +37,52 @@ static void _hall_detect_task(void *args);
 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, 121/*1*/, 240/*2*/, 190/*3*/, 13/*4*/, 58/*5*/, 306/*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_sample_t h_samples;
+
+
 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)
 
-static float __inline _delta_seconds(u32 prev) {
-	return (float)timer_count32_delta_us(prev, NULL)/1000000.0f;
-}
 
 static void __inline _hall_put_sample(u32 ticks) {
-	hall_sample_t *s = &h_samples;
+	hall_sample_t *s = &_hall.samples;
+	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->full = true;
 		s->index = 0;
 	}
-	s->ticks[s->index] = ticks;
 }
 
 static float __inline _hall_avg_speed(void){
-	hall_sample_t *s = &h_samples;
-	float t_angle = (float)(PHASE_60_DEGREE * SAMPLE_MAX_COUNT);
-	u32   t_ticks = 0;
-	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
-		t_ticks += s->ticks[i];
-	}
-	if (t_ticks == 0.0f) {
+	hall_sample_t *s = &_hall.samples;
+	if (s->ticks_sum == 0.0f) {
 		return 0.0f;
 	}
-	return (t_angle / us_2_s(t_ticks));
+	return (((float)PHASE_60_DEGREE * (float)SAMPLE_MAX_COUNT) / (us_2_s(s->ticks_sum)));
 }
 
-void hall_sensor_init(void) {
-	mc_hall_init();
+static void hall_sensor_default(void) {
 	memset(&_hall, 0, sizeof(_hall));
-	read_hall(_hall.state, _hall.theta);
 	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+}
+
+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) {
-	memset(&_hall, 0, sizeof(_hall));
-	read_hall(_hall.state, _hall.theta);
-	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
+	hall_sensor_default();
+	read_hall(_hall.state, _hall.measured_el_angle);
 }
 
 static void _hall_detect_task(void *args) {
@@ -102,40 +104,63 @@ u16 *hall_phase_angle(void) {
 }
 
 float hall_sensor_get_theta(void){
-	if (_hall.is_override_theta) {
-		return _hall.override_theta;
+	if (_hall.is_override_angle) {
+		return _hall.override_el_angle;
 	}
 	if (!_hall.working) {
-		read_hall(_hall.state, _hall.theta);
-		return _hall.theta;
+		read_hall(_hall.state, _hall.measured_el_angle);
+		return _hall.measured_el_angle;
 	}
-	_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;
-		if (_hall.est_theta >= PHASE_360_DEGREE){
-			_hall.est_theta -= PHASE_360_DEGREE;
-		}
-	}else if (est_delta < -PHASE_60_DEGREE){
-		_hall.est_theta = _hall.theta - PHASE_60_DEGREE;
-		if (_hall.est_theta < 0/*= -PHASE_360_DEGREE*/) {
-			_hall.est_theta += PHASE_360_DEGREE;
+	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 {
+			_hall.estimate_el_angle -= compensated_step;
+			if (_hall.estimate_el_angle < 0) {
+			_hall.estimate_el_angle += PHASE_360_DEGREE;
+			}
 		}
 	}
-	return _hall.est_theta;
+	os_enable_irq();
+	return _hall.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_theta = override;
-	_hall.override_theta = theta;
+	_hall.is_override_angle = override;
+	_hall.override_el_angle = theta;
 }
 
 float hall_sensor_get_speed(void) {
+	_hall.rpm = _hall.el_speed / 360.0f * 60.0f;
 	return _hall.rpm;
 }
 
 float hall_sensor_avg_speed(void) {
-	return _hall.angle_speed_avg / 360 * 60.0f;
+	return _hall.el_speed_avg / 360 * 60.0f;
 }
 
 int hall_offset_increase(int inc) {
@@ -205,7 +230,7 @@ int hall_sensor_calibrate(float voltage, u16 *hall_table){
 			fast_norm_angle(&ang);
 			hall_table[i] = (u16)ang;
 		} else {
-			hall_table[i] = 0xFFFF;
+			hall_table[i] = THETA_NONE;
 			fails++;
 		}
 	}
@@ -279,52 +304,50 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 }
 
 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();
-
-	_hall.state = get_hall_stat(HALL_READ_TIMES);	
+	u32 ticks_now = timer_count32_get();	
 	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;
+			_hall.measured_el_angle = _hall_table[state_now];
+			_hall.estimate_el_angle = _hall.measured_el_angle;
 		}
 		return;
 	}
-	s32 theta_now = _hall_position(_hall.state, state_prev);
+	s32 theta_now = _hall_position(state_now, state_prev);
 	if (theta_now == 0xFFFFFFFF) {
-		_hall.state = state_prev;
+		sys_debug("hall error\n");
 		return;
 	}
 
 	u32 delta_us = timer_count32_getus(ticks_now, _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);
-
-	_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();
+	if (_hall.samples.full) {
+		_hall.el_speed_avg = _hall_avg_speed();
+		_hall.estimate_angle_aliment = true;
+	}else {
+		_hall.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
 	}
-	
-	_hall.theta = theta_now;	
+	_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_enable_irq();
-	
-	_hall.rpm = _hall.angle_speed / 360.0f * 60.0f;
 
 	time_measure_end(&g_meas_hall);
 }

Applications/foc/hall_sensor.h

@@ -23,29 +23,37 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
+#define SAMPLE_MAX_COUNT 6
+
+typedef struct {
+	u32   ticks[SAMPLE_MAX_COUNT];
+	u32   ticks_sum;
+	u32   index;
+	bool  full;
+}hall_sample_t;
+
 typedef struct {
-	bool  alignmnet;
-	s32   theta;
-	float est_theta;
+	bool  estimate_angle_aliment;
+	s32   measured_el_angle; //hall测量到的电角度
+	float estimate_el_angle; //60度区间内的估计电角度
+	float estimate_delta_angle;
+	float el_speed; 		//当前的电角速度, 单位：rad/s
+	float el_speed_avg;	//滤波后的电角速度
+	float el_speed_compensate; //电角速度补偿
+	//float el_angle_mod;
 	float rpm;        		//当前的电速度, 单位：RPM
-	float angle_speed; 		//当前的电角速度, 单位：rad/s
-	float angle_speed_avg;	//滤波后的电角速度
 	u8    state;
 	u32   ticks;
 	bool  working;
 	s8    direction;
 	u32   phase_offset;
-	bool  is_override_theta;
-	float override_theta;
+	bool  is_override_angle;
+	float override_el_angle;
+	hall_sample_t samples;
 }hall_t;
 
-#define SAMPLE_MAX_COUNT 6
 
-typedef struct {
-	u32   ticks[SAMPLE_MAX_COUNT];
-	u32   index;
-	bool  full;
-}hall_sample_t;
+
 
 void hall_sensor_init(void);
 void hall_sensor_clear(void);

Applications/libs/utils.h

@@ -26,6 +26,11 @@ static __inline__ u16 decode_u32(u8 *buff) {
 	return ((u32)(buff[3]) << 24 | decode_u24(buff));
 }
 
+static __inline__ void encode_u8(u8 *buff, u8 value)
+{
+	buff[0] = value;
+}
+
 static __inline__ void encode_u16(u8 *buff, u16 value)
 {
 	buff[0] = value;

Applications/os/port/cortex-m4.c

@@ -4,8 +4,6 @@
 void co_task_tick_init(int ticks){
 	/* setup systick timer for 1000Hz interrupts */
 	SysTick_Config(SystemCoreClock / ticks);
-	/* configure the systick handler priority */
-	NVIC_SetPriority(SysTick_IRQn, 0x00U);
 }
 
 u32 arch_stack_init(u32 *stack_base, u16 stack_size, void *fptr, void *p_arg) {

Applications/prot/can_foc_msg.c

@@ -37,4 +37,10 @@ void can_report_dq_current(int can, int id, int iq) {
 	can_send_message(get_request_can_id(can, 1), data, sizeof(data), 100);
 }
 
-
+void can_response_hall_offset(int can, int offset) {
+	u8 data[7];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset));
+	encode_u8(data + 2, 1);
+	encode_u32(data + 3, offset);
+	can_send_message(get_reponse_can_id(can), data, sizeof(data), 100);
+}

Applications/prot/can_foc_msg.h

@@ -5,5 +5,7 @@ void can_report_speed(int can, int rpm);
 void can_report_phase_current(int can, int iA, int iB, int iC);
 void can_report_phase_voltage(int can, int vA, int vB, int vC);
 void can_report_dq_current(int can, int id, int iq);
+void can_response_hall_offset(int can, int offset);
+
 #endif	/*_Can_Foc_Msg_H__ */