|
|
@@ -31,8 +31,14 @@ static hall_t g_hall;
|
|
|
measure_time_t g_meas_hall = {.exec_max_time = 6,};
|
|
|
|
|
|
#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
|
|
|
+#define HALL_MAX_TIME 1000000UL
|
|
|
|
|
|
-u32 stop_cnt = 0;
|
|
|
+static u32 stop_cnt = 0;
|
|
|
+static u32 g_delta_cnt = 0;
|
|
|
+static u32 g_trns_cnt = 0;
|
|
|
+static u32 g_min_delta;
|
|
|
+//static float g_low_off[6];
|
|
|
+//static u32 g_low_off_cnt[6];
|
|
|
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++) {
|
|
|
@@ -61,23 +67,25 @@ static void hall_init_low_pos(void) {
|
|
|
return;
|
|
|
}
|
|
|
g_hall.state = state;
|
|
|
- g_hall.prev_dir = g_hall.dir = POSITIVE;
|
|
|
- g_hall.low_res_pos = pos + 0.5f;
|
|
|
+ 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, float angle) {
|
|
|
+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->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->filled < SAMPLE_MAX_COUNT) {
|
|
|
+ s->filled++;
|
|
|
+ }
|
|
|
if (s->index >= SAMPLE_MAX_COUNT) {
|
|
|
- s->full = true;
|
|
|
s->index = 0;
|
|
|
}
|
|
|
}
|
|
|
@@ -85,17 +93,35 @@ static void __INLINE hall_put_sample(u32 ticks, float angle) {
|
|
|
|
|
|
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 s->angles_sum / us_2_s(s->ticks_sum);
|
|
|
+ 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, sp %d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors, stop_cnt);
|
|
|
+ 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,%d\n", stop_cnt, g_delta_cnt, g_trns_cnt, g_min_delta);
|
|
|
+ sys_debug("RPM: %f\n", hall_get_velocity());
|
|
|
+#if 0
|
|
|
+ if (g_low_off_cnt[0] < 10) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ int value[6];
|
|
|
+ u32 mask = cpu_enter_critical();
|
|
|
+ for (int i = 0; i < 6; i++) {
|
|
|
+ value[i] = (int)(g_low_off[i]/(float)g_low_off_cnt[i]);
|
|
|
+ g_low_off[i] = 0;
|
|
|
+ g_low_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]);
|
|
|
+#endif
|
|
|
}
|
|
|
|
|
|
-
|
|
|
void hall_init(void) {
|
|
|
g_hall.phase_offset = mc_conf()->m.encoder_offset;
|
|
|
g_hall.mot_poles = mc_conf()->m.poles;
|
|
|
@@ -103,15 +129,21 @@ void hall_init(void) {
|
|
|
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.samples.filled = 0;
|
|
|
g_hall.elec_angle_vel = 0;
|
|
|
+ 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] = 0;
|
|
|
- g_hall.samples.angles[i] = 0;
|
|
|
+ g_hall.samples.ticks[i] = HALL_MAX_TIME;
|
|
|
}
|
|
|
+ for (int i = 0; i < 6; i++) {
|
|
|
+ //g_low_off[i] = 0;
|
|
|
+ //g_low_off_cnt[i] = 0;
|
|
|
+ }
|
|
|
+ g_hall.samples.ticks_sum = HALL_MAX_TIME * SAMPLE_MAX_COUNT;
|
|
|
+
|
|
|
if (!g_hall.inited) {
|
|
|
g_hall.inited = true;
|
|
|
gpio_hall_init();
|
|
|
@@ -120,6 +152,14 @@ void hall_init(void) {
|
|
|
stop_cnt = 0;
|
|
|
}
|
|
|
|
|
|
+void hall_set_direction(s8 dir) {
|
|
|
+ if (dir == g_hall.dir) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ 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;
|
|
|
@@ -132,7 +172,7 @@ static float get_angle_diff(float a1, float a2) {
|
|
|
}
|
|
|
#endif
|
|
|
|
|
|
-static bool hall_update_low_pos(void) {
|
|
|
+static __INLINE bool hall_update_low_pos(void) {
|
|
|
u8 state = hall_read_state();
|
|
|
s16 pos = hall_2_pos[state];
|
|
|
if (pos == 7) {
|
|
|
@@ -140,9 +180,7 @@ static bool hall_update_low_pos(void) {
|
|
|
return false;
|
|
|
}
|
|
|
s16 pos_prev = hall_2_pos[g_hall.state];
|
|
|
- g_hall.state = state;
|
|
|
s16 delta_pos = pos - pos_prev;
|
|
|
- g_hall.low_res_pos = pos;
|
|
|
if (delta_pos != 0) {
|
|
|
s8 prev_dir = g_hall.dir;
|
|
|
if (delta_pos == 1 || delta_pos == -5) {
|
|
|
@@ -152,12 +190,20 @@ static bool hall_update_low_pos(void) {
|
|
|
g_hall.dir = NEGATIVE;
|
|
|
g_hall.prev_dir = prev_dir;
|
|
|
}else {
|
|
|
- //keep prev dir value
|
|
|
+ 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;
|
|
|
}
|
|
|
|
|
|
@@ -166,7 +212,11 @@ hall_t *hall_get(void) {
|
|
|
}
|
|
|
|
|
|
float hall_get_elec_angle(void) {
|
|
|
- float angle = g_hall.elec_angle + g_hall.phase_offset;
|
|
|
+ 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;
|
|
|
}
|
|
|
@@ -202,10 +252,10 @@ float hall_update_elec_angle(void) {
|
|
|
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.full && (delta_ticks / g_hall.last_delta_ticks >= 1.3f)) {
|
|
|
+ if ((g_hall.samples.filled > 1) && (delta_ticks / g_hall.last_delta_ticks >= 1.4f)) {
|
|
|
stop_cnt ++;
|
|
|
- g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.99f;
|
|
|
- if (g_hall.elec_angle_vel < 10) {
|
|
|
+ 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;
|
|
|
}
|
|
|
float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
|
|
|
@@ -223,47 +273,46 @@ 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);
|
|
|
- if (!g_hall.samples.full && g_hall.samples.index == 0) {
|
|
|
- if (delta_cnt <= 1000) {
|
|
|
- delta_cnt = 1000;
|
|
|
- }
|
|
|
+static __INLINE void hall_calc_mot_velocity(u32 delta_cnt) {
|
|
|
+ if (g_hall.samples.filled == 0) {
|
|
|
+ hall_put_sample(HALL_MAX_TIME/10);
|
|
|
+ return;
|
|
|
}
|
|
|
- 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.5f);
|
|
|
- }else {
|
|
|
- g_hall.elec_angle_vel = hall_elec_angle_vel();
|
|
|
+ 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;
|
|
|
- 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, 1.0f);
|
|
|
+ LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.9F);
|
|
|
}
|
|
|
|
|
|
|
|
|
float hall_offset_detect(float *off) {
|
|
|
return 0.0f;
|
|
|
}
|
|
|
-
|
|
|
+extern float foc_observer_sensorless_angle(void);
|
|
|
void HALL_IRQHandler(void) {
|
|
|
+ g_delta_cnt++;
|
|
|
+ //int pos_int;
|
|
|
+ //float delta;
|
|
|
u32 mask = cpu_enter_critical();
|
|
|
- u32 prev_ticks = g_hall.edge_ticks;
|
|
|
- if (!hall_update_low_pos()) {
|
|
|
+ u32 delta_cnt = time_delta_us(g_hall.edge_ticks, NULL);
|
|
|
+ if (delta_cnt < 300) {
|
|
|
+ g_hall.noise_errors++;
|
|
|
goto hall_end;
|
|
|
}
|
|
|
- if (time_delta_us(prev_ticks, NULL) == 0) {
|
|
|
- g_hall.noise_errors++;
|
|
|
+ if (!hall_update_low_pos()) {
|
|
|
goto hall_end;
|
|
|
}
|
|
|
+#if 0
|
|
|
+ pos_int = g_hall.low_res_pos;
|
|
|
+ delta = g_hall.low_res_pos * 60.0f - foc_observer_sensorless_angle();
|
|
|
+ norm_angle_deg(delta);
|
|
|
+ g_low_off_cnt[pos_int]++;
|
|
|
+ g_low_off[pos_int] += delta;
|
|
|
+#endif
|
|
|
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);
|
|
|
@@ -275,7 +324,7 @@ void HALL_IRQHandler(void) {
|
|
|
g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
|
|
}
|
|
|
#endif
|
|
|
- hall_calc_mot_velocity(prev_ticks);
|
|
|
+ hall_calc_mot_velocity(delta_cnt);
|
|
|
hall_end:
|
|
|
cpu_exit_critical(mask);
|
|
|
return;
|
kevin