功率pi，修改kp为1.8，ki为6，每个挡位单独定义转把，转速和扭矩的对应关系

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

Applications/app/app.c

@@ -112,7 +112,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-static int plot_type = 3;
+static int plot_type = 4;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_smo_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
@@ -132,6 +132,8 @@ static u32 _app_plot_task(void * args) {
 	}else if (plot_type == 3) {
 		//can_plot2(PMSM_FOC_GetSpeed(), foc_observer_smo_speed());
 		plot_smo_angle();
+	}else if (plot_type == 4) {
+		can_plot2(eCtrl_get_RefTorque(), PMSM_FOC_Get()->in.s_targetTorque);
 	}
 	
 	return 20;

Applications/app/nv_storage.c

@@ -79,8 +79,8 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
 	foc_params.pid_conf[PID_Spd_id].kb = 1.0f;
 
-	foc_params.pid_conf[PID_Pow_id].kp = 0.6f;
-	foc_params.pid_conf[PID_Pow_id].ki = 4.0f;
+	foc_params.pid_conf[PID_Pow_id].kp = 1.8f;
+	foc_params.pid_conf[PID_Pow_id].ki = 6.0f;
 	foc_params.pid_conf[PID_Pow_id].kb = 0;
 
 	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);

Applications/foc/core/PI_Controller.h

@@ -77,9 +77,7 @@ static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
 		pi->is_sat = false;
 	}
 	pi->sat = out_sat - out;
-	if (pi->sat >= 0.0f) {
-		pi->sat = 0.0f;
-	}
+
 	return out_sat;
 }
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -500,6 +500,22 @@ static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 	return maxTrq;
 #endif
 }
+
+static __INLINE float PMSM_FOC_Limit_Speed(float refTorque) {
+	if (refTorque >= 0) {
+		gFoc_Ctrl.pi_torque->max = refTorque;
+		gFoc_Ctrl.pi_torque->min = -CONFIG_MAX_NEG_TORQUE;
+	}else {
+		gFoc_Ctrl.pi_torque->min = refTorque;
+		gFoc_Ctrl.pi_torque->max = 0;
+	}
+
+	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
+	float trq_res = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
+
+	return trq_res;
+}
+
 static __INLINE void PMSM_FOC_idq_Assign(void) {
 	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
 		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
@@ -547,15 +563,7 @@ void PMSM_FOC_idqCalc(void) {
 		}
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp));
-		if (refTorque >= 0) {
-			gFoc_Ctrl.pi_torque->max = refTorque;
-			gFoc_Ctrl.pi_torque->min = -CONFIG_MAX_NEG_CURRENT;
-		}else {
-			gFoc_Ctrl.pi_torque->min = refTorque;
-			gFoc_Ctrl.pi_torque->max = 0;
-		}
-		float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
+		float maxTrq = PMSM_FOC_Limit_Speed(refTorque);
 		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
 		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
