|
|
@@ -12,20 +12,21 @@
|
|
|
|
|
|
//#define USE_DETECTED_ANGLE 1
|
|
|
|
|
|
-#define HALL_READ_TIMES 9
|
|
|
+#define HALL_READ_TIMES 5
|
|
|
#define SMOOTH_COUNT 10.0F
|
|
|
|
|
|
/*
|
|
|
-4,5,1,3,2,6,4
|
|
|
+1,5,4,6,2,3
|
|
|
+0,1,2,3,4,5
|
|
|
*/
|
|
|
-static s8 hall_2_pos[] = {7,5,1,0,3,4,2,7};
|
|
|
+static s8 hall_2_pos[] = {7,0,4,5,2,1,3,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)
|
|
|
+#define HALL_TIMEOUT_US (1000000L/4)
|
|
|
|
|
|
|
|
|
static u8 __INLINE hall_read_state(void) {
|
|
|
@@ -58,12 +59,12 @@ static void hall_init_low_pos(void) {
|
|
|
u8 state = hall_read_state();
|
|
|
s16 pos = hall_2_pos[state];
|
|
|
if (pos == 7) {
|
|
|
- g_hall.errors ++;
|
|
|
+ g_hall.sig_errors ++;
|
|
|
return;
|
|
|
}
|
|
|
g_hall.state = state;
|
|
|
g_hall.prev_dir = g_hall.dir = POSITIVE;
|
|
|
- g_hall.low_res_pos = pos;
|
|
|
+ g_hall.low_res_pos = pos + 0.5f - g_hall.phase_offset/PHASE_60_DEGREE;
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -92,13 +93,14 @@ static float __INLINE hall_elec_angle_vel(void){
|
|
|
}
|
|
|
|
|
|
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);
|
|
|
+ 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);
|
|
|
}
|
|
|
|
|
|
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->elec_angle_vel = 0;
|
|
|
phall->velocity_raw = phall->velocity_filted = 0;
|
|
|
}
|
|
|
}
|
|
|
@@ -110,12 +112,16 @@ void hall_init(void) {
|
|
|
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.samples.angles_sum = 0;
|
|
|
g_hall.position = 0;
|
|
|
+ g_hall.samples.full = false;
|
|
|
+ g_hall.samples.index = 0;
|
|
|
+ g_hall.elec_angle_vel = 0;
|
|
|
for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
|
|
|
- g_hall.samples.ticks[i] = 120*1000000*1;
|
|
|
+ g_hall.samples.ticks[i] = 0;
|
|
|
g_hall.samples.angles[i] = 0;
|
|
|
- g_hall.samples.ticks_sum += g_hall.samples.ticks[i];
|
|
|
}
|
|
|
if (!g_hall.inited) {
|
|
|
g_hall.inited = true;
|
|
|
@@ -139,19 +145,21 @@ static bool hall_update_low_pos(void) {
|
|
|
u8 state = hall_read_state();
|
|
|
s16 pos = hall_2_pos[state];
|
|
|
if (pos == 7) {
|
|
|
- g_hall.errors ++;
|
|
|
+ g_hall.sig_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) {
|
|
|
+ s8 prev_dir = g_hall.dir;
|
|
|
+ if (g_hall.samples.full || g_hall.samples.index != 0) {
|
|
|
+ if (delta_pos == 1 || delta_pos == -5) {
|
|
|
+ g_hall.dir = POSITIVE;
|
|
|
+ }else{
|
|
|
+ g_hall.dir = NEGATIVE;
|
|
|
+ }
|
|
|
+ }
|
|
|
g_hall.edge_ticks = task_ticks_abs();
|
|
|
g_hall.prev_dir = prev_dir;
|
|
|
}
|
|
|
@@ -171,14 +179,13 @@ float hall_get_elec_angle(void) {
|
|
|
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;
|
|
|
+ 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 delta_angle = delta_pos * PHASE_60_DEGREE;
|
|
|
float elec_smooth_angle;
|
|
|
@@ -209,11 +216,16 @@ float hall_get_position(void) {
|
|
|
|
|
|
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);
|
|
|
+ if (!g_hall.samples.full && g_hall.samples.index == 0) {
|
|
|
+ if (delta_cnt <= 1000) {
|
|
|
+ delta_cnt = 1000;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ hall_put_sample(delta_cnt, PHASE_60_DEGREE);
|
|
|
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);
|
|
|
+ LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 1.0f);
|
|
|
}else {
|
|
|
g_hall.elec_angle_vel = hall_elec_angle_vel();
|
|
|
}
|
|
|
@@ -225,7 +237,7 @@ static void hall_calc_mot_velocity(u32 prev_ticks) {
|
|
|
g_hall.b_trns_det = false;
|
|
|
}
|
|
|
g_hall.velocity_raw = velocity_raw;
|
|
|
- LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.5f);
|
|
|
+ LowPass_Filter(g_hall.velocity_filted, velocity_raw, 1.0f);
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -238,15 +250,16 @@ void HALL_IRQHandler(void) {
|
|
|
if (!hall_update_low_pos()) {
|
|
|
return;
|
|
|
}
|
|
|
+ if (time_delta_us(prev_ticks, NULL) == 0) {
|
|
|
+ g_hall.noise_errors++;
|
|
|
+ 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;
|
|
|
+ g_hall.angle_smooth_step = 0;//g_hall.delta_angle_edge/SMOOTH_COUNT;
|
|
|
+ g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
|
|
}else {
|
|
|
g_hall.angle_smooth_step = 0;
|
|
|
g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
kevin