add hall_sensor.* one copy

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

Applications/foc/hall_sensor_old.c

@@ -0,0 +1,400 @@
+#include <string.h>
+#include "bsp/bsp.h"
+#include "bsp/mc_hall_gpio.h"
+#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"
+#include "app/nv_storage.h"
+#include "bsp/timer_count32.h"
+
+
+#define HALL_READ_TIMES 3
+
+static void _hall_detect_task(void *args);
+static void _hall_init_el_angle(void);
+
+#define PWM_T  (0.000033f)
+
+#define HALL_PLACE_OFFSET (315)//(345) //315
+/* 
+100
+101
+001
+011
+010
+110
+4,5,1,3,2,6,4
+*/
+
+static hall_sensor_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)
+
+#define rand_angle(a) {if (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;else if (a < 0) a +=PHASE_360_DEGREE;};
+
+static float hall_speed[8];
+static float  hall_angle[8];
+static void __inline _hall_put_sample(u32 ticks, u8 hall) {
+	hall_sample_t *s = &_sensor_hander.samples[hall];
+	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;
+	}
+}
+
+static u32 __inline _hall_angle_us_speed(u8 hall){
+	hall_sample_t *s = &_sensor_hander.samples[hall];
+	if (s->ticks_sum == 0) {
+		return 0;
+	}
+
+	if (!s->full) {
+		return (s->ticks[s->index-1]);
+	}else {
+		return s->ticks_sum/SAMPLE_MAX_COUNT;
+	}
+}
+
+static float __inline _hall_angle_speed(void){
+	u32 sum = 0 ;
+	u32 num = 0;
+	for (int hall = 1; hall < 7; hall++) {
+		hall_sample_t *s = &_sensor_hander.samples[hall];
+		if (s->ticks_sum == 0) {
+			continue;
+		}
+	
+		if (!s->full) {
+			sum +=(s->ticks[s->index-1]);
+			num ++;
+		}else {
+			sum  += s->ticks_sum;
+			num += SAMPLE_MAX_COUNT;
+		}
+	}
+	return (float)PHASE_60_DEGREE * (float)num / us_2_s(sum);
+}
+
+
+static bool __inline _hall_data_empty(u8 hall) {
+	hall_sample_t *s = &_sensor_hander.samples[hall];
+	if ((!s->full) && (s->index == 0)){
+		return true;
+	}
+	return false;
+}
+static void hall_sensor_default(void) {
+	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();
+	co_task_create(_hall_detect_task, NULL, 512);
+}
+
+void hall_sensor_clear(void) {
+	hall_sensor_default();
+}
+
+
+void hall_debug_log(void) {
+	for (int i = 0; i < 8; i++) {
+		if (i != 0 && i != 7) {
+			sys_debug("hall speed: %d,  %f - %f, %d\n", i, hall_speed[i], hall_angle[i], _sensor_hander.sensor_error);
+		}
+	}
+	sys_debug("angle dir %d\n", _sensor_hander.direction);
+	
+}
+
+static void _hall_detect_task(void *args) {
+	while(1) {
+		if (_sensor_hander.el_speed != 0) {
+			u32 ticks_now = timer_count32_get();
+			u32 delta_us = timer_count32_getus(ticks_now, _sensor_hander.hall_ticks);
+			if (delta_us >= (1200*1000)) {
+				hall_sensor_clear();
+			}
+		}
+		co_task_delay(100);
+	}
+}
+
+float hall_sensor_get_theta(void){
+	if (_sensor_hander.is_override_angle) {
+		return _sensor_hander.override_el_angle;
+	}
+
+	u32 us_now = timer_count32_delta_us(_sensor_hander.estimate_time_ticks, NULL);
+	float ration = (float)us_now / (float)_sensor_hander.speed_us_for_estimate;
+	
+	float angle_step = (float)PHASE_60_DEGREE * ration;
+	_sensor_hander.estimate_delta_angle = angle_step;
+	if (angle_step >= PHASE_60_DEGREE) {
+		angle_step = PHASE_60_DEGREE;
+	}
+
+	if (_sensor_hander.direction == POSITIVE) {
+		_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle + angle_step;
+	}else {
+		_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle - angle_step;
+	}
+
+	rand_angle(_sensor_hander.estimate_el_angle);
+
+	//log_chan_value(1, (int)_sensor_hander.estimate_el_angle);
+
+	return _sensor_hander.estimate_el_angle;
+}
+
+
+void hall_sensor_set_theta(bool override, float theta){
+	_sensor_hander.is_override_angle = override;
+	_sensor_hander.override_el_angle = theta;
+}
+
+float hall_sensor_get_speed(void) {
+	_sensor_hander.rpm = _sensor_hander.el_speed / 360.0f * 60.0f;
+	return _sensor_hander.rpm;
+}
+
+float hall_sensor_avg_speed(void) {
+	return _sensor_hander.el_speed / 360 * 60.0f;
+}
+
+int hall_offset_increase(int inc) {
+	if (_sensor_hander.phase_offset + inc >= 360) {
+		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - 360;
+	}else {
+		_sensor_hander.phase_offset += inc;
+	}
+	return _sensor_hander.phase_offset;
+}
+
+
+int hall_sensor_calibrate(float voltage){
+	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 < 100;i++) {
+		foc_set_dq_command((float)i * voltage / 100.0f, 0.0f);
+		co_task_delay(1);
+		wdog_reload();
+	}
+	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));
+	co_task_delay(2 * 1000);
+	// Forwards
+	for (int i = 0;i < 5;i++) {
+		for (int j = 0;j < 360;j++) {
+			hall_sensor_set_theta(true, j);
+			co_task_delay(50);
+			wdog_reload();
+			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]++;
+		}
+	}
+	//hall_sensor_set_theta(true, 360);
+	//co_task_delay(2 * 1000);
+	sys_debug("Revers\n");
+	// Reverse
+	for (int i = 0;i < 5;i++) {
+		for (int j = 360;j >= 0;j--) {
+			hall_sensor_set_theta(true, j);
+			co_task_delay(50);
+			wdog_reload();
+			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) {
+		} else {
+			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_180_DEGREE + PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_6:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + PHASE_60_DEGREE / 2;
+      	break;
+    case STATE_4:
+		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + PHASE_60_DEGREE / 2;
+      	break;
+    default:
+      	/* Bad hall sensor configutarion so update the speed reliability */
+      	_sensor_hander.sensor_error ++;
+      	break;
+ 	}
+  	/* Initialize the measured angle */
+	rand_angle(_sensor_hander.measured_el_angle);
+  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
+	_sensor_hander.hall_ticks = timer_count32_get();
+}
+
+/* 4,5,1,3,2,6,4 */
+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) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
+			}else if (state_prev == STATE_3) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
+			}
+			break;
+		case STATE_2:
+			if (state_prev == STATE_3) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
+			}else if (state_prev == STATE_6) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_240_DEGREE;
+			}
+			break;
+		case STATE_3:
+			if (state_prev == STATE_1) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
+			}else if (state_prev == STATE_2) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
+			}
+			break;
+		case STATE_4:
+			if (state_prev == STATE_6) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_300_DEGREE;
+			}else if (state_prev == STATE_5) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset;
+			}
+			break;
+		case STATE_5:
+			if (state_prev == STATE_4) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset;
+			}else if (state_prev == STATE_1) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
+			}
+			break;
+		case STATE_6:
+			if (state_prev == STATE_2) {
+				_sensor_hander.direction = POSITIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_240_DEGREE;
+			}else if (state_prev == STATE_4) {
+				_sensor_hander.direction = NEGATIVE;
+				theta_now = _sensor_hander.phase_offset + PHASE_300_DEGREE;
+			}
+			break;
+		default:
+			_sensor_hander.sensor_error ++;
+			return 0xFFFFFFFF;
+	}
+	rand_angle(theta_now);
+	return theta_now;
+}
+
+void hall_sensor_handler(void) {
+	if (_sensor_hander.is_override_angle) {
+		sys_debug("%d:%d\n", (int)get_hall_stat(HALL_READ_TIMES), (int)_sensor_hander.override_el_angle);
+		return;
+	}
+	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 = timer_count32_get();	
+
+	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
+	if (theta_now == 0xFFFFFFFF) {
+		return;
+	}
+
+	u32 delta_us = timer_count32_getus(hall_ticks_now, _sensor_hander.hall_ticks);
+	if (delta_us == 0) {
+		return;
+	}
+
+	float delta_time = us_2_s(delta_us);
+	_hall_put_sample(delta_us, hall_stat_prev);
+	u32 us_speed_prev = _sensor_hander.speed_us_for_estimate;
+	u32 us_speed_now = 0;
+	if (_hall_data_empty(hall_stat_now)) {
+		us_speed_now = delta_us;
+	}else {
+		us_speed_now = _hall_angle_us_speed(hall_stat_now);
+	}
+	if (us_speed_now != 0) {
+		hall_speed[hall_stat_now] = (float)us_speed_prev / (float)us_speed_now;
+	}
+	hall_angle[hall_stat_now] = (float)_sensor_hander.estimate_delta_angle/(float)PHASE_60_DEGREE;
+	os_disable_irq();
+	
+	_sensor_hander.estimate_delta_angle = 0;
+	_sensor_hander.measured_el_angle = theta_now;	
+	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
+
+	_sensor_hander.estimate_time_ticks = hall_ticks_now;
+	_sensor_hander.speed_us_for_estimate = us_speed_now;
+	os_enable_irq();
+	_sensor_hander.el_speed = _hall_angle_speed();
+	_sensor_hander.hall_stat = hall_stat_now;
+	_sensor_hander.hall_ticks = hall_ticks_now;
+	
+	time_measure_end(&g_meas_hall);
+}
+
+

