重构encoder的校准

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

Applications/app/nv_storage.c

@@ -20,8 +20,8 @@ void nv_save_hall_table(s32 *hall_table) {
 	nv_save_motor_params();
 }
 
-void nv_save_hall_offset(s16 offset) {
-	m_params.hall_offset = offset;
+void nv_save_angle_offset(float offset) {
+	m_params.offset = offset;
 	nv_save_motor_params();
 }
 
@@ -31,11 +31,10 @@ static void nv_default_motor_params(void) {
 	m_params.r = MOTOR_R;
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
-	m_params.encoder_offset = (360-128);
+	m_params.offset = (360-128);
 	m_params.est_pll_band = 200;
 	m_params.pos_lock_pll_band = 200;
 	m_params.flux_linkage = 0.0f;
-	m_params.hall_offset = 0;
 }
 
 static void nv_default_foc_params(void) {

Applications/app/nv_storage.h

@@ -33,9 +33,8 @@ typedef struct {
 	float lq;
 	float flux_linkage;
 	float back_emf;
-	s16   hall_offset;
 	s32   hall_table[8];
-	float encoder_offset;
+	float offset;
 	float est_pll_band; //normal工作模式下的pll带宽
 	float pos_lock_pll_band; //电机锁定模式下的pll带宽
 	u16   crc16;
@@ -60,7 +59,7 @@ typedef struct {
 void nv_storage_init(void);
 motor_params_t *nv_get_motor_params(void);
 foc_params_t *nv_get_foc_params(void);
-void nv_save_hall_offset(s16 offset);
+void nv_save_angle_offset(float offset);
 void nv_save_motor_params(void);
 void nv_read_motor_params(void);
 void nv_save_foc_params(void);

Applications/foc/commands.c

@@ -81,7 +81,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Cali_Hall_Phase:
 		{
 			s16 vd = decode_s16((u8 *)command->data);
-			mc_encoder_calibrate((vd));
+			mc_encoder_off_calibrate((vd));
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:

Applications/foc/motor/encoder.c

@@ -6,25 +6,12 @@
 #include "libs/logger.h"
 #include "app/nv_storage.h"
 #include "math/fast_math.h"
-
 #include "encoder_off.h"
 
-#define ANGLE_OFFSET (-50.0f)//133.0f
 /* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
-#define DIR_ADJUGE_MAX_CNT 10
-#define PLL_BANDWIDTH 200
-encoder_t g_encoder;
 
-u8   encoder_off_count[1024] = {0};
-s16  encoder_off_map[1024] = {0};
-s16  encoder_off_r_map[1024] = {0};
-u8   encoder_off_r_count[1024] = {0};
+encoder_t g_encoder;
 
-bool encoder_off_finished = false;
-s16  encoder_off_comp = 940.0f;
-float encoder_off_mul = 1.0f;
-int encoder_off_cn_add = 542;
-float encoder_off_angle_add = 0.0f;
 static __INLINE void encoder_pll_update_gain(void) {
 	if (g_encoder.pll_bandwidth_shadow != g_encoder.pll_bandwidth) {
 		g_encoder.pll_bandwidth = g_encoder.pll_bandwidth_shadow;
@@ -44,18 +31,12 @@ static void _init_pll(void) {
 
 
 void encoder_init(void) {
-	for (int i = i; i < 1024; i++) {
-		encoder_off_count[i] = 0;
-		encoder_off_map[i] = 0;
-		encoder_off_r_map[i] = 0;
-		encoder_off_r_count[i] = 0;
-	}
 	encoder_init_clear(POSITIVE);
 	enc_intf_init(ENC_MAX_RES);
 }
 
 void encoder_set_direction(s8 direction) {
-	encoder_init_clear(direction);
+	g_encoder.direction = direction;
 }
 
 void encoder_set_bandwidth(float bandwidth) {
@@ -65,7 +46,7 @@ void encoder_set_bandwidth(float bandwidth) {
 void encoder_init_clear(s8 diretcion) {
 	_init_pll();
 	g_encoder.cpr = ENC_MAX_RES;
-	g_encoder.enc_offset = nv_get_motor_params()->encoder_offset;
+	g_encoder.enc_offset = nv_get_motor_params()->offset;
 	g_encoder.motor_poles = nv_get_motor_params()->poles;
 	g_encoder.b_index_found = false;
 	g_encoder.direction = diretcion;
@@ -98,7 +79,7 @@ static __INLINE float _pll_over_comp(void) {
 	return (float)g_encoder.cpr;
 }
 
-static __INLINE void encoder_run_pll(float cnt) {
+static __INLINE bool encoder_run_pll(float cnt) {
 	float pll_comp = 0.0f;
 	if (g_encoder.b_timer_ov) {
 		pll_comp = _pll_over_comp();
@@ -107,6 +88,12 @@ static __INLINE void encoder_run_pll(float cnt) {
 	encoder_pll_update_gain();
 	g_encoder.est_vel_counts = PLL_run(&g_encoder.est_pll, cnt, pll_comp);
 	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
+    bool snap_to_zero_vel = false;
+    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
+        g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
+        snap_to_zero_vel = true;
+    }
+	return snap_to_zero_vel;
 }
 
 static __INLINE u32 _abi_count(void) {
@@ -117,6 +104,19 @@ static __INLINE u32 _abi_count(void) {
 #endif
 }
 
+/* 偏心补偿 */
+static __INLINE float _eccentricity_compensation(int cnt) {
+#ifdef FIR_PHASE_SHIFT
+	int cnt_off = (cnt + FIR_PHASE_SHIFT) % g_encoder.cpr;
+	if (g_encoder.encoder_off_map != NULL) { //do offset calibrate, can not do encentricity compensation
+		return 0.0f;
+	}
+	return -((_encoder_off_map[cnt_off])/100.0f);
+#else
+	return 0.0f;
+#endif
+}
+
 float encoder_get_theta(void) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
@@ -128,12 +128,9 @@ float encoder_get_theta(void) {
 		g_encoder.b_timer_ov = true;
 		ENC_ClearUpFlags();
 	}
-	encoder_run_pll((float)(cnt));
-    bool snap_to_zero_vel = false;
-    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
-        g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
-        snap_to_zero_vel = true;
-    }
+	
+    bool snap_to_zero_vel = encoder_run_pll((float)(cnt));
+
 	if (snap_to_zero_vel) {
 		g_encoder.interpolation = 0.5f;
 	}else {
@@ -149,13 +146,9 @@ float encoder_get_theta(void) {
 		}
 	}
 	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
-	if (encoder_off_finished) {
-		int cnt_off = (cnt + FIR_PHASE_SHIFT) % g_encoder.cpr;
-		g_encoder.abi_angle -= ((_encoder_off_map[cnt_off])/100.0f);
-	}
+	g_encoder.abi_angle += _eccentricity_compensation(cnt);
 	rand_angle(g_encoder.abi_angle);
 
-	
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = timer_count32_get();
 	return g_encoder.abi_angle;
@@ -166,8 +159,30 @@ float encoder_get_speed(void) {
 }
 
 
-extern int jtag_plot;
-void encoder_detect_offset(float angle, bool r){
+void _encoder_caliberate_init(void) {
+	if (g_encoder.encoder_off_map != NULL) {
+		return;
+	}
+	g_encoder.encoder_off_map = (s16 *)os_alloc(g_encoder.cpr * sizeof(s16));
+	g_encoder.encoder_off_count = (u8 *)os_alloc(g_encoder.cpr);
+	
+	for (int i = 0; i < g_encoder.cpr; i++) {
+		g_encoder.encoder_off_map[i] = 0;
+		g_encoder.encoder_off_count[i] = 0;
+	}
+}
+
+void _encoder_caliberate_deinit(void) {
+	if (g_encoder.encoder_off_map != NULL) {
+		os_free(g_encoder.encoder_off_map);
+		os_free(g_encoder.encoder_off_count);
+	}
+	g_encoder.encoder_off_map = NULL;
+	g_encoder.encoder_off_count = NULL;
+}
+
+void encoder_detect_offset(float angle){
+	_encoder_caliberate_init();
 	float delta = (g_encoder.abi_angle - angle);
 	if (delta > 200) {
 		delta = delta - 360;
@@ -175,57 +190,53 @@ void encoder_detect_offset(float angle, bool r){
 	if (delta < -200) {
 		delta = delta + 360;
 	}
-	int last_cn = (g_encoder.last_cnt + 0) % 1024;
-	if (!r) {
-		encoder_off_map[last_cn] = (s16)(delta*100.0f);
-		encoder_off_count[last_cn] ++;
+	if (g_encoder.direction == POSITIVE) {
+		if ((g_encoder.encoder_off_count[g_encoder.last_cnt] & 0xF) <= 2) {
+			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
+			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x01;
+		}
 	}else {
-		encoder_off_r_map[last_cn] = (s16)(delta*100.0f);
-		encoder_off_r_count[last_cn] ++;
-	}
-	if (jtag_plot==1) {
-		plot_1data16((s16)(delta*100.0f));
+		if (((g_encoder.encoder_off_count[g_encoder.last_cnt] >> 4) & 0xF) <= 2) {
+			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
+			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x10;
+		}
 	}
-	encoder_off_finished = false;
 }
-
-
-float encoder_detect_finish(bool r) {
+static void _detect_off_finished(void);
+bool encoder_detect_finish(void) {
+	u8 off_count = 0;
 	for (int i = 0; i < 1024; i++) {
-		if (!r) {
-			if (encoder_off_count[i] <= 2) {
-				return false;
-			}
+		if (g_encoder.direction == POSITIVE) {
+			off_count = g_encoder.encoder_off_count[i] & 0xF;
 		}else {
-			if (encoder_off_r_count[i] <= 2) {
-				return false;
-			}
+			off_count = (g_encoder.encoder_off_count[i] >> 4)& 0xF;
+		}
+		if (off_count <= 2) {
+			return false;
 		}
 	}
-	encoder_off_finished = true;
-	return encoder_off_finished;
+	if (g_encoder.direction == NEGATIVE) {
+		_detect_off_finished();//output data to PC tools, and use Matlab do FIR filter
+		_encoder_caliberate_deinit();
+	}
+	return true;
 }
 
 
-void encoder_detect_off_finished(void) {
-	for (int i = 0; i < 100; i++) {
-		plot_3data16(0,0,1000);
-		delay_ms(2);
-	}
+static void _detect_off_finished(void) {
 	for (int i = 0; i < 1024; i++) {
-		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
-		plot_3data16(0, 0 , angle_off);
+		float angle_off = g_encoder.encoder_off_map[i] / (((g_encoder.encoder_off_count[i] >> 4)&0xF) + (g_encoder.encoder_off_count[i]&0xF));
+		plot_1data16((s16)angle_off);
 		delay_ms(2);
 	}
 	for (int i = 0; i < 1024; i++) {
-		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
-		plot_3data16(0, 0 , angle_off);
+		float angle_off =  g_encoder.encoder_off_map[i] / (((g_encoder.encoder_off_count[i] >> 4)&0xF) + (g_encoder.encoder_off_count[i]&0xF));
+		plot_1data16((s16)angle_off);
 		delay_ms(2);
-
 	}
 	for (int i = 0; i < 1024; i++) {
-		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
-		plot_3data16(0, 0 , angle_off);
+		float angle_off =  g_encoder.encoder_off_map[i] / (((g_encoder.encoder_off_count[i] >> 4)&0xF) + (g_encoder.encoder_off_count[i]&0xF));
+		plot_1data16((s16)angle_off);
 		delay_ms(2);
 	}	
 }
@@ -234,6 +245,22 @@ float encoder_get_vel_count(void) {
 	return g_encoder.est_vel_counts;
 }
 
+float encoder_zero_phase_detect(void) {
+	float phase = g_encoder.pwm_angle;
+	float total_ph = phase;
+	int count = 0;
+	for(; count < 10; count++) {
+		delay_ms(ENC_PWM_Min_P * 1000 + 2); //wait time for pwm
+		if ABS(phase - g_encoder.pwm_angle > 2.0f) {
+			return INVALID_ANGLE;
+		}
+		phase = g_encoder.pwm_angle;
+		total_ph += phase;
+	}
+	return (total_ph/(float)count);
+}
+
+
 static void encoder_sync_pwm_abs(void) {
 	ENC_COUNT = g_encoder.pwm_count;
 	g_encoder.last_cnt = g_encoder.pwm_count;

Applications/foc/motor/encoder.h

@@ -20,26 +20,24 @@ typedef struct {
 	u32   last_cnt;
 	u32   b_index_cnt;
 	u32   last_us;
-	s8    enc_dir; //encoder count dir UP or DOWN
 	s8    direction; //motor running dir, 逆时针 正，顺时针负
-	u16   p_dir_cnt;
-	u16   n_dir_cnt;
 	float est_vel_counts; //每秒多少个计数
 	float est_angle_counts;
 	float rpm;
+	u8   *encoder_off_count;
+	s16  *encoder_off_map;	
 }encoder_t;
 
 void encoder_init(void);
 void encoder_init_clear(s8 direction);
 float encoder_get_theta(void);
 float encoder_get_speed(void);
-void encoder_detect_offset(float angle, bool r);
+void encoder_detect_offset(float angle);
 void encoder_set_direction(s8 direction);
 float encoder_get_vel_count(void);
 void encoder_lock_position(bool enable);
-float encoder_detect_finish(bool r);
+bool encoder_detect_finish(void);
 float encoder_get_pwm_angle(void);
-void encoder_detect_off_finished(void);
-
+float encoder_zero_phase_detect(void);
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -16,6 +16,7 @@
 #include "foc/core/e_ctrl.h"
 #include "foc/motor/motor_param.h"
 #include "foc/core/torque.h"
+#include "app/nv_storage.h"
 
 static motor_t motor = {
 	.s_direction = POSITIVE,
@@ -114,7 +115,7 @@ void mc_use_throttle(void) {
 }
 
 
-void mc_encoder_calibrate(s16 vd) {
+void mc_encoder_off_calibrate(s16 vd) {
 	if (PMSM_FOC_Is_Start()) {
 		return;
 	}
@@ -128,28 +129,29 @@ void mc_encoder_calibrate(s16 vd) {
 	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
 	delay_ms(2000);
+	motor_encoder_set_direction(POSITIVE);
 	for (int i = 0; i < 1000000; i++) {
 		for (float angle = 0; angle < 360; angle++) {
 			PMSM_FOC_Set_Angle(angle);
 			delay_ms(2);
-			encoder_detect_offset(angle, false);
+			motor_encoder_offset(angle);
 		}
-		if (encoder_detect_finish(false)) {
+		if (motor_encoder_offset_is_finish()) {
 			break;
 		}
 	}
 	delay_ms(500);
+	motor_encoder_set_direction(NEGATIVE);
 	for (int i = 0; i < 1000000; i++) {
 		for (float angle = 359; angle >= 0; angle--) {
 			PMSM_FOC_Set_Angle(angle);
 			delay_ms(2);
-			encoder_detect_offset(angle, true);
+			motor_encoder_offset(angle);
 		}
-		if (encoder_detect_finish(true)) {
+		if (motor_encoder_offset_is_finish()) {
 			break;
 		}
 	}
-	encoder_detect_off_finished();
 	delay_ms(500);
 	PMSM_FOC_SetOpenVdq(0, 0);
 	delay_ms(500);
@@ -159,6 +161,35 @@ void mc_encoder_calibrate(s16 vd) {
 	
 }
 
+bool mc_encoder_zero_calibrate(s16 vd) {
+	if (PMSM_FOC_Is_Start()) {
+		return false;
+	}
+	pwm_turn_on_low_side();
+	task_udelay(500);
+	PMSM_FOC_Start(CTRL_MODE_OPEN);
+	phase_current_offset_calibrate();
+	pwm_start();
+	adc_start_convert();
+	phase_current_calibrate_wait();
+	PMSM_FOC_Set_Angle(0);
+	PMSM_FOC_SetOpenVdq(vd, 0);
+	delay_ms(2000);
+	float phase = motor_encoder_zero_phase_detect();
+	delay_ms(500);
+	PMSM_FOC_SetOpenVdq(0, 0);
+	delay_ms(500);
+	adc_stop_convert();
+	pwm_stop();
+	PMSM_FOC_Stop();
+	if (phase != INVALID_ANGLE) {
+		nv_save_angle_offset(phase);
+		return true;
+	}
+	return false;
+}
+
+
 bool mc_current_sensor_calibrate(float current) {
 	if (!mc_start(CTRL_MODE_OPEN)) {
 		return false;

Applications/foc/motor/motor.h

@@ -18,12 +18,13 @@ typedef struct {
 void mc_init(void);
 bool mc_start(u8 mode);
 bool mc_stop(void);
-void mc_encoder_calibrate(s16 vd);
+void mc_encoder_off_calibrate(s16 vd);
 bool mc_throttle_released(void);
 bool mc_lock_motor(bool lock);
 void mc_set_spd_torque(s32 target);
 void mc_use_throttle(void);
 bool mc_current_sensor_calibrate(float current);
+bool mc_encoder_zero_calibrate(s16 vd);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL
@@ -79,7 +80,7 @@ static __INLINE void motor_encoder_offset(float angle) {
 #ifdef USE_ENCODER_HALL
 		hall_detect_offset(angle);
 #elif defined (USE_ENCODER_ABI)
-		encoder_detect_offset(angle, false);
+		encoder_detect_offset(angle);
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -89,7 +90,27 @@ static __INLINE void motor_encoder_offset_finish(void) {
 #ifdef USE_ENCODER_HALL
 		hall_detect_offset_finish();
 #elif defined (USE_ENCODER_ABI)
-		encoder_detect_finish(false);
+		encoder_detect_finish();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE bool motor_encoder_offset_is_finish(void) {
+#ifdef USE_ENCODER_HALL
+	return false;
+#elif defined (USE_ENCODER_ABI)
+	return encoder_detect_finish();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+static __INLINE float motor_encoder_zero_phase_detect(void){
+#ifdef USE_ENCODER_HALL
+	return 0.0f;
+#elif defined (USE_ENCODER_ABI)
+	return encoder_zero_phase_detect();
 #else
 	#error "Postion sensor ERROR"
 #endif

Applications/os/heap_4.c

@@ -38,7 +38,7 @@
 
 #define portBYTE_ALIGNMENT			8
 #define portBYTE_ALIGNMENT_MASK 	0x0007
-#define configTOTAL_HEAP_SIZE    (6*1024)
+#define configTOTAL_HEAP_SIZE    (12*1024)
 #define configASSERT(x)
 /* Block sizes must not get too small. */
 #define heapMINIMUM_BLOCK_SIZE	( ( size_t ) ( xHeapStructSize << 1 ) )

Project/MC100.uvoptx

@@ -117,10 +117,6 @@
         <pMon>Segger\JL2CM3.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGUARM</Key>
-        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMRTXEVENTFLAGS</Key>