|
|
@@ -6,13 +6,12 @@
|
|
|
#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 HALL_READ_TIMES 5
|
|
|
#define SMOOTH_COUNT 0
|
|
|
|
|
|
/*
|
|
|
@@ -28,8 +27,6 @@
|
|
|
static s8 hall_2_pos[] = {7,2,0,1,4,3,5,7};
|
|
|
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
|
|
|
|
|
|
@@ -37,8 +34,19 @@ 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];
|
|
|
+
|
|
|
+#define COUNT_2_US(c) (c/(SYSTEM_CLOCK/1000000))
|
|
|
+
|
|
|
+static __INLINE u32 hall_delta_us(u32 count) {
|
|
|
+ u32 now = task_ticks_abs();
|
|
|
+ u32 delta = now - count;
|
|
|
+ if (now < count) { //wrap
|
|
|
+ delta = 0xFFFFFFFF - count + now + 1;
|
|
|
+ }
|
|
|
+ return COUNT_2_US(delta);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
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++) {
|
|
|
@@ -82,12 +90,12 @@ static void __INLINE hall_put_sample(u32 ticks) {
|
|
|
s->ticks[s->index] = ticks;
|
|
|
s->ticks_sum += s->ticks[s->index];
|
|
|
s->index += 1;
|
|
|
- if (s->filled < SAMPLE_MAX_COUNT) {
|
|
|
- s->filled++;
|
|
|
- }
|
|
|
if (s->index >= SAMPLE_MAX_COUNT) {
|
|
|
s->index = 0;
|
|
|
}
|
|
|
+ if (s->filled < SAMPLE_MAX_COUNT) {
|
|
|
+ s->filled++;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -106,20 +114,6 @@ void hall_debug_log(void) {
|
|
|
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) {
|
|
|
@@ -138,10 +132,6 @@ void hall_init(void) {
|
|
|
for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
|
|
|
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) {
|
|
|
@@ -222,7 +212,7 @@ 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 delta_ticks = (float)hall_delta_us(g_hall.edge_ticks);//上次hall变换到目前的时间
|
|
|
float low_res = g_hall.low_res_pos;
|
|
|
float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
|
|
|
if (delta_pos > 1.0f) {
|
|
|
@@ -259,8 +249,7 @@ float hall_update_elec_angle(void) {
|
|
|
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;
|
|
|
- g_hall.velocity_raw = velocity_raw;
|
|
|
- LowPass_Filter(g_hall.velocity_filted, velocity_raw, 1.0f);
|
|
|
+ g_hall.velocity_filted = velocity_raw;
|
|
|
}
|
|
|
return hall_get_elec_angle();
|
|
|
}
|
|
|
@@ -284,7 +273,6 @@ static __INLINE void hall_calc_mot_velocity(u32 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;
|
|
|
- g_hall.velocity_raw = velocity_raw;
|
|
|
LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.9F);
|
|
|
}
|
|
|
|
|
|
@@ -295,10 +283,8 @@ float hall_offset_detect(float *off) {
|
|
|
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 delta_cnt = time_delta_us(g_hall.edge_ticks, NULL);
|
|
|
+ u32 delta_cnt = hall_delta_us(g_hall.edge_ticks);
|
|
|
if (delta_cnt < 300) {
|
|
|
g_hall.noise_errors++;
|
|
|
goto hall_end;
|
|
|
@@ -306,19 +292,12 @@ void HALL_IRQHandler(void) {
|
|
|
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);
|
|
|
if (ABS(g_hall.delta_angle_edge) >= 2.0f) {
|
|
|
- g_hall.angle_smooth_step = 0;//g_hall.delta_angle_edge/SMOOTH_COUNT;
|
|
|
- g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
|
|
+ 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;
|
kevin