MC100/Applications/foc/motor/hall.c

#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 "libs/time_measure.h"
#include "app/nv_storage.h"
#include "libs/logger.h"

//#define USE_DETECTED_ANGLE 1

#define HALL_READ_TIMES 9
#define SMOOTH_COUNT 10.0F

/* 
4,5,1,3,2,6,4
*/
static s8 hall_2_pos[] = {7,5,1,0,3,4,2,7};
static hall_t g_hall;

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
#define HALL_TIMEOUT_US (1*1000000L)


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++) {
		if (gpio_hall_a_value()) {
			hall_a++;
		}
		if (gpio_hall_b_value()) {
			hall_b++;
		}
		if (gpio_hall_c_value()) {
			hall_c++;
		}
	}
	u8 state = 0;
	if (hall_a > (HALL_READ_TIMES/2 + 1)) {
		state = 1<<2;
	}
	if (hall_b > (HALL_READ_TIMES/2 + 1)) {
		state = state | (1<<1);
	}
	if (hall_c > (HALL_READ_TIMES/2 + 1)) {
		state = state | 1;
	}
	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.errors ++;
		return;
	}
	g_hall.state = state;
	g_hall.prev_dir = g_hall.dir = POSITIVE;
	g_hall.low_res_pos = pos;
}


static void __INLINE hall_put_sample(u32 ticks, float angle) {
	hsample_t *s = &g_hall.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 float __INLINE hall_elec_angle_vel(void){
	hsample_t *s = &g_hall.samples;
	if (s->ticks_sum == 0) {
		return 0.0f;
	}
	return s->angles_sum / us_2_s(s->ticks_sum);
}

void hall_debug_log(void) {
	sys_debug("angle dir %d, stat %d, lowres %d, err %d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.errors);
}

static u32 hall_timeout_task(void *args) {
	hall_t *phall = (hall_t *)args;
	if (phall->velocity_raw != 0) {
		if (time_delta_us(phall->edge_ticks, NULL) >= HALL_TIMEOUT_US) {
			phall->velocity_raw = phall->velocity_filted = 0;
		}
	}
	return 0;
}

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.samples.ticks_sum = 0;
	g_hall.position = 0;
	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
		g_hall.samples.ticks[i] = 120*1000000*1;
		g_hall.samples.angles[i] = 0;
		g_hall.samples.ticks_sum += g_hall.samples.ticks[i];
	}
	if (!g_hall.inited) {
		g_hall.inited = true;
		gpio_hall_init();
		shark_task_create(hall_timeout_task, &g_hall);
	}
	hall_init_low_pos();
}

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;
	}
}

static bool hall_update_low_pos(void) {
	u8 state = hall_read_state();
	s16 pos = hall_2_pos[state];
	if (pos == 7) {
		g_hall.errors ++;
		return false;
	}
	g_hall.state = state;
	s16 delta_pos = pos - g_hall.low_res_pos;
	g_hall.low_res_pos = pos;
	s8  prev_dir = g_hall.dir;
	if (delta_pos == 1 || delta_pos == -5) {
		g_hall.dir = POSITIVE;
	}else{
		g_hall.dir = NEGATIVE;
	}
	if (delta_pos != 0) {
		g_hall.edge_ticks = task_ticks_abs();
		g_hall.prev_dir = prev_dir;
	}
	return true;
}

hall_t *hall_get(void) {
	return &g_hall;
}

float hall_get_elec_angle(void) {
	float angle = g_hall.elec_angle + g_hall.phase_offset;
	norm_angle_deg(angle);
	return angle;
}

float hall_update_elec_angle(void) {
	float delta_ticks = (float)time_delta_us(g_hall.edge_ticks, NULL);//上次hall变换到目前的时间
	float low_res = g_hall.low_res_pos;
	if (g_hall.dir == NEGATIVE) {
		low_res += 1.0f;
	}
	float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
	float high_res_pos = delta_pos  + low_res;
	if (high_res_pos < 0) {
		high_res_pos = 0;
	}
	float elec_angle = high_res_pos * PHASE_60_DEGREE;
	float delta_angle = delta_pos * PHASE_60_DEGREE;
	float elec_smooth_angle;
	if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
		elec_smooth_angle = g_hall.delta_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;
	}
	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;
	return hall_get_elec_angle();
}

float hall_get_velocity(void) {
	return g_hall.velocity_filted;
}

float hall_get_position(void) {
	return g_hall.position;
}

static void hall_calc_mot_velocity(u32 prev_ticks) {
	u32 delta_cnt = time_delta_us(prev_ticks, NULL);
	hall_put_sample(PHASE_60_DEGREE, delta_cnt);
	float elec_vel;
	if (g_hall.b_trns_det) {
		elec_vel = PHASE_60_DEGREE/(us_2_s(delta_cnt));
		LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 0.8f);
	}else {
		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;
	float del_abs = ABS(velocity_raw - g_hall.velocity_raw);
	if (del_abs > 140) {
		g_hall.b_trns_det = true;
	}else if (del_abs < 100) {
		g_hall.b_trns_det = false;
	}
	g_hall.velocity_raw = velocity_raw;
	LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.5f);
}


float hall_offset_detect(float *off) {
	return 0.0f;
}

void HALL_IRQHandler(void) {
	u32 prev_ticks = g_hall.edge_ticks;
	if (!hall_update_low_pos()) {
		return;
	}
	g_hall.elec_angle_edge = g_hall.elec_angle;
	float low_res = g_hall.low_res_pos;
	if (g_hall.dir == NEGATIVE) {
		low_res += 1.0f;
	}
	g_hall.delta_angle_edge = get_angle_diff(low_res * 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;
	}
	hall_calc_mot_velocity(prev_ticks);
}