Applications/foc/hall_sensor_old.h

@@ -0,0 +1,67 @@
+#ifndef _HALL_SENSOR_H__
+#define _HALL_SENSOR_H__
+#include "os/os_type.h"
+#include "bsp/bsp.h"
+
+#define NEGATIVE          (int8_t)-1
+#define POSITIVE          (int8_t)1
+
+#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
+#define STATE_2 (uint8_t)2
+#define STATE_3 (uint8_t)3
+#define STATE_4 (uint8_t)4
+#define STATE_5 (uint8_t)5
+#define STATE_6 (uint8_t)6
+#define STATE_7 (uint8_t)7
+
+#define THETA_NONE        (float)0xFFFF
+#define SAMPLE_MAX_COUNT 3
+
+typedef struct {
+	u32   ticks[SAMPLE_MAX_COUNT];
+	u32   ticks_sum;
+	u32   index;
+	bool  full;
+}hall_sample_t;
+
+typedef struct {
+	float estimate_el_angle; //60度区间内的估计电角度
+	u32   estimate_time_ticks;
+	s32   estimate_delta_angle;//for debug
+	s32   measured_el_angle; //hall测量到的电角度
+	u32   speed_us_for_estimate; //hall经过60°的时间，给角度估计使用
+	float el_speed; 		//当前的电角速度, 单位：rad/s
+	float rpm;        		//当前的电速度, 单位：RPM
+	u8    hall_stat;
+	u32   hall_ticks;
+	bool  working;
+	s8    direction;
+	s32   phase_offset;
+	bool  is_override_angle;
+	float override_el_angle;
+	hall_sample_t samples[8];
+	u32  sensor_error;
+}hall_sensor_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);
+int hall_sensor_calibrate(float voltage);
+void hall_sensor_set_theta(bool override, float theta);
+int hall_offset_increase(int inc);
+
+#endif /* _HALL_SENSOR_H__ */
+