@@ -567,10 +575,10 @@ void PMSM_FOC_idqCalc(void) {
 		}
 		if (maxSpeed >= 0) {
 			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->min = -CONFIG_MAX_NEG_CURRENT;
+			gFoc_Ctrl.pi_speed->min = -CONFIG_MAX_NEG_TORQUE;
 		}else if (maxSpeed < 0) {
 			gFoc_Ctrl.pi_speed->min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_CURRENT;
+			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_TORQUE;
 		}
 
 		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {

Applications/foc/core/torque.c

@@ -8,22 +8,30 @@
 #include "libs/logger.h"
 #include "prot/can_foc_msg.h"
 
+static rpm_trq_map_t gear_torques[5][5] = {
+	[0] = {{500,  100}, {1200, 100}, {1700, 80}, {2200, 75}, {2800, 50},},
+	[1] = {{800,  130}, {1600, 120}, {2400, 110}, {3200, 80}, {4300, 60},},
+	[2] = {{1200, 150}, {2200, 140}, {3200, 120}, {4200, 80}, {5300, 70},},
+	[3] = {{3000, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
+	[4] = {{4500, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
+};
+
 /*
 通过查表获取对应扭矩和速度时的Id和IQ的分配
 */
-static torque_lut_t *_trq_tbl = NULL;
-static torque_manager_t g_trq_mn;
+static torque_lut_t *torque_lkup_table = NULL;
+static torque_manager_t torque_ctrl;
 void torque_init(void) {
-	_trq_tbl = nv_get_trq_tlb();
+	torque_lkup_table = nv_get_trq_tlb();
 	torque_reset();
 }
 
 void torque_reset(void) {
-	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
+	memset(&torque_ctrl, 0, sizeof(torque_ctrl));
 }
 
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
-	if ((_trq_tbl == NULL) || (torque < 0 || rpm < 0)) {
+	if ((torque_lkup_table == NULL) || (torque < 0 || rpm < 0)) {
 		dq_out->d = 0;
 		dq_out->q = torque;
 		return;
@@ -36,16 +44,16 @@ void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
 	if (rpm_idx >= MAX_SPD_POINTS) {
 		rpm_idx = MAX_SPD_POINTS -1;
 	}
-	s16 d = _trq_tbl->dq[trq_idx][rpm_idx].d;
-	s16 q = _trq_tbl->dq[trq_idx][rpm_idx].q;
+	s16 d = torque_lkup_table->dq[trq_idx][rpm_idx].d;
+	s16 q = torque_lkup_table->dq[trq_idx][rpm_idx].q;
 	if (trq_idx < MAX_TRQ_POINTS - 1) {
 		trq_idx += 1;
 	}
 	if (rpm_idx < MAX_SPD_POINTS - 1) {
 		rpm_idx += 1;
 	}
-	s16 d_delta = _trq_tbl->dq[trq_idx][rpm_idx].d - d;
-	s16 q_delta = _trq_tbl->dq[trq_idx][rpm_idx].q - q;
+	s16 d_delta = torque_lkup_table->dq[trq_idx][rpm_idx].d - d;
+	s16 q_delta = torque_lkup_table->dq[trq_idx][rpm_idx].q - q;
 	float comp_ceof = 0.5f * ((torque - torque/TBL_TRQ_INTVAL*TBL_TRQ_INTVAL)/(float)TBL_TRQ_INTVAL + (rpm - rpm/TBL_SPD_INTVAL*TBL_SPD_INTVAL)/(float)TBL_SPD_INTVAL);
 
 	dq_out->d = d + d_delta * comp_ceof;
@@ -53,6 +61,35 @@ void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
 
 }
 
+
+float torque_max_from_gear_rpm(void) {
+	u8 gear = mc_get_gear();
+	if (gear > 4) {
+		gear = 0;
+	}
+	rpm_trq_map_t *map = &gear_torques[gear][0];
+	s16 rpm = (s16)torque_ctrl.spd_filted;
+	if (rpm <= map[0].rpm) {
+		return (float)map[0].torque;
+	}
+	int map_size = 5;
+	for (int i = 1; i < map_size; i++) {
+		if (rpm <= map[i].rpm) { //线性插值
+			float trq1 = map[i-1].torque;
+			float min_rpm = map[i-1].rpm;
+			float trq2 = map[i].torque;
+			float max_rpm = map[i].rpm;
+			if (trq1 > trq2) {
+				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
+			}else {
+				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
+			}
+		}
+	}
+	return (float)map[map_size-1].torque;	
+}
+
+
 /* 获取油门开度 */
 static float throttle_ration(float f_throttle) {
 	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
@@ -79,29 +116,29 @@ float throttle_to_torque(float f_throttle) {
 void request_torque(float thro_ration) {
 	float curr_rpm = PMSM_FOC_GetSpeed();
 	if (curr_rpm == 0) {
-		g_trq_mn.spd_filted = 0;
+		torque_ctrl.spd_filted = 0;
 	}else {
-		LowPass_Filter(g_trq_mn.spd_filted, curr_rpm, 0.5f);
+		LowPass_Filter(torque_ctrl.spd_filted, curr_rpm, 0.01f);
 	}
-	float trq_map = (float)get_max_torque_for_rpm((s16)g_trq_mn.spd_filted);
-	float trq_lim = PMSM_FOC_GetTorqueLimit();
-	float max_trq = min(trq_map, trq_lim);
-	float ref_trq = max_trq * 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;
 	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
 		return;
 	}
 	
 	if (!mc_throttle_released()) {
 		if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
-			g_trq_mn.torque_req = eCtrl_get_FinalTorque() * FINAL_DQ_CEFO;
-			ref_trq  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_trq );
+			torque_ctrl.torque_req = eCtrl_get_FinalTorque() * FINAL_DQ_CEFO;
+			ref_torque  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_torque);
 		}
-		if (ref_trq > g_trq_mn.torque_req) { //加扭矩step给定
-			step_towards(&g_trq_mn.torque_req, ref_trq, 1.0f);
+		if (ref_torque > torque_ctrl.torque_req) { //加扭矩step给定
+			step_towards(&torque_ctrl.torque_req, ref_torque, 1.0f);
 		}else { //减扭矩，直接给定
-			g_trq_mn.torque_req = ref_trq;
+			torque_ctrl.torque_req = ref_torque;
 		}
-		PMSM_FOC_Set_Torque(g_trq_mn.torque_req);
+		PMSM_FOC_Set_Torque(torque_ctrl.torque_req);
 	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
 		float real_trq = PMSM_FOC_Get_Real_Torque() * REAL_DQ_CEOF;
 		float ref_now = min(real_trq, eCtrl_get_RefTorque());
@@ -119,17 +156,17 @@ void request_speed(float thro_ration) {
 
 void throttle_process(u8 run_mode, float f_throttle) {
 	if (mc_throttle_released()){
-		g_trq_mn.throttle_value = 0;
-		g_trq_mn.torque_req = 0;
+		torque_ctrl.throttle_value = 0;
+		torque_ctrl.torque_req = 0;
 	}else {
-		LowPass_Filter(g_trq_mn.throttle_value, f_throttle, THRO_REF_LP_CEOF);
+		LowPass_Filter(torque_ctrl.throttle_value, f_throttle, THRO_REF_LP_CEOF);
 	}
-	g_trq_mn.thro_ration = throttle_ration(f_throttle);
+	torque_ctrl.thro_ration = throttle_ration(f_throttle);
 
 	if (run_mode == CTRL_MODE_TRQ) {
-		request_torque(g_trq_mn.thro_ration);
+		request_torque(torque_ctrl.thro_ration);
 	}else if (run_mode == CTRL_MODE_SPD) {
-		request_speed(g_trq_mn.thro_ration);
+		request_speed(torque_ctrl.thro_ration);
 	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
 		eCtrl_reset_Torque(0);
 		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {

Applications/foc/core/torque.h

@@ -15,6 +15,5 @@ void torque_reset(void);
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out);
 void throttle_process(u8 run_mode, float f_throttle);
 void request_torque(float thro_ration);
-
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/foc_config.h

@@ -54,7 +54,7 @@
 
 #define CONFIG_TORQUE_MODE_MIN_RPM 600
 
-#define CONFIG_MAX_NEG_CURRENT 2.0F
+#define CONFIG_MAX_NEG_TORQUE 5.0F
 
 #ifdef CONFIG_SMO_OBSERVER
 	#define CONFIG_SMO_MIN_SPEED    1000 //RPM

Applications/foc/limit.c

@@ -11,10 +11,9 @@ static limter_t motor_temp_lim[] = {//电机过温限流，限制相电流
 	//{.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 = 120, .exit_pointer = 20, .limit_value = 0},
+	{.enter_pointer = 95, .exit_pointer = 85, .limit_value = 0},
+	{.enter_pointer = 86, .exit_pointer = 80, .limit_value = 80},
+	{.enter_pointer = 81, .exit_pointer = 70, .limit_value = 120},
 };  
 static limter_t vol_under_lim[] = { //欠压限流,限制母线
 	{.enter_pointer = 20, .exit_pointer = 20, .limit_value = 10},

Applications/foc/motor/motor.c

@@ -146,7 +146,7 @@ static void mc_gear_vmode_changed(void) {
 	sys_debug("limit %d-%d-%d, mode = %s\n", gears[motor.n_gear].u_maxRPM, gears[motor.n_gear].u_maxIdc, gears[motor.n_gear].u_maxTorque, motor.b_is96Mode?"96V":"48V");
 	PMSM_FOC_SpeedRampLimit(gears[motor.n_gear].u_maxRPM, 500, 500);
 	PMSM_FOC_DCCurrLimit(gears[motor.n_gear].u_maxIdc);
-	PMSM_FOC_TorqueLimit(gears[motor.n_gear].u_maxTorque);
+	//PMSM_FOC_TorqueLimit(gears[motor.n_gear].u_maxTorque);
 }
 
 void mc_init(void) {

Applications/foc/motor/motor_param.c

@@ -5,9 +5,10 @@
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
 static rpm_trq_map_t mot_map[] = {
 	{4500, 200},
-	{4740, 170},
-	{5050, 125},
-	{5400, 85},
+	{4740, 150},
+	{5050, 110},
+	{5200, 85},
+	{5300, 70},
 	//{5740, 85},//5
 	//{6050, 90},//10
 	//{6430, 107},//16

Applications/libs/time_measure.c

@@ -25,11 +25,11 @@ void time_measure_start(measure_time_t *m){
 		m->first = false;
 		return;
 	}
-	if (m->intval_time > m->intval_max_time+1) {
+	if (m->intval_time > m->intval_max_time+5) {
 		m->intval_time_h_error ++;
 		m->intval_hi_err = m->intval_time;
 	}
-	if (m->intval_time < m->intval_max_time-1) {
+	if (m->intval_time < m->intval_max_time-5) {
 		m->intval_time_l_error ++;
 		m->intval_low_err = m->intval_time;
 	}