实现编码器零位置校准功能

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

Applications/app/app.c

@@ -98,13 +98,13 @@ static u32 _app_report_task(void *p) {
 	
 	if (++loop % 10 == 0) {
 		//sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation);
-		sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
-		sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
-		sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
+		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+		//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
+		//sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
 		sys_debug("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
 		sys_debug("throttle %f\n", get_throttle_float());
 		sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
-		sys_debug("curr lfp %f\n", PMSM_FOC_Get()->params.n_PhaseFilterCeof);
+		//sys_debug("curr lfp %f\n", PMSM_FOC_Get()->params.n_PhaseFilterCeof);
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
 		encoder_log();
 		PMSM_FOC_LogDebug();

Applications/app/nv_storage.c

@@ -331,6 +331,9 @@ void nv_storage_init(void) {
 		nv_save_foc_params();
 		nv_save_motor_params();
 	}
+#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW1
+	m_params.offset = 0.0f; //编码器做了零位置校准
+#endif
 	sys_debug("current band %f\n", foc_params.n_currentBand);
 }
 

Applications/bsp/board_mc100_v1.h

@@ -331,7 +331,7 @@
 #endif
 #define DEBUG_PORT_UART2
 
-#define CONFIG_MOT_TYPE MOTOR_BLUESHARK_ZD_100
+#define CONFIG_MOT_TYPE MOTOR_BLUESHARK_NEW1
 
 //#define CONFIG_DQ_STEP_RESPONSE
 

Applications/bsp/bsp.h

@@ -54,7 +54,7 @@
 #define ENCODER_MT  2
 
 
-#define MOTOR_BLUESHARK_NEW1  1//蓝鲨大功率电机，双E形
+#define MOTOR_BLUESHARK_NEW1  1//蓝鲨大功率电机，双I形
 #define MOTOR_BLUESHARK_NEW2  2//蓝鲨大功率电机，V形
 #define MOTOR_BLUESHARK_OLD   3//目前量产的电机
 #define MOTOR_BLUESHARK_ZD_100  4//中动100码编码器电机样品

Applications/foc/commands.c

@@ -242,8 +242,21 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			s16 vd = decode_s16((u8 *)command->data);
 			sys_debug("cali encoder %d\n", vd);
-			//mc_encoder_off_calibrate((vd));
-			mc_encoder_zero_calibrate(vd);
+			erroCode = mc_encoder_zero_calibrate(vd)? FOC_Success:FOC_Param_Err;
+			break;
+		}
+		case Foc_Enc_Zero_Cali_Result:
+		{
+			response[2] = encoder_get_cali_error()?1:0;
+			u32 off = encoder_get_cnt_offset();
+			encode_u32(response + 3, off);
+			len += 4;
+			break;
+		}
+		case Foc_Force_Open_Run:
+		{
+			s16 vd = decode_s16((u8 *)command->data);
+			mc_force_run_open(vd, 0);
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:

Applications/foc/commands.h

@@ -33,6 +33,8 @@ typedef enum {
 	Foc_Get_Gear_Limit,
 	Foc_Set_Thro_Ration,
 	Foc_Use_SensorLess_Angle,
+	Foc_Force_Open_Run,
+	Foc_Enc_Zero_Cali_Result,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -229,8 +229,8 @@ void PMSM_FOC_CoreInit(void) {
 	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
 	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);
 
-	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS));
-	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS));	
+	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (CONFIG_FOC_VDQ_RAMP_TS));
+	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_FOC_VDQ_RAMP_TS));	
 	PMSM_FOC_Reset_PID();
 
 	foc_observer_init();

Applications/foc/core/torque.c

@@ -110,6 +110,22 @@ float throttle_to_torque(float f_throttle) {
 	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
 }
 
+#define FUNC_SEG1_X_END 0.5F
+#define FUNC_SEG1_Y_END 0.25F
+#define FUNC_SEG1_K    (FUNC_SEG1_Y_END/FUNC_SEG1_X_END)
+
+#define FUNC_SEG2_X_OFF FUNC_SEG1_X_END
+#define FUNC_SEG2_Y_OFF FUNC_SEG1_Y_END
+#define FUNC_SEG2_K ((1.0F-FUNC_SEG1_Y_END)/(1.0F - FUNC_SEG1_X_END))
+
+static float __INLINE unline_func(float x) {
+	if (x <= FUNC_SEG1_X_END) {
+		return x * FUNC_SEG1_K;
+	}
+
+	return (x - FUNC_SEG1_X_END) * FUNC_SEG2_K + FUNC_SEG2_Y_OFF;
+} 
+
 #define REAL_DQ_CEOF 0.9f
 #define FINAL_DQ_CEFO 1.1F
 
