超前角标定调试

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

Applications/app/app.c

@@ -16,6 +16,8 @@
 static u32 _app_low_task(void *args);
 static u32 _app_report_task(void *args);
 extern void PMSM_FOC_LogDebug(void);
+extern void err_code_log(void);
+extern void encoder_log(void);
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
 
@@ -92,10 +94,12 @@ static u32 _app_report_task(void *p) {
 	can_report_dq_current(0x45);
 	can_report_foc_status(0x45);
 	can_report_phase_voltage(0x45);
+	can_report_mpta_values(0x45);
 	if (++loop % 10 == 0) {
 		can_report_pid_value(0x45, PID_TRQ_id);
 		sys_debug("max exec time %d, %d\n", g_meas_foc.exec_max_error_time, g_meas_foc.exec_time);
-		sys_debug("sat = %d, %d, %d\n", PMSM_FOC_Get()->pi_id->is_sat, PMSM_FOC_Get()->pi_iq->is_sat, PMSM_FOC_Get()->pi_speed->is_sat);
+		encoder_log();
+		err_code_log();
 	}
 	return 500;
 }

Applications/app/nv_storage.h

@@ -42,7 +42,7 @@ typedef struct {
 
 #define MAX_TRQ_POINTS 60
 #define MAX_SPD_POINTS 100
-#define TBL_TRQ_INTVAL 1
+#define TBL_TRQ_INTVAL 10
 #define TBL_SPD_INTVAL 100
 struct lut_dq {
 	s16 d;
@@ -50,7 +50,6 @@ struct lut_dq {
 };
 typedef struct {
 	struct lut_dq dq[MAX_TRQ_POINTS][MAX_SPD_POINTS];
-	//s16  q[MAX_TRQ_POINTS][MAX_SPD_POINTS];
 	u16  magic;
 	u16  crc16;
 }torque_lut_t;

Applications/bsp/board_mc_v1.h

@@ -11,7 +11,7 @@
 #define CONFIG_RATED_DC_VOL (96.0f)   /* 母线最大电压 V*/
 #define CONFIG_MIN_DC_VOL   (25.0f)
 
-#define CONFIG_MAX_VBUS_CURRENT 150.0f
+#define CONFIG_MAX_VBUS_CURRENT 200.0f
 #define CONFIG_MAX_MOT_RPM      9000.0f
 #define CONFIG_MAX_PHASE_CURR   400.0F
 #define CONFIG_MAX_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)

Applications/foc/commands.c

@@ -189,9 +189,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			u8 start = decode_u8((u8 *)command->data);
 			if (start == Foc_Start) {
+				sys_debug("start mpta cali\n");
+				mc_set_foc_mode(CTRL_MODE_CURRENT);
 				PMSM_FOC_MTPA_Calibrate(true);
 			}else {
 				PMSM_FOC_MTPA_Calibrate(false);
+				mc_set_foc_mode(CTRL_MODE_TRQ);
 			}
 			break;
 		}
@@ -202,6 +205,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}else {
 				s16 is_curr = decode_s16((u8 *)command->data);
 				s16 is_angle = decode_s16((u8 *)command->data + 2);
+				sys_debug("curr %d, angle %d\n", is_curr, is_angle);
 				PMSM_FOC_Set_Current(is_curr);
 				PMSM_FOC_Set_Angle(is_angle);
 			}

Applications/foc/core/PMSM_FOC_Core.c

