加入hall相移的记录和打印，通过HALL_DETECT_OFFSET宏开关

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

Applications/foc/motor/hall.c

@@ -9,11 +9,9 @@
 #include "app/nv_storage.h"
 #include "libs/logger.h"
 
-//#define USE_DETECTED_ANGLE 1
-
 #define HALL_READ_TIMES 5
 #define SMOOTH_COUNT 0
-
+#define HALL_DETECT_OFFSET 0
 /* 
 1,5,4,6,2,3
 0,1,2,3,4,5
@@ -36,6 +34,47 @@ static u32 g_min_delta;
 
 #define COUNT_2_US(c) (c/(SYSTEM_CLOCK/1000000))
 
+#if HALL_DETECT_OFFSET > 0
+static float hall_off[6];
+static u32	 hall_off_cnt[6];
+
+static void hall_offset_dec_init(void) {
+	for (int i = 0; i < 6; i++) {
+		hall_off[i] = 0;
+		hall_off_cnt[i] = 0;
+	}
+}
+float mc_get_mot_angle(void);
+static __INLINE void hall_offset_update(void) {
+	int pos_int = (int)g_hall.low_res_pos;
+	float hall_pos = g_hall.low_res_pos * PHASE_60_DEGREE;
+	float delta_pos = hall_pos - (mc_get_mot_angle() + 90.0f);
+	norm_angle_deg(delta_pos);
+	hall_off[pos_int] = delta_pos;
+	hall_off_cnt[pos_int] += 1;
+}
+static void hall_offset_log(void) {
+	if (hall_off_cnt[0] < 20) {
+		return;
+	}
+	s16 value[6];
+	u32 mask = cpu_enter_critical();
+	for (int i = 0; i < 6; i++) {
+		value[i] = (s16)(hall_off[i]/(float)hall_off_cnt[i]);
+		hall_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]);
+}
+
+#else
+static void hall_offset_dec_init(void) {
+}
+static __INLINE void hall_offset_update(void) {
+}
+static void hall_offset_log(void) {
+}
+#endif
 static __INLINE u32 hall_delta_us(u32 count) {
 	u32 now = task_ticks_abs();
 	u32 delta = now - count;
@@ -113,6 +152,7 @@ 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\n", g_delta_cnt, g_trns_cnt, g_min_delta);
 	sys_debug("RPM: %f\n", hall_get_velocity());
+	hall_offset_log();
 }
 
 void hall_init(void) {
@@ -137,6 +177,7 @@ void hall_init(void) {
 		g_hall.inited = true;
 		gpio_hall_init();
 	}
+	hall_offset_dec_init();
 	hall_init_low_pos();
 }
 
@@ -297,6 +338,7 @@ void HALL_IRQHandler(void) {
 	if (!hall_update_low_pos()) {
 		goto hall_end;
 	}
+	hall_offset_update();
 	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);

Applications/foc/motor/motor.c

@@ -800,6 +800,10 @@ void mc_use_throttle(void) {
 	motor.b_ignor_throttle = false;
 }
 
+float mc_get_mot_angle(void) {
+	return motor.controller.foc.in.mot_angle;
+}
+
 void mc_get_running_status(u8 *data) {
 	data[0] = motor.mode;
 	data[0] |= (mot_contrl_is_auto_holdding(&motor.controller)?1:0) << 2;