@@ -123,7 +139,7 @@ void request_torque(float thro_ration) {
 	float torque_map = torque_max_from_gear_rpm();// (float)get_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
 	float torque_lim = PMSM_FOC_GetTorqueLimit();
 	float max_torque = min(torque_map, torque_lim);
-	float ref_torque = max_torque * thro_ration;
+	float ref_torque = max_torque * unline_func(thro_ration);
 	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
 		return;
 	}

Applications/foc/foc_config.h

@@ -31,7 +31,7 @@
 #define CONFIG_SPD_CTRL_MS (1000/CONFIG_SPD_CTRL_TS)
 #define CONFIG_SPD_CTRL_US (CONFIG_SPD_CTRL_MS * 1000)
 #define CONFIG_FOC_VDQ_RAMP_TS 100
-#define CONFIG_FOC_VDQ_RAMP_FINAL_TIME 3000
+#define CONFIG_FOC_VDQ_RAMP_FINAL_TIME 1000
 #define CONFIG_ZERO_SPEED_RPM  10
 /* 电子刹车，动能回收，加速 */
 #define CONFIG_eCTRL_STEP_TS CONFIG_SPD_CTRL_MS     /* 斜率给定的step的时间值，单位 ms */

Applications/foc/motor/encoder.c

@@ -72,6 +72,8 @@ void encoder_init_clear(void) {
 	g_encoder.position = 0.0f;
 	g_encoder.interpolation = 0.0f;
 	g_encoder.cali_angle = INVALID_ANGLE;
+	g_encoder.enc_count_off = 0xFFFFFFFF;
+	g_encoder.b_cali_err = false;
 }
 
 void encoder_lock_position(bool enable) {
@@ -277,27 +279,52 @@ float encoder_get_position(void) {
 	return g_encoder.position;
 }
 
-float encoder_zero_phase_detect(void) {
-	float phase = g_encoder.pwm_angle;
-	float total_ph = phase;
-	float prev_offset = g_encoder.enc_offset;
+float encoder_zero_phase_detect(float *enc_off) {	
 	g_encoder.enc_offset = 0;
+	delay_ms(5);	
+	float total_enc_off = g_encoder.pwm_count;
+	float prev_offset = g_encoder.enc_offset;
+	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) {
+		delay_ms(5); //wait time for pwm
+		float angle_now = g_encoder.pwm_angle;
+		if (ABS(phase - angle_now) > 2.0f) {
 			g_encoder.enc_offset = prev_offset;
+			g_encoder.enc_count_off = 0xFFFFFFFF;
+			g_encoder.b_cali_err = true;
+			sys_debug("err %f, %f, %d\n", phase, angle_now, count);
 			return INVALID_ANGLE;
 		}
-		phase = g_encoder.pwm_angle;
+		
+		phase = angle_now;
 		total_ph += phase;
+		total_enc_off += g_encoder.pwm_count;
 	}
 	float offset_now = total_ph/(float)count;
 	g_encoder.enc_offset = offset_now;
-	sys_debug("offset = %f\n", offset_now);
+	g_encoder.enc_count_off = (u32)(total_enc_off/(float)count);
+	if (enc_off) {
+		*enc_off = (float)g_encoder.enc_count_off;
+		sys_debug("encoder off %f\n", *enc_off);
+	}
+	sys_debug("encoder ph off = %f\n", offset_now);
 	return offset_now;
 }
 
+void encoder_clear_cnt_offset(void) {
+	g_encoder.b_cali_err = false;
+	g_encoder.enc_count_off = 0xFFFFFFFF;
+}
+
+u32 encoder_get_cnt_offset(void) {
+	return g_encoder.enc_count_off;
+}
+
+bool encoder_get_cali_error(void) {
+	return g_encoder.b_cali_err;
+}
 
 static void encoder_sync_pwm_abs(void) {
 	ENC_COUNT = g_encoder.pwm_count;
@@ -378,4 +405,5 @@ float encoder_get_abi_angle(void) {
 
 void encoder_log(void) {
 	sys_debug("pwm %f, abi %f\n", encoder_get_pwm_angle(), encoder_get_abi_angle());
+	sys_debug("pwm count %d\n", g_encoder.pwm_count);
 }

Applications/foc/motor/encoder.h

@@ -5,6 +5,7 @@
 typedef struct {
 	bool  b_index_found; //I 对齐
 	float enc_offset;
+	u32   enc_count_off;
 	u8    motor_poles;
 	float pwm_angle;
 	u32   pwm_count;
@@ -17,6 +18,7 @@ typedef struct {
 	PLL_t est_pll;
 	bool  b_timer_ov;
 	bool  b_lock_pos;
+	bool  b_cali_err;
 	u32   cpr;
 	u32   last_cnt;
 	u32   b_index_cnt;
@@ -39,11 +41,14 @@ float encoder_get_vel_count(void);
 void encoder_lock_position(bool enable);
 bool encoder_detect_finish(void);
 float encoder_get_pwm_angle(void);
-float encoder_zero_phase_detect(void);
+float encoder_zero_phase_detect(float *enc_off);
 bool ENC_Check_error(void);
 float encoder_get_abi_angle(void);
 float encoder_get_position(void);
 void encoder_detect_upload(void);
+void encoder_clear_cnt_offset(void);
+u32 encoder_get_cnt_offset(void);
+bool encoder_get_cali_error(void);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -32,7 +32,8 @@ static shark_timer_t _autohold_beep_timer = TIMER_INIT(_autohold_beep_timer, _au
 static void _fan_det_timer_handler(shark_timer_t *);
 static shark_timer_t _fan_det_timer1 = TIMER_INIT(_fan_det_timer1, _fan_det_timer_handler);
 static shark_timer_t _fan_det_timer2 = TIMER_INIT(_fan_det_timer2, _fan_det_timer_handler);
-
+static void _encoder_zero_off_timer_handler(shark_timer_t *);
+static shark_timer_t _encoder_zero_off_timer = TIMER_INIT(_encoder_zero_off_timer, _encoder_zero_off_timer_handler);
 
 static motor_t motor = {
 	.s_direction = POSITIVE,
@@ -133,6 +134,7 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.b_auto_hold = 0;
 	motor.b_break = false;
 	motor.b_wait_brk_release = false;
+	motor.b_force_run = false;
 }
 
 static void mc_gear_vmode_changed(void) {
@@ -440,9 +442,47 @@ void mc_get_running_status(u8 *data) {
 	data[0] |= (motor.b_lock_motor) << 7; //motor locked
 }
 
+static float _force_angle = 0.0f;
+static int   _force_wait = 2000;
+/* 开环，强制给定电角度和DQ的电压 */
+void mc_force_run_open(s16 vd, s16 vq) {
+	if (motor.b_start || motor.b_force_run) {
+		if (vd == 0 && vq == 0) {
+			PMSM_FOC_SetOpenVdq(0, 0);
+			delay_ms(500);
+			wdog_reload();
+			adc_stop_convert();
+			pwm_stop();
+			PMSM_FOC_Stop();
+			pwm_up_enable(true);
+			motor.b_force_run = false;
+		}
+		return;
+	}
+	if (vd == 0 && vq == 0) {
+		return;
+	}
+	MC_Check_MosVbusThrottle();
+	if (mc_unsafe_critical_error()) {
+		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+	}
+	
+	pwm_up_enable(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);
+	_force_wait = 2000;
+	motor.b_force_run = true;
+}
 
 void mc_encoder_off_calibrate(s16 vd) {
-	if (motor.b_start) {
+	if (motor.b_start || motor.b_calibrate) {
 		return;
 	}
 	motor.b_calibrate = true;
@@ -498,10 +538,47 @@ void mc_encoder_off_calibrate(s16 vd) {
 	motor.b_calibrate = false;
 }
 
+
+static void _encoder_zero_off_timer_handler(shark_timer_t *t){
+	if (!motor.b_calibrate) {
+		return;
+	}
+	float enc_off = 0.0f;
+	float phase = motor_encoder_zero_phase_detect(&enc_off);
+	PMSM_FOC_SetOpenVdq(0, 0);
+	delay_ms(50);
+	adc_stop_convert();
+	pwm_stop();
+	PMSM_FOC_Stop();
+	_mc_internal_init(CTRL_MODE_OPEN, false);
+	motor.b_calibrate = false;
+	if (phase != INVALID_ANGLE) {
+		nv_save_angle_offset(phase);
+	}
+}
+
 bool mc_encoder_zero_calibrate(s16 vd) {
-	if (PMSM_FOC_Is_Start()) {
+	if (motor.b_calibrate) {
+		if (vd == 0) {
+			encoder_clear_cnt_offset();
+			shark_timer_cancel(&_encoder_zero_off_timer);
+			PMSM_FOC_SetOpenVdq(0, 0);
+			delay_ms(500);
+			adc_stop_convert();
+			pwm_stop();
+			PMSM_FOC_Stop();
+			_mc_internal_init(CTRL_MODE_OPEN, false);
+			motor.b_calibrate = false;
+		}
+		return true;
+	}
+	encoder_clear_cnt_offset();
+	MC_Check_MosVbusThrottle();
+	if (mc_unsafe_critical_error()) {
+		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
 		return false;
 	}
+	_mc_internal_init(CTRL_MODE_OPEN, true);
 	motor.b_calibrate = true;
 	pwm_turn_on_low_side();
 	task_udelay(500);
@@ -509,26 +586,14 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
+	pwm_enable_channel();
 	phase_current_calibrate_wait();
 	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
-	for (int i = 0; i < 10; i++) {
-		delay_ms(1000);
-		wdog_reload();
-	}
-	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();
-	motor.b_calibrate = false;
-	if (phase != INVALID_ANGLE) {
-		nv_save_angle_offset(phase);
-		return true;
-	}
-	return false;
+
+	shark_timer_post(&_encoder_zero_off_timer, 6*1000);
+
+	return true;
 }
 
 
@@ -872,13 +937,24 @@ void Sched_MC_mTask(void) {
 	/* 母线电流，实际采集的相电流矢量大小的计算 */
 	PMSM_FOC_Calc_Current();
 
-	if ((is96v_prev != motor.b_is96Mode) || limted) {
-		mc_gear_vmode_changed();
-	}
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
+		if (motor.b_force_run) {
+			if (_force_wait > 0) {
+				--_force_wait;
+			}else {
+				_force_angle += 1.0f;
+				rand_angle(_force_angle);
+				PMSM_FOC_Set_Angle(_force_angle);
+			}
+		}
 		mc_TaskEnd;
 		return;
 	}
+
+	if ((is96v_prev != motor.b_is96Mode) || limted) {
+		mc_gear_vmode_changed();
+	}
+
 	/* 堵转处理 */
 	if (mc_run_stall_process(runMode) || (runMode == CTRL_MODE_CURRENT)) {
 		eCtrl_Running();

Applications/foc/motor/motor.h

@@ -19,6 +19,7 @@ typedef struct {
 	float  throttle;
 	bool   b_ignor_throttle;
 	bool   b_calibrate;
+	bool   b_force_run;
 	bool   b_runStall; //是否堵转
 	bool   b_lock_motor;
 	bool   b_auto_hold;
@@ -69,6 +70,7 @@ void mc_set_critical_error(u8 err);
 void mc_clr_critical_error(u8 err);
 u32 mc_get_critical_error(void);
 void mc_set_fan_duty(u8 duty);
+void mc_force_run_open(s16 vd, s16 vq);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL
@@ -182,11 +184,11 @@ static __INLINE void motor_encoder_data_upload(void) {
 }
 
 
-static __INLINE float motor_encoder_zero_phase_detect(void){
+static __INLINE float motor_encoder_zero_phase_detect(float *enc_off){
 #ifdef USE_ENCODER_HALL
 	return 0.0f;
 #elif defined (USE_ENCODER_ABI)
-	return encoder_zero_phase_detect();
+	return encoder_zero_phase_detect(enc_off);
 #else
 	#error "Postion sensor ERROR"
 #endif

Applications/foc/motor/motor_param.c

@@ -2,7 +2,7 @@
 #include "foc/motor/motor_param.h"
 #include "math/fast_math.h"
 
-#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
+//#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
 static rpm_trq_map_t mot_map[] = {
 	{4500, 200},
 	{4740, 150},
@@ -13,7 +13,7 @@ static rpm_trq_map_t mot_map[] = {
 	//{6050, 90},//10
 	//{6430, 107},//16
 };
-#endif
+//#endif
 
 /* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
 s16 get_max_torque_for_rpm(s16 rpm) {