@@ -391,6 +391,9 @@ void PMSM_FOC_LogDebug(void) {
 /*called in media task */
 u8 PMSM_FOC_CtrlMode(void) {
 	u8 preMode = gFoc_Ctrl.out.n_RunMode;
+	if (gFoc_Ctrl.in.b_cruiseEna && gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_CRUISE_RPM) {
+		gFoc_Ctrl.in.b_cruiseEna = false;
+	}
 	if (!gFoc_Ctrl.in.b_motEnable) {
 		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
 	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
@@ -444,13 +447,10 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 			
 			if (gFoc_Ctrl.in.s_targetIdq.d > gFoc_Ctrl.hwLim.s_FWDCurrMax) {
 				gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.hwLim.s_FWDCurrMax;
-				gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
 			}else if (gFoc_Ctrl.in.s_targetIdq.d < -gFoc_Ctrl.hwLim.s_FWDCurrMax) {
 				gFoc_Ctrl.in.s_targetIdq.d = -gFoc_Ctrl.hwLim.s_FWDCurrMax;
-				gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
-			}else {
-				gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent * s;
 			}
+			gFoc_Ctrl.in.s_targetIdq.q = sqrtf(SQ(gFoc_Ctrl.in.s_targetCurrent) - SQ(gFoc_Ctrl.in.s_targetIdq.d));
 		}else {
 			gFoc_Ctrl.in.s_targetIdq.d = 0;
 			gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent;

Applications/foc/foc_config.h

@@ -1,8 +1,8 @@
 #ifndef _FOC_CONFIG_H__
 #define _FOC_CONFIG_H__
 
-#define CONFIG_DEFAULT_IDC_LIM 100
-#define CONFIG_DEFAULT_PHASE_CURR_LIM 300
+#define CONFIG_DEFAULT_IDC_LIM 200
+#define CONFIG_DEFAULT_PHASE_CURR_LIM 400
 #define CONFIG_DEFAULT_RPM_LIM       8000
 
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 50

Applications/foc/limit.c

@@ -1,18 +1,19 @@
 #include "foc/limit.h"
 #include "foc/core/PMSM_FOC_Core.h"
 #include "foc/samples.h"
+#include "foc/mc_error.h"
 
 static limter_t motor_temp_lim[] = {//电机过温限流，限制相电流
-	{.enter_pointer = 130, .exit_pointer = 120, .limit_value = 0},
-	{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 90},
-	{.enter_pointer = 110, .exit_pointer = 100, .limit_value = 120},
-	{.enter_pointer = 100, .exit_pointer = 90, .limit_value = 130},
+	{.enter_pointer = 120, .exit_pointer = 20, .limit_value = 0},
+	//{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 90},
+	//{.enter_pointer = 110, .exit_pointer = 100, .limit_value = 120},
+	//{.enter_pointer = 100, .exit_pointer = 90, .limit_value = 130},
 }; 
 static limter_t mos_temp_lim[] = { //mos过温限流，限制相电流
-	{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 0},
-	{.enter_pointer = 110, .exit_pointer = 90, .limit_value = 40},
-	{.enter_pointer = 90, .exit_pointer = 80, .limit_value = 80},
-	{.enter_pointer = 80, .exit_pointer = 70, .limit_value = 100},
+	//{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 0},
+	//{.enter_pointer = 110, .exit_pointer = 90, .limit_value = 40},
+	//{.enter_pointer = 90, .exit_pointer = 80, .limit_value = 80},
+	{.enter_pointer = 85, .exit_pointer = 20, .limit_value = 0},
 };  
 static limter_t vol_under_lim[] = { //欠压限流,限制母线
 	{.enter_pointer = 40, .exit_pointer = 44, .limit_value = 20},
@@ -87,6 +88,7 @@ static u16 _motor_limit(void) {
 		u16 lim_value = _temp_limiter(temp, lim);
 		if (lim_value != CURRENT_LIMIT_NONE) {
 			PMSM_FOC_SetCriticalError(FOC_CRIT_MOTOR_TEMP_Err);
+			err_add_record(FOC_CRIT_MOTOR_TEMP_Err, temp);
 			return lim_value;
 		}
 	}
@@ -101,6 +103,7 @@ static u16 _mos_limit(void) {
 		u16 lim_value = _temp_limiter(temp, lim);
 		if (lim_value != CURRENT_LIMIT_NONE) {
 			PMSM_FOC_SetCriticalError(FOC_CRIT_MOS_TEMP_Err);
+			err_add_record(FOC_CRIT_MOS_TEMP_Err, temp);
 			return lim_value;
 		}
 	}

Applications/foc/mc_error.c

@@ -1,5 +1,6 @@
 #include "foc/mc_error.h"
 #include "foc/core/PMSM_FOC_Core.h"
+#include "libs/logger.h"
 
 static err_record_t err[FOC_CRIT_Err_Max];
 
@@ -19,3 +20,12 @@ int err_get_record(u8 index, int size, err_record_t *err_out) {
 	}
 	return size;
 }
+
+void err_code_log(void) {
+	for (int i = 0;  i < FOC_CRIT_Err_Max; i++) {
+		if (err[i].err_time == 0) {
+			continue;
+		}
+		sys_error("err:%d, time = %d\n", err[i].err_value, err[i].err_time);
+	}
+}

Applications/foc/motor/encoder.c

@@ -344,3 +344,14 @@ bool ENC_Check_error(void) {
 float encoder_get_pwm_angle(void) {
 	return g_encoder.pwm_angle;
 }
+
+float encoder_get_abi_angle(void) {
+	u32 cnt = _abi_count();
+	float angle = ENC_Pluse_Nr_2_angle((float)cnt) * g_encoder.motor_poles + g_encoder.enc_offset;
+	rand_angle(angle);
+	return angle;
+}
+
+void encoder_log(void) {
+	sys_debug("pwm %f, abi %f\n", encoder_get_pwm_angle(), encoder_get_abi_angle());
+}

Applications/foc/motor/encoder.h

@@ -41,5 +41,6 @@ bool encoder_detect_finish(void);
 float encoder_get_pwm_angle(void);
 float encoder_zero_phase_detect(void);
 bool ENC_Check_error(void);
+float encoder_get_abi_angle(void);
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -185,7 +185,7 @@ bool mc_set_foc_mode(u8 mode) {
 	bool ret = false;
 	if (PMSM_FOC_SetCtrlMode(mode)) {
 		motor.mode = mode;
-		if (mode == CTRL_MODE_OPEN) {
+		if (mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) {
 			PMSM_FOC_Start(motor.mode);
 			pwm_enable_channel();
 		}
@@ -552,6 +552,11 @@ void Sched_MC_mTask(void) {
 	if (mc_run_stall_process(runMode)) {
 		return;
 	}
+	if (runMode == CTRL_MODE_CURRENT) {
+		eCtrl_Running();
+		PMSM_FOC_Slow_Task();
+		return;
+	}
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 #ifndef CONFIG_DQ_STEP_RESPONSE
 		if (motor.mode != CTRL_MODE_OPEN) {

Applications/prot/can_foc_msg.c

@@ -92,13 +92,13 @@ void can_report_foc_status(u8 can) {
 
 
 void can_report_mpta_values(u8 can) {
-	u8 data[14];
+	u8 data[8];
 	if (!PMSM_FOC_Get()->in.b_MTPA_calibrate) {
 		return;
 	}
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
-	encode_float(data + 2, PMSM_FOC_Get()->in.s_targetCurrent);
-	encode_float(data + 6, PMSM_FOC_Get()->in.s_targetIdq.d);
-	encode_float(data + 10, PMSM_FOC_Get()->in.s_targetIdq.q);
+	encode_s16(data + 2, (s16)(PMSM_FOC_Get()->in.s_targetCurrent * 10.0f));
+	encode_s16(data + 4, (s16)(PMSM_FOC_Get()->in.s_targetIdq.d * 10.0f));
+	encode_s16(data + 6, (s16)(PMSM_FOC_Get()->in.s_targetIdq.q * 10.0f));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }

Applications/prot/can_foc_msg.h

@@ -10,6 +10,6 @@ void can_response_hall_offset(u8 can, int offset);
 void can_report_power(u8 can);
 void can_report_pid_value(u8 can, u8 id);
 void can_report_foc_status(u8 can);
-
+void can_report_mpta_values(u8 can);
 #endif	/*_Can_Foc_Msg_H__ */