转把对应的扭矩需求从电机外特性map中获取最大扭矩

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

Applications/app/app.c

@@ -100,11 +100,13 @@ static u32 _app_report_task(void *p) {
 		//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("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
 		sys_debug("throttle %f\n", get_throttle_float());
 		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();
 		//err_code_log();
 	}
 	return 200;
@@ -115,7 +117,7 @@ static u32 _app_plot_task(void * args) {
 	//can_report_plot_values(0x45);
 	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
 	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
-	can_plot2(PMSM_FOC_GetSpeed(), smo_observer_est_rpm());
+	can_plot2(eCtrl_get_RefTorque() * 50.0f, PMSM_FOC_GetSpeed());
 	//can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
 #if 0
 	if (!_mc_start) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -236,7 +236,7 @@ void PMSM_FOC_CoreInit(void) {
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 #endif
-	gFoc_Ctrl.plot_type = Plot_Phase_curr;
+	gFoc_Ctrl.plot_type = Plot_None;
 }
 
 //#define CONFIG_USER_PHASE_LFP
@@ -415,7 +415,7 @@ void PMSM_FOC_Schedule(void) {
 }
 
 void PMSM_FOC_LogDebug(void) {
-
+	sys_debug("DC curr %f\n", gFoc_Ctrl.out.s_CalciDC);
 }
 
 /*called in media task */
@@ -1035,7 +1035,7 @@ void PMSM_FOC_Calc_Current(void) {
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.1f);
 
 	raw_idc = get_vbus_current();
-	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.01f);
 
 	gFoc_Ctrl.out.s_RealCurrent = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 

Applications/foc/core/torque.c

@@ -1,6 +1,7 @@
 #include "foc/core/torque.h"
 #include "foc/foc_config.h"
 #include "foc/motor/motor.h"
+#include "foc/motor/motor_param.h"
 #include "foc/core/e_ctrl.h"
 #include "foc/core/PMSM_FOC_Core.h"
 #include "app/nv_storage.h"
@@ -72,62 +73,54 @@ float throttle_to_torque(float f_throttle) {
 	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
 }
 
-/* 软件设置油门开度，调试使用 */
-void throttle_set_ration(float r) {
-	g_trq_mn.thro_value = r;
-}
-
 #define REAL_DQ_CEOF 0.9f
 #define FINAL_DQ_CEFO 1.1F
 
-void torque_mode_process(void) {
-	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
+void request_torque(float thro_ration) {
+	float curr_rpm = PMSM_FOC_GetSpeed();
+	if (curr_rpm == 0) {
+		g_trq_mn.spd_filted = 0;
+	}else {
+		LowPass_Filter(g_trq_mn.spd_filted, curr_rpm, 0.5f);
+	}
+	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;
 	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
-		g_trq_mn.thro_value = 0;
-		g_trq_mn.torque_prev = 0;
-		g_trq_mn.torque_base = 0;
+		g_trq_mn.thro_ration = 0;
 		return;
 	}
 	
 	if (!mc_throttle_released()) {
-		if (PMSM_FOC_GetSpeed() <= CONFIG_ZERO_SPEED_RPM) {
+		if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
 			ref_trq  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_trq );
-			g_trq_mn.torque_base = ref_trq;
 		}
 		PMSM_FOC_Set_Torque(ref_trq );
-		g_trq_mn.torque_prev = ref_trq;
 	}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());
 		eCtrl_reset_Torque(ref_now);
 		PMSM_FOC_Set_Torque(0);
-		g_trq_mn.thro_value = 0;
-		g_trq_mn.torque_prev = 0;
-		g_trq_mn.torque_base = 0;
+		g_trq_mn.thro_ration = 0;
 	}
 }
 
