默认减速50ms，lq通过iq查表获取，然后更新iq的pi控制器

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

Applications/app/app.c

@@ -88,7 +88,7 @@ static float test_id = 0;
 static float test_iq = 0;
 static u32 _app_trq_test_task(void *args) {
 	DQ_t dq;
-	mpta_fw_lookup(test_rpm, test_trq, &dq);
+	motor_mpta_fw_lookup(test_rpm, test_trq, &dq);
 	test_id = dq.d;
 	test_iq = dq.q;
 	sys_debug("lookup: %d, %f, %f, %f\n", test_rpm, test_trq, test_id, test_iq);

Applications/app/nv_storage.c

@@ -134,14 +134,14 @@ static void nv_default_gear_config(void) {
 		gear_config.gears_48[i].n_max_speed = 4000;
 		gear_config.gears_48[i].n_max_trq = CONFIG_MAX_MOTOR_TORQUE/2;
 		gear_config.gears_48[i].n_max_idc = 60;
-		gear_config.gears_48[i].n_zero_accl = 1500;
-		gear_config.gears_48[i].n_accl_time = 500;
+		gear_config.gears_48[i].n_zero_accl = 500;
+		gear_config.gears_48[i].n_accl_time = 10;
 
 		gear_config.gears_96[i].n_max_speed = 8000;
 		gear_config.gears_96[i].n_max_trq = CONFIG_MAX_MOTOR_TORQUE;
 		gear_config.gears_96[i].n_max_idc = 100;
-		gear_config.gears_96[i].n_zero_accl = 1500;
-		gear_config.gears_96[i].n_accl_time = 400;
+		gear_config.gears_96[i].n_zero_accl = 500;
+		gear_config.gears_96[i].n_accl_time = 10;
 
 		for (int j = 0; j < GEAR_SPEED_TRQ_NUM; j++) {
 			gear_config.gears_48[i].n_torque[j] = 60+j;

Applications/bsp/board_mc100_v1.h

@@ -33,7 +33,6 @@
 
 #define CONFIG_SMO_OBSERVER 1
 #define CONFIG_SPEED_LADRC  1
-//#define CONFIG_FORCE_96V_MODE 1
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5
@@ -341,7 +340,11 @@
 #endif
 #define DEBUG_PORT_UART2
 
-#define CONFIG_MOT_TYPE MOTOR_BLUESHARK_ZD_100
+#define CONFIG_MOT_TYPE MOTOR_BLUESHARK_A1
+
+#if (CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1)
+#define CONFIG_FORCE_96V_MODE 1
+#endif
 
 //#define CONFIG_DQ_STEP_RESPONSE
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -239,7 +239,6 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.plot_type = Plot_None;
 }
 
-//#define CONFIG_USER_PHASE_LFP
 static __INLINE void PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
 
@@ -270,6 +269,9 @@ static __INLINE void PMSM_FOC_Update_Input(void) {
 	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 
 	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
+
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
 }
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -320,6 +322,11 @@ void PMSM_FOC_Schedule(void) {
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
 
+		/* update kp&ki from lq for iq PI controller */
+		float lq = motor_get_lq_from_iq((s16)gFoc_Ctrl.out.s_FilterIdq.q);
+		gFoc_Ctrl.pi_iq.kp = ((float)nv_get_foc_params()->n_currentBand * lq);
+		gFoc_Ctrl.pi_iq.ki = (nv_get_motor_params()->r/lq);
+
 		/* limiter Vq output for PI controller */
 		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
 		gFoc_Ctrl.pi_iq.max = max_vq;
@@ -357,9 +364,6 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
 
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
-
 	if (gFoc_Ctrl.plot_type != Plot_None) {
 		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
 			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
@@ -518,7 +522,7 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 		}
 	}else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) ||
 				(gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
-		mpta_fw_lookup(gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
+		motor_mpta_fw_lookup(gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
 	}
 	u32 mask = cpu_enter_critical();
 	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);

Applications/foc/core/PMSM_FOC_Core.h

@@ -74,6 +74,7 @@ typedef enum {
 typedef struct {
 	u8 	  n_poles;
 	float n_modulation;
+	float lq;
 	float n_PhaseFilterCeof;
 }FOC_Params;
 

Applications/foc/core/torque_unused.c

@@ -104,7 +104,7 @@ static float get_throttle_torque(float trq_ration) {
 	}else {
 		LowPass_Filter(torque_ctrl.spd_filted, curr_rpm, 0.01f);
 	}
-	float torque_map = torque_max_from_gear_rpm();// (float)get_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
+	float torque_map = torque_max_from_gear_rpm();// (float)motor_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
 	float torque_lim = PMSM_FOC_GetTorqueLimit();
 	float max_torque = min(torque_map, torque_lim);
 

Applications/foc/foc_config.h

@@ -38,8 +38,8 @@
 /* 电子刹车，动能回收，加速 */
 #define CONFIG_eCTRL_STEP_TS CONFIG_SPD_CTRL_MS     /* 斜率给定的step的时间值，单位 ms */
 #define CONFIG_eCTRL_Brake_TIME 1500     /* 捏住刹车的时间，超过这个时间启动ebrake，单位 ms */
-#define CONFIG_ACC_TIME 1
-#define CONFIG_DEC_TIME 1
+#define CONFIG_ACC_TIME 50
+#define CONFIG_DEC_TIME 50
 #define CONFIG_EBRK_RAMP_TIME 500
 #define CONFIG_AUTOHOLD_DETECT_TIME 3000
 

Applications/foc/motor/A1_motor_config.c

@@ -135,3 +135,15 @@ static motor_map_t mot_map[] = {
 	{8000, 193},
 };
 
+static iq_lq_map_t iq_lq_map[] = {
+	{35, 0.000178f},
+	{91, 0.000166f},
+	{141, 0.000145f},
+	{181, 0.000130f},
+	{217, 0.000120f},
+	{245, 0.000112f},
+	{283, 0.000098f},
+	{332, 0.000092f},
+	{353, 0.000085f},
+};
+

Applications/foc/motor/motor.c

@@ -981,11 +981,7 @@ static bool mc_run_stall_process(u8 run_mode) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
 					PMSM_FOC_Set_Torque(0);
-#if 0
-					torque_reset();
-#else
 					thro_torque_reset();
-#endif
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
@@ -1081,21 +1077,13 @@ static void mc_process_curise(void) {
 			if ((cruise_time >= 3) && motor.cruise_torque == 0.0f) {
 				motor.cruise_torque = PMSM_FOC_Get()->in.s_targetTorque;
 			}else if ((cruise_time >= 3) && (motor.cruise_torque > 0.0f)){
-#if 0
-				float trq_req = get_thro_request_torque();
-#else
 				float trq_req = get_user_request_torque();
-#endif
 				if (trq_req > motor.cruise_torque * 1.2f) {
 					PMSM_FOC_PauseCruise(); //需要加速,暂停定速巡航
 				}
 			}
 		}else {
-#if 0
-			float trq_req = get_thro_request_torque();
-#else
 			float trq_req = get_user_request_torque();
-#endif
 			if (trq_req <= motor.cruise_torque * 1.1f) {
 				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
 				motor.cruise_time = shark_get_seconds();
@@ -1160,11 +1148,7 @@ void Sched_MC_mTask(void) {
 				mask = cpu_enter_critical();
 				PMSM_FOC_Start(motor.mode);
 				mc_gear_vmode_changed();
-				#if 0
-				torque_reset();
-				#else
 				thro_torque_reset();
-				#endif
 				pwm_enable_channel();
 				cpu_exit_critical(mask);
 			}
@@ -1174,21 +1158,12 @@ void Sched_MC_mTask(void) {
 			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
 				mc_process_throttle_epm();
 			}else {
-				#if 0
-				if (motor.b_ignor_throttle) { //使用软件设置的油门开度
-					float r = (float)motor.u_throttle_ration/100.0f;
-					request_torque(r);
-				}else {
-					throttle_process(runMode, get_throttle_float());
-				}
-				#else
 				float thro = get_throttle_float();
 				if (motor.b_ignor_throttle) {
 					float r = (float)motor.u_throttle_ration/100.0f;
 					thro = thro_ration_to_voltage(r);
 				}
 				thro_torque_process(runMode, thro);
-				#endif
 			}
 			PMSM_FOC_Slow_Task();
 		}

Applications/foc/motor/motor_param.c

@@ -14,7 +14,7 @@ static int map_rpm[] = {3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 80
 #endif
 
 /* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
-s16 get_max_torque_for_rpm(s16 rpm) {
+s16 motor_max_torque_for_rpm(s16 rpm) {
 #ifdef MOT_HAVE_MAPS
 	if (rpm <= mot_map[0].rpm) {
 		return mot_map[0].torque;
@@ -39,6 +39,21 @@ s16 get_max_torque_for_rpm(s16 rpm) {
 #endif
 }
 
+float motor_get_lq_from_iq(s16 iq) {
+#ifdef MOT_HAVE_MAPS
+	iq = ABS(iq);
+	int map_size = ARRAY_SIZE(iq_lq_map);
+	for (int i = map_size-1; i >= 0; i--) {
+		if (iq >= iq_lq_map[i].iq) {
+			return iq_lq_map[i].lq;
+		}
+	}
+	return iq_lq_map[0].lq;
+#else
+	return MOTOR_Lq;
+#endif
+}
+
 s16 motor_map_rpm_idx(float rpm, int *ilow, int *ihigh, s16 *lowrpm, s16 *highrpm) {
 	*ilow = *ihigh = 0xFF;
 	s16 irpm = (s16)rpm;
@@ -129,7 +144,7 @@ static void get_torque_range(s16 z, int index, int max_index, int *left, int *ri
 	*right = low_right;
 }
 
-void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
+void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 	bool neg_trq = false;
 	s16 itorque = torque * 10;
 	if (itorque < 0) {
@@ -169,7 +184,7 @@ void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 	}
 }
 #else
-void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
+void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 	dq_out->d = 0;
 	dq_out->q = torque;
 }

Applications/foc/motor/motor_param.h

@@ -16,9 +16,14 @@ typedef struct{
 	s16 q;
 }torque2dq_t;
 
+typedef struct {
+	s16 iq;
+	float lq;
+}iq_lq_map_t;
 typedef torque2dq_t torque_map_t;
-s16 get_max_torque_for_rpm(s16 rpm);
-void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out);
+s16 motor_max_torque_for_rpm(s16 rpm);
+void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out);
+float motor_get_lq_from_iq(s16 iq);
 
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW1
 #define MOTOR_R   0.010f
@@ -39,7 +44,7 @@ void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out);
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 #define MOTOR_R   0.011f
 #define MOTOR_Ld (0.000140F*0.5f)
-#define MOTOR_Lq (0.000300f*0.5f)
+#define MOTOR_Lq (0.000356f*0.5f)
 
 //#define MOTOR_R   0.033f
 //#define MOTOR_Ld (0.000168f * 0.5f)//(0.000140F*0.5f)