-void speed_mode_process(void) {
-	float speed_Ref = g_trq_mn.spd_ref;
+void request_speed(float thro_ration) {
+	float speed_Ref = PMSM_FOC_GetSpeedLimit() * thro_ration;
 	PMSM_FOC_Set_Speed(speed_Ref);
 }
 
 #define THRO_REF_LP_CEOF 0.2f
 
 void throttle_process(u8 run_mode, float f_throttle) {
+	float thro_value = throttle_ration(f_throttle);
+	g_trq_mn.thro_ration = LowPass_Filter(g_trq_mn.thro_ration, thro_value, THRO_REF_LP_CEOF);
+
 	if (run_mode == CTRL_MODE_TRQ) {
-		float thro_value = throttle_ration(f_throttle);
-		g_trq_mn.thro_value = LowPass_Filter(g_trq_mn.thro_value, thro_value, THRO_REF_LP_CEOF);
-		if ((g_trq_mn.count++ % 20) == 0) {
-			torque_mode_process();
-		}
-	
+		request_torque(g_trq_mn.thro_ration);
 	}else if (run_mode == CTRL_MODE_SPD) {
-		float spd_ref = throttle_to_speed(f_throttle);
-		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
-		if ((g_trq_mn.count++ % 20) == 0) {
-			speed_mode_process();
-		}
+		request_speed(g_trq_mn.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

@@ -3,21 +3,16 @@
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
 typedef struct {
-	float thro_prev;
-	float speed_prev;
-	float torque_prev;
-	float torque_base;
 	bool  accl;
-	float thro_value; //油门开度百分比
-	float spd_ref;
+	float thro_ration; //油门开度百分比
+	float spd_filted;
 	u32   count;
 }torque_manager_t;
 void torque_init(void);
 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 throttle_set_ration(float r);
-void torque_mode_process(void);
+void request_torque(float thro_ration);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/motor/motor.c

@@ -894,8 +894,7 @@ void Sched_MC_mTask(void) {
 			}else {
 				if (motor.b_ignor_throttle) { //使用软件设置的油门开度
 					float r = (float)motor.u_throttle_ration/100.0f;
-					throttle_set_ration(r);
-					torque_mode_process();
+					request_torque(r);
 				}else {
 					throttle_process(runMode, get_throttle_float());
 				}

Applications/foc/motor/motor_param.c

@@ -8,9 +8,9 @@ static motor_map_t mot_map[] = {
 	{4740, 170},
 	{5050, 125},
 	{5400, 85},
-	{5740, 85},//5
-	{6050, 90},//10
-	{6430, 107},//16
+	//{5740, 85},//5
+	//{6050, 90},//10
+	//{6430, 107},//16
 };
 #endif
 
@@ -19,16 +19,20 @@ s16 get_max_torque_for_rpm(s16 rpm) {
 	if (rpm <= mot_map[0].rpm) {
 		return mot_map[0].torque;
 	}
-	for (int i = 1; i < ARRAY_SIZE(mot_map); i++) {
+	int map_size = ARRAY_SIZE(mot_map);
+	for (int i = 1; i < map_size; i++) {
 		if (rpm <= mot_map[i].rpm) { //线性插值
 			float trq1 = mot_map[i-1].torque;
 			float min_rpm = mot_map[i-1].rpm;
 			float trq2 = mot_map[i].torque;
 			float max_rpm = mot_map[i].rpm;
-
-			return (s16)f_map(rpm, min_rpm, max_rpm, min(trq1, trq2), MAX(trq1, trq2));
+			if (trq1 > trq2) {
+				return (s16)f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
+			}else {
+				return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
+			}
 		}
 	}
-	return mot_map[ARRAY_SIZE(mot_map)-1].torque;
+	return mot_map[map_size-1].torque;
 }
 

Applications/foc/motor/motor_param.h

@@ -39,7 +39,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_Lq (0.000091f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 145.0F
-#define TRQ_PI_KP 0.13F
+#define TRQ_PI_KP 0.6F //0.13f
 #define TRQ_PI_KI 0.5F
 #define MOTOR_NR 0x13
 

Applications/math/fast_math.h

@@ -54,11 +54,17 @@ static void fast_norm_angle(float *angle) {
 		*angle += 360.0f;
 	}
 }
-
+/* 递增map */
 static __INLINE float f_map(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
+/* 递减map */
+static __INLINE float f_map_inv(float x, float in_min, float in_max, float out_min, float out_max) {
+	return out_max - (x - in_min) * (out_max - out_min) / (in_max - in_min);
+}
+
+
 static __INLINE void step_towards(float *value, float goal, float step) {
     if (*value < goal) {
         if ((*value + step) < goal) {