real torque control

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

Applications/app/app.c

@@ -1,5 +1,6 @@
-#include "app/app.h"
 #include "bsp/bsp.h"
+#include "app/app.h"
+#include "bsp/adc.h"
 #include "os/os_task.h"
 #include "libs/logger.h"
 #include "libs/utils.h"
@@ -12,7 +13,9 @@
 #include "libs/time_measure.h"
 #include "app/nv_storage.h"
 #include "foc/commands.h"
-#include "bsp/adc.h"
+#include "foc/core/thro_torque.h"
+#include "foc/motor/motor_param.h"
+
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
 extern float target_d;
@@ -21,6 +24,7 @@ extern float target_q;
 static u32 _app_low_task(void *args);
 static u32 _app_report_task(void *args);
 static u32 _app_plot_task(void *args);
+//static u32 _app_trq_test_task(void *args);
 extern void PMSM_FOC_LogDebug(void);
 extern void err_code_log(void);
 extern void encoder_log(void);
@@ -76,6 +80,22 @@ void fetch_jtag_cmd(void){
 }
 #endif
 
+#if 0
+static s16 test_rpm = 0000;
+static float test_trq = 0;
+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);
+	test_id = dq.d;
+	test_iq = dq.q;
+	sys_debug("lookup: %d, %f, %f, %f\n", test_rpm, test_trq, test_id, test_iq);
+	test_rpm -= 100;
+	test_trq -= 0.5f;
+	return 500;
+}
+#endif
 void app_start(void){
 	set_log_level(MOD_SYSTEM, L_debug);
 	can_message_init();
@@ -89,6 +109,7 @@ void app_start(void){
 	shark_task_create(_app_low_task, NULL);
 	shark_task_create(_app_report_task, NULL);
 	shark_task_create(_app_plot_task, NULL);
+	//shark_task_create(_app_trq_test_task, NULL);
 	sys_debug("mc start\n");
 	shark_task_run();
 }
@@ -108,12 +129,12 @@ static u32 _app_report_task(void *p) {
 		//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\n", get_acc_vol());
-		//sys_debug("throttle %f\n", get_throttle_float());
+		sys_debug("throttle %f\n", get_throttle_float());
 		sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
 		//sys_debug("target current %f\n", PMSM_FOC_Get()->in.s_targetCurrent);
 		//thro_torque_log();
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
-		encoder_log();
+		//encoder_log();
 		///PMSM_FOC_LogDebug();
 		//eCtrl_debug_log();
 		//err_code_log();
@@ -134,8 +155,9 @@ static void plot_smo_angle(void) {
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2((s16)(PMSM_FOC_Get()->in.s_targetTorque*100.0f), (s16)PMSM_FOC_GetSpeed());
-		//can_plot3((s16)PMSM_FOC_Get()->vel_lim_adrc.z1, (s16)PMSM_FOC_Get()->vel_lim_adrc.z2, (s16)PMSM_FOC_GetSpeed());
+		//can_plot2((s16)(PMSM_FOC_Get()->in.s_targetTorque*10.0f), (s16)PMSM_FOC_GetSpeed());
+		can_plot3((s16)PMSM_FOC_Get()->vel_lim_adrc.z1, (s16)PMSM_FOC_Get()->vel_lim_adrc.z2, (s16)PMSM_FOC_GetSpeed());
+		//can_plot2(test_id, test_iq);
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_RefTorque(), eCtrl_get_FinalTorque());
 	}else if (plot_type == 3) {

Applications/bsp/bsp.h

@@ -13,7 +13,6 @@
 #include "bsp/fmc_flash.h"
 #include "bsp/can.h"
 
-
 #define SYSTEM_CLOCK (120000000u) //system clk 120M Hz
 #define TIM_CLOCK (SYSTEM_CLOCK) /*SystemClock_Config��TIM1��clk��sys PLL �������̶�2����PLLƵ��*/
 #define TIM_CLOCK_MHz (120u)

Applications/foc/core/PMSM_FOC_Core.c

@@ -220,11 +220,11 @@ void PMSM_FOC_CoreInit(void) {
 		PMSM_FOC_RT_LimInit();
 		shark_task_create(PMSM_FOC_Debug_Task, NULL);
 		g_focinit = true;
-		_DEBUG("User Limit:\n");
-		_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
-			gFoc_Ctrl.userLim.s_PhaseCurrLim, gFoc_Ctrl.userLim.s_vDCMaxLim, gFoc_Ctrl.userLim.s_vDCMinLim, gFoc_Ctrl.userLim.s_TorqueBrkLim);
-		_DEBUG("Hw Limit:\n");
-		_DEBUG("dc %f, rpm %f, torque %f, phase %f\n", gFoc_Ctrl.hwLim.s_iDCMax, gFoc_Ctrl.hwLim.s_motRPMMax, gFoc_Ctrl.hwLim.s_torqueMax, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
+		//_DEBUG("User Limit:\n");
+		//_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
+		//	gFoc_Ctrl.userLim.s_PhaseCurrLim, gFoc_Ctrl.userLim.s_vDCMaxLim, gFoc_Ctrl.userLim.s_vDCMinLim, gFoc_Ctrl.userLim.s_TorqueBrkLim);
+		//_DEBUG("Hw Limit:\n");
+		//_DEBUG("dc %f, rpm %f, torque %f, phase %f\n", gFoc_Ctrl.hwLim.s_iDCMax, gFoc_Ctrl.hwLim.s_motRPMMax, gFoc_Ctrl.hwLim.s_torqueMax, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
 	}
 	
 	gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
@@ -570,7 +570,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)) {
-		trq2dq_lookup((int)gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque, &gFoc_Ctrl.in.s_targetIdq);
+		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/thro_torque.c

@@ -9,12 +9,11 @@
 #include "prot/can_foc_msg.h"
 
 static thro_torque_t _torque;
-static motor_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},},
+static motor_map_t gear_torques[4][5] = {
+	[0] = {{500,  10}, {1200, 10}, {1700, 10}, {2200, 10}, {2900, 10},},
+	[1] = {{800,  10}, {1600, 100}, {2400, 10}, {3200, 10}, {4300, 10},},
+	[2] = {{1200, 10}, {2200, 10}, {3200, 10}, {4200, 10}, {5300, 10},},
+	[3] = {{3000, 10}, {4740, 10}, {5050, 11}, {5200, 10}, {5300, 10},},
 };
 
 void thro_torque_reset(void) {
@@ -26,7 +25,6 @@ void thro_torque_reset(void) {
 
 void thro_torque_init(void) {
 	thro_torque_reset();
-	trq2dq_lookup_init();
 	_torque.spd_filted = 0.0f;
 }
 

Applications/foc/core/thro_torque.h

@@ -12,8 +12,6 @@ typedef struct {
 	float thro_ration_last;
 }thro_torque_t;
 
-void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out);
-void trq2dq_lookup_init(void);
 void thro_torque_reset(void);
 void thro_torque_init(void);
 float thro_ration_to_voltage(float r);

Applications/foc/core/trq2dq_table.c

@@ -1,19 +1,14 @@
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
-#include "app/nv_storage.h"
+#include "foc/motor/motor_param.h"
 #include "libs/logger.h"
 
-#define RPM_I_MAX MAX_SPD_POINTS
-#define TRQ_I_MAX MAX_TRQ_POINTS
-#define RPM_INTVAL TBL_SPD_INTVAL
-#define IDX2RPM(I) ((I)*RPM_INTVAL + RPM_INTVAL)
+#define _DEBUG(fmt, args...) sys_debug(fmt, ##args)
 
-#define _DEBUG(fmt, args...) no_debug(fmt, ##args)
-
-static trq2dq_t **table_map = NULL;
+extern torque_map_t table_map[14][10];
 //x -> rpm
 //z -> torque
-static void intp_line2(float frac_x, float z, trq2dq_t **map, float *d, float *q) {
+static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {
 	float frac_z1 = 0; //对应x1索引的t_maps
 	float frac_z2 = 0; //对应x2索引的t_maps
 
@@ -37,15 +32,16 @@ static void intp_line2(float frac_x, float z, trq2dq_t **map, float *d, float *q
 	*q = c1 * (1.0f - frac_x) + c2 * frac_x;
 }
 
-static void get_torque_range(float z, int index, int max_index, int *left, int *right) {
+static void get_torque_range(s16 z, int index, int max_index, int *left, int *right) {
 	int low_left = max_index - 1, low_right = max_index - 1;
 	if (z < table_map[index][0].torque) {
 		low_right = low_left = 0;
+		_DEBUG("---%d, %d--%d\n", z, table_map[index][0].torque, table_map[0][0].torque);
 	}else if (z > table_map[index][max_index - 1].torque) {
 		low_right = low_left = max_index - 1;
 	}else {
 		for (int i = 0; i < max_index; i++) {
-			_DEBUG("index %d, trq %d\n", i, table_map[index][i].torque);
+			//_DEBUG("index %d, trq %d\n", i, table_map[index][i].torque);
 			if (z >= table_map[index][i].torque) {
 				low_left = i;
 				low_right = i + 1;
@@ -62,69 +58,47 @@ static void get_torque_range(float z, int index, int max_index, int *left, int *
 
 
 void trq2dq_lookup_init(void) {
-	if (table_map == NULL) {
-		trq2dq_table_t *table = nv_get_trq2dq_table();
-		if (table != NULL) {
-			table_map = (trq2dq_t **)&table->tdq[0][0];
-		}
-	}
+	//if (table_map == NULL) {
+		//table_map = get_torque2dq_maps();
+	//}
 }
 
-void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
-	if (table_map == NULL) {
-		step_towards(&dq_out->d, 0, 10.0f);
-		dq_out->q = torque;
-		return;
-	}
-	if (torque == 0.0f) {
+void trq2dq_lookup(int rpm, s16 torque, DQ_t *dq_out) {
+	//if (table_map == NULL) {
+	//	step_towards(&dq_out->d, 0, 10.0f);
+	//	dq_out->q = torque;
+	//	return;
+	//}
+	if (torque < 0.0f) {
 		dq_out->d = 0.0f;
-		dq_out->q = 0.0f;
+		dq_out->q = torque;
 		return;
 	}
 
-	bool neg_torque = torque < 0.0f?1:0;
-	torque = ABS(torque);
 	int low = 0, high = 0;
+	s16 x1 = 0, x2 = 0;
+	rpm = motor_map_rpm_idx(rpm, &low, &high, &x1, &x2);
 
-	low = rpm / RPM_INTVAL - 1;
-
-	if (low >= RPM_I_MAX) {
-		low = RPM_I_MAX - 1;
-	}
-	if (low < 0)  {
-		high = low = 0;
-	}else if (low == (RPM_I_MAX-1)) {
-		high = low;
-	}else {
-		high = low + 1;
-	}
-
-	_DEBUG("speed %d-%d\n", low, high);
-
-	int low_left = TRQ_I_MAX - 1, low_right = TRQ_I_MAX - 1;
-	get_torque_range(torque, low, TRQ_I_MAX, &low_left, &low_right);
+	_DEBUG("speed %d-%d, %d-%d\n", low, high, x1, x2);
+	int max_trq_idx = motor_map_torque_max_count();
+	int low_left = max_trq_idx - 1, low_right = max_trq_idx - 1;
+	get_torque_range(torque, low, max_trq_idx, &low_left, &low_right);
 	_DEBUG("low speed torque %d-%d\n", low_left, low_right);
 	
-	int high_left = TRQ_I_MAX - 1, high_right = TRQ_I_MAX - 1;
-	get_torque_range(torque, high, TRQ_I_MAX, &high_left, &high_right);
+	int high_left = max_trq_idx - 1, high_right = max_trq_idx - 1;
+	get_torque_range(torque, high, max_trq_idx, &high_left, &high_right);
 	_DEBUG("high speed torque %d-%d\n", high_left, high_right);
-	trq2dq_t *maps[4];
+	torque_map_t *maps[4];
 	maps[0] = &table_map[low][low_left];
 	maps[1] = &table_map[low][low_right];
 	maps[2] = &table_map[high][high_left];
 	maps[3] = &table_map[high][high_right];
 	float frac_x = 0, d = 0, q = 0;
-	int x1 = IDX2RPM(low);
-	int x2 = IDX2RPM(high);
 	if (x1 != x2) {
 		frac_x = (float)(rpm - x1)/(x2 - x1);
 	}
 	intp_line2(frac_x, torque, maps, &d, &q);
-	if (neg_torque) {
-		step_towards(&dq_out->d, 0, 10.0f);
-		dq_out->q = -q;
-	}else {
-		step_towards(&dq_out->d, d, 10.0f);
-		dq_out->q = q;
-	}
+	//step_towards(&dq_out->d, d, 10.0f);
+	dq_out->d = d;
+	dq_out->q = q;
 }

Applications/foc/foc_config.h

@@ -59,9 +59,9 @@
 
 #define CONFIG_MAX_NEG_TORQUE 0.0F
 #ifdef CONFIG_SPEED_LADRC
-	#define CONFIG_LADRC_Wo  200.0F
-	#define CONFIG_LADRC_Wcv 7.0F
-	#define CONFIG_LADRC_B0  1500.0F  
+	#define CONFIG_LADRC_Wo  300.0F
+	#define CONFIG_LADRC_Wcv 3.0f
+	#define CONFIG_LADRC_B0  300.0F  
 #endif
 #ifdef CONFIG_SMO_OBSERVER
 	#define CONFIG_SMO_MIN_SPEED    1000 //RPM

Applications/foc/limit.c

@@ -21,16 +21,16 @@ static void limiter_init(void) {
 		motor_temp_lim[i].enter_pointer = limiter->motor[i].enter_pointer;
 		motor_temp_lim[i].exit_pointer = limiter->motor[i].exit_pointer;
 		motor_temp_lim[i].limit_value = limiter->motor[i].limit_value;
-		sys_debug("%d-%d-%d\n", motor_temp_lim[i].enter_pointer, motor_temp_lim[i].exit_pointer, motor_temp_lim[i].limit_value);
+		//sys_debug("%d-%d-%d\n", motor_temp_lim[i].enter_pointer, motor_temp_lim[i].exit_pointer, motor_temp_lim[i].limit_value);
 		mos_temp_lim[i].enter_pointer = limiter->mos[i].enter_pointer;
 		mos_temp_lim[i].exit_pointer = limiter->mos[i].exit_pointer;
 		mos_temp_lim[i].limit_value = limiter->mos[i].limit_value;
-		sys_debug("%d-%d-%d\n", mos_temp_lim[i].enter_pointer, mos_temp_lim[i].exit_pointer, mos_temp_lim[i].limit_value);
+		//sys_debug("%d-%d-%d\n", mos_temp_lim[i].enter_pointer, mos_temp_lim[i].exit_pointer, mos_temp_lim[i].limit_value);
 	}
 	vol_under_lim[0].enter_pointer = limiter->vbus.enter_pointer;
 	vol_under_lim[0].exit_pointer = limiter->vbus.exit_pointer;
 	vol_under_lim[0].limit_value = limiter->vbus.limit_value;
-	sys_debug("%d-%d-%d\n", vol_under_lim[0].enter_pointer, vol_under_lim[0].exit_pointer, vol_under_lim[0].limit_value);
+	//sys_debug("%d-%d-%d\n", vol_under_lim[0].enter_pointer, vol_under_lim[0].exit_pointer, vol_under_lim[0].limit_value);
 }
 
 static u16 _temp_limiter(s16 temp, limter_t *lim) {

Applications/foc/motor/A1_motor_config.c

@@ -0,0 +1,157 @@
+static torque_map_t mtpa_fw_map[14][10] = {
+	{ // 转速：0
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -130, 480 }, // 扭矩：-5, D轴电流：-13, Q轴电流：48
+		{ 92, -340, 720 }, // 扭矩：-9.2, D轴电流：-34, Q轴电流：72
+		{ 146, -600, 1040 }, // 扭矩：-14.6, D轴电流：-60, Q轴电流：104
+		{ 200, -920, 1310 }, // 扭矩：-20, D轴电流：-92, Q轴电流：131
+		{ 254, -1410, 1410 }, // 扭矩：-25.4, D轴电流：-141, Q轴电流：141
+		{ 320, -1770, 1770 }, // 扭矩：-32, D轴电流：-177, Q轴电流：177
+		{ 382, -2120, 2120 }, // 扭矩：-38.2, D轴电流：-212, Q轴电流：212
+		{ 438, -2470, 2470 }, // 扭矩：-43.8, D轴电流：-247, Q轴电流：247
+	}, { // 转速：-500
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 53, -130, 480 }, // 扭矩：-5.3, D轴电流：-13, Q轴电流：48
+		{ 116, -420, 900 }, // 扭矩：-11.6, D轴电流：-42, Q轴电流：90
+		{ 180, -750, 1300 }, // 扭矩：-18, D轴电流：-75, Q轴电流：130
+		{ 244, -1150, 1640 }, // 扭矩：-24.4, D轴电流：-115, Q轴电流：164
+		{ 303, -1500, 2000 }, // 扭矩：-30.3, D轴电流：-150, Q轴电流：200
+		{ 358, -2120, 2120 }, // 扭矩：-35.8, D轴电流：-212, Q轴电流：212
+		{ 410, -2470, 2470 }, // 扭矩：-41, D轴电流：-247, Q轴电流：247
+		{ 456, -2830, 2830 }, // 扭矩：-45.6, D轴电流：-283, Q轴电流：283
+	}, { // 转速：-1000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 52, -130, 480 }, // 扭矩：-5.2, D轴电流：-13, Q轴电流：48
+		{ 112, -420, 900 }, // 扭矩：-11.2, D轴电流：-42, Q轴电流：90
+		{ 177, -750, 1300 }, // 扭矩：-17.7, D轴电流：-75, Q轴电流：130
+		{ 240, -1140, 1640 }, // 扭矩：-24, D轴电流：-114, Q轴电流：164
+		{ 300, -1600, 1920 }, // 扭矩：-30, D轴电流：-160, Q轴电流：192
+		{ 355, -2110, 2120 }, // 扭矩：-35.5, D轴电流：-211, Q轴电流：212
+		{ 406, -2470, 2470 }, // 扭矩：-40.6, D轴电流：-247, Q轴电流：247
+		{ 453, -2830, 2830 }, // 扭矩：-45.3, D轴电流：-283, Q轴电流：283
+	}, { // 转速：-2000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 51, -130, 480 }, // 扭矩：-5.1, D轴电流：-13, Q轴电流：48
+		{ 111, -420, 900 }, // 扭矩：-11.1, D轴电流：-42, Q轴电流：90
+		{ 175, -760, 1300 }, // 扭矩：-17.5, D轴电流：-76, Q轴电流：130
+		{ 238, -1150, 1640 }, // 扭矩：-23.8, D轴电流：-115, Q轴电流：164
+		{ 297, -1540, 1970 }, // 扭矩：-29.7, D轴电流：-154, Q轴电流：197
+		{ 353, -1930, 2300 }, // 扭矩：-35.3, D轴电流：-193, Q轴电流：230
+		{ 404, -2470, 2470 }, // 扭矩：-40.4, D轴电流：-247, Q轴电流：247
+		{ 450, -2830, 2830 }, // 扭矩：-45, D轴电流：-283, Q轴电流：283
+	}, { // 转速：-3000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -170, 470 }, // 扭矩：-5, D轴电流：-17, Q轴电流：47
+		{ 110, -510, 850 }, // 扭矩：-11, D轴电流：-51, Q轴电流：85
+		{ 174, -850, 1230 }, // 扭矩：-17.4, D轴电流：-85, Q轴电流：123
+		{ 234, -1140, 1630 }, // 扭矩：-23.4, D轴电流：-114, Q轴电流：163
+		{ 292, -1460, 2030 }, // 扭矩：-29.2, D轴电流：-146, Q轴电流：203
+		{ 350, -1920, 2290 }, // 扭矩：-35, D轴电流：-192, Q轴电流：229
+		{ 400, -2340, 2600 }, // 扭矩：-40, D轴电流：-234, Q轴电流：260
+		{ 447, -2820, 2830 }, // 扭矩：-44.7, D轴电流：-282, Q轴电流：283
+	}, { // 转速：-4000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -170, 470 }, // 扭矩：-5, D轴电流：-17, Q轴电流：47
+		{ 109, -460, 880 }, // 扭矩：-10.9, D轴电流：-46, Q轴电流：88
+		{ 169, -850, 1230 }, // 扭矩：-16.9, D轴电流：-85, Q轴电流：123
+		{ 236, -1140, 1630 }, // 扭矩：-23.6, D轴电流：-114, Q轴电流：163
+		{ 294, -1530, 1970 }, // 扭矩：-29.4, D轴电流：-153, Q轴电流：197
+		{ 352, -2000, 2220 }, // 扭矩：-35.2, D轴电流：-200, Q轴电流：222
+		{ 374, -2320, 2240 }, // 扭矩：-37.4, D轴电流：-232, Q轴电流：224
+		{ 374, -2320, 2240 }, // 扭矩：-37.4, D轴电流：-232, Q轴电流：224
+	}, { // 转速：-4500
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -130, 480 }, // 扭矩：-5, D轴电流：-13, Q轴电流：48
+		{ 109, -350, 940 }, // 扭矩：-10.9, D轴电流：-35, Q轴电流：94
+		{ 173, -700, 1320 }, // 扭矩：-17.3, D轴电流：-70, Q轴电流：132
+		{ 237, -1090, 1640 }, // 扭矩：-23.7, D轴电流：-109, Q轴电流：164
+		{ 294, -1790, 1710 }, // 扭矩：-29.4, D轴电流：-179, Q轴电流：171
+		{ 320, -2160, 1720 }, // 扭矩：-32, D轴电流：-216, Q轴电流：172
+		{ 320, -2160, 1720 }, // 扭矩：-32, D轴电流：-216, Q轴电流：172
+		{ 320, -2160, 1720 }, // 扭矩：-32, D轴电流：-216, Q轴电流：172
+	}, { // 转速：-5000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -170, 470 }, // 扭矩：-5, D轴电流：-17, Q轴电流：47
+		{ 110, -420, 900 }, // 扭矩：-11, D轴电流：-42, Q轴电流：90
+		{ 174, -860, 1230 }, // 扭矩：-17.4, D轴电流：-86, Q轴电流：123
+		{ 232, -1460, 1360 }, // 扭矩：-23.2, D轴电流：-146, Q轴电流：136
+		{ 274, -2100, 1360 }, // 扭矩：-27.4, D轴电流：-210, Q轴电流：136
+		{ 275, -2010, 1400 }, // 扭矩：-27.5, D轴电流：-201, Q轴电流：140
+		{ 276, -2020, 1400 }, // 扭矩：-27.6, D轴电流：-202, Q轴电流：140
+		{ 277, -2020, 1410 }, // 扭矩：-27.7, D轴电流：-202, Q轴电流：141
+	}, { // 转速：-5500
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -170, 470 }, // 扭矩：-5, D轴电流：-17, Q轴电流：47
+		{ 110, -420, 900 }, // 扭矩：-11, D轴电流：-42, Q轴电流：90
+		{ 168, -1040, 1070 }, // 扭矩：-16.8, D轴电流：-104, Q轴电流：107
+		{ 218, -1620, 1160 }, // 扭矩：-21.8, D轴电流：-162, Q轴电流：116
+		{ 246, -1960, 1190 }, // 扭矩：-24.6, D轴电流：-196, Q轴电流：119
+		{ 246, -1960, 1190 }, // 扭矩：-24.6, D轴电流：-196, Q轴电流：119
+		{ 246, -1960, 1190 }, // 扭矩：-24.6, D轴电流：-196, Q轴电流：119
+		{ 246, -1960, 1190 }, // 扭矩：-24.6, D轴电流：-196, Q轴电流：119
+	}, { // 转速：-6000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 48, -170, 470 }, // 扭矩：-4.8, D轴电流：-17, Q轴电流：47
+		{ 105, -630, 780 }, // 扭矩：-10.5, D轴电流：-63, Q轴电流：78
+		{ 158, -1180, 920 }, // 扭矩：-15.8, D轴电流：-118, Q轴电流：92
+		{ 204, -1730, 1000 }, // 扭矩：-20.4, D轴电流：-173, Q轴电流：100
+		{ 220, -1950, 1020 }, // 扭矩：-22, D轴电流：-195, Q轴电流：102
+		{ 222, -1950, 1020 }, // 扭矩：-22.2, D轴电流：-195, Q轴电流：102
+		{ 222, -1950, 1020 }, // 扭矩：-22.2, D轴电流：-195, Q轴电流：102
+		{ 222, -1950, 1020 }, // 扭矩：-22.2, D轴电流：-195, Q轴电流：102
+	}, { // 转速：-6500
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -17, 198 }, // 扭矩：-2, D轴电流：-1.7, Q轴电流：19.8
+		{ 50, -170, 470 }, // 扭矩：-5, D轴电流：-17, Q轴电流：47
+		{ 100, -690, 700 }, // 扭矩：-10, D轴电流：-69, Q轴电流：70
+		{ 142, -1150, 810 }, // 扭矩：-14.2, D轴电流：-115, Q轴电流：81
+		{ 187, -1730, 900 }, // 扭矩：-18.7, D轴电流：-173, Q轴电流：90
+		{ 196, -1875, 900 }, // 扭矩：-19.6, D轴电流：-187.5, Q轴电流：90
+		{ 196, -1875, 900 }, // 扭矩：-19.6, D轴电流：-187.5, Q轴电流：90
+		{ 196, -1875, 900 }, // 扭矩：-19.6, D轴电流：-187.5, Q轴电流：90
+		{ 196, -1875, 900 }, // 扭矩：-19.6, D轴电流：-187.5, Q轴电流：90
+	}, { // 转速：-7000
+		{ 0, 0, 0 }, // 扭矩：0, D轴电流：0, Q轴电流：0
+		{ 20, -68, 187 }, // 扭矩：-2, D轴电流：-6.8, Q轴电流：18.7
+		{ 46, -310, 390 }, // 扭矩：-4.6, D轴电流：-31, Q轴电流：39
+		{ 91, -770, 600 }, // 扭矩：-9.1, D轴电流：-77, Q轴电流：60
+		{ 138, -1290, 730 }, // 扭矩：-13.8, D轴电流：-129, Q轴电流：73
+		{ 178, -1800, 790 }, // 扭矩：-17.8, D轴电流：-180, Q轴电流：79
+		{ 190, -1970, 800 }, // 扭矩：-19, D轴电流：-197, Q轴电流：80
+		{ 190, -1970, 800 }, // 扭矩：-19, D轴电流：-197, Q轴电流：80
+		{ 190, -1970, 800 }, // 扭矩：-19, D轴电流：-197, Q轴电流：80
+		{ 190, -1970, 800 }, // 扭矩：-19, D轴电流：-197, Q轴电流：80
+	}, { // 转速：-7500
+		{ 0, -85, 32 }, // 扭矩：0, D轴电流：-8.5, Q轴电流：3.2
+		{ 20, -86, 180 }, // 扭矩：-2, D轴电流：-8.6, Q轴电流：18
+		{ 38, -378, 330 }, // 扭矩：-3.8, D轴电流：-37.8, Q轴电流：33
+		{ 84, -830, 540 }, // 扭矩：-8.4, D轴电流：-83, Q轴电流：54
+		{ 128, -1340, 670 }, // 扭矩：-12.8, D轴电流：-134, Q轴电流：67
+		{ 164, -1820, 730 }, // 扭矩：-16.4, D轴电流：-182, Q轴电流：73
+		{ 170, -1890, 730 }, // 扭矩：-17, D轴电流：-189, Q轴电流：73
+		{ 170, -1890, 730 }, // 扭矩：-17, D轴电流：-189, Q轴电流：73
+		{ 170, -1890, 730 }, // 扭矩：-17, D轴电流：-189, Q轴电流：73
+		{ 170, -1890, 730 }, // 扭矩：-17, D轴电流：-189, Q轴电流：73
+	}, { // 转速：-8000
+		{ 0, -250, 45 }, // 扭矩：0, D轴电流：-25, Q轴电流：4.5
+		{ 20, -86, 180 }, // 扭矩：-2, D轴电流：-8.6, Q轴电流：18
+		{ 47, -560, 340 }, // 扭矩：-4.7, D轴电流：-56, Q轴电流：34
+		{ 93, -1060, 540 }, // 扭矩：-9.3, D轴电流：-106, Q轴电流：54
+		{ 134, -1550, 640 }, // 扭矩：-13.4, D轴电流：-155, Q轴电流：64
+		{ 154, -1810, 670 }, // 扭矩：-15.4, D轴电流：-181, Q轴电流：67
+		{ 154, -1810, 670 }, // 扭矩：-15.4, D轴电流：-181, Q轴电流：67
+		{ 154, -1810, 670 }, // 扭矩：-15.4, D轴电流：-181, Q轴电流：67
+		{ 154, -1810, 670 }, // 扭矩：-15.4, D轴电流：-181, Q轴电流：67
+		{ 154, -1810, 670 }, // 扭矩：-15.4, D轴电流：-181, Q轴电流：67
+	},
+};

Applications/foc/motor/motor_param.c

@@ -1,22 +1,35 @@
 #include "bsp/bsp.h"
 #include "foc/motor/motor_param.h"
 #include "math/fast_math.h"
+#include "libs/logger.h"
 
-//#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
+#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
+#define MOT_HAVE_MAPS
+#define RPM_MAX_IDX 14
+#define TRQ_MAX_IDX 10
+static int map_rpm[] = {0, 500, 1000, 2000, 3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000};
+#include "foc/motor/A1_motor_config.c"
 static motor_map_t mot_map[] = {
-	{4500, 200},
-	{4740, 150},
-	{5050, 110},
-	{5200, 85},
-	{5300, 70},
-	//{5740, 85},//5
-	//{6050, 90},//10
-	//{6430, 107},//16
+	{0, 320},
+	{500, 456},
+	{1000, 453},
+	{2000, 450},
+	{3000, 447},
+	{4000, 374},
+	{4500, 320},
+	{5000, 277},
+	{5500, 246},
+	{6000, 222},
+	{6500, 196},
+	{7000, 190},
+	{7500, 170},
+	{8000, 154},
 };
-//#endif
+#endif
 
 /* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
 s16 get_max_torque_for_rpm(s16 rpm) {
+#ifdef MOT_HAVE_MAPS
 	if (rpm <= mot_map[0].rpm) {
 		return mot_map[0].torque;
 	}
@@ -35,5 +48,140 @@ s16 get_max_torque_for_rpm(s16 rpm) {
 		}
 	}
 	return mot_map[map_size-1].torque;
+#else
+	return CONFIG_MAX_MOTOR_TORQUE;
+#endif
 }
 
+s16 motor_map_rpm_idx(float rpm, int *ilow, int *ihigh, s16 *lowrpm, s16 *highrpm) {
+	*ilow = *ihigh = 0xFF;
+	s16 irpm = (s16)rpm;
+#ifdef MOT_HAVE_MAPS
+	if (irpm > map_rpm[RPM_MAX_IDX-1]) {
+		irpm = map_rpm[RPM_MAX_IDX-1];
+	}
+	for (int i = 0; i < RPM_MAX_IDX; i++) {
+		if (irpm <= map_rpm[i]) {
+			*ihigh = i;
+			if (*ilow == 0xFF) {
+				*ilow = 0;
+			}
+			break;
+		}
+		*ilow = i;
+	}
+	*lowrpm = map_rpm[*ilow];
+	*highrpm = map_rpm[*ihigh];
+#endif
+	return irpm;
+}
+
+int motor_map_torque_max_count(void) {
+#ifdef MOT_HAVE_MAPS
+	return TRQ_MAX_IDX;
+#else
+	return 0;
+#endif
+}
+
+#define _DEBUG(fmt, args...) no_debug(fmt, ##args)
+
+#ifdef MOT_HAVE_MAPS
+
+//x -> rpm
+//z -> torque
+static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {
+	float frac_z1 = 0; //对应x1索引的t_maps
+	float frac_z2 = 0; //对应x2索引的t_maps
+
+	_DEBUG("low --> %d %d\n", map[1]->torque, map[0]->torque);
+	if ((map[1]->torque != map[0]->torque)) {
+		frac_z1 = (float)(z - map[0]->torque)/(map[1]->torque - map[0]->torque);
+	}
+	_DEBUG("high --> %d %d\n", map[3]->torque, map[2]->torque);
+	if ((map[3]->torque != map[2]->torque)) {
+		frac_z2 = (float)(z - map[2]->torque)/(map[3]->torque - map[2]->torque);
+	}
+	
+	_DEBUG("%f -- %f -- %f\n", frac_x, frac_z1, frac_z2);
+
+	float c1 = (1.0f - frac_z1) * map[0]->d + frac_z1 * map[1]->d; //第一行插值
+	float c2 = (1.0f - frac_z2) * map[2]->d + frac_z2 * map[3]->d; //第二行插值
+	*d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
+	
+	c1 = (1.0f - frac_z1) * map[0]->q + frac_z1 * map[1]->q;
+	c2 = (1.0f - frac_z2) * map[2]->q + frac_z2 * map[3]->q;
+	*q = c1 * (1.0f - frac_x) + c2 * frac_x;
+}
+
+static void get_torque_range(s16 z, int index, int max_index, int *left, int *right) {
+	int low_left = max_index - 1, low_right = max_index - 1;
+	if (z < mtpa_fw_map[index][0].torque) {
+		low_right = low_left = 0;
+		_DEBUG("---%d, %d--%d\n", z, mtpa_fw_map[index][0].torque, mtpa_fw_map[0][0].torque);
+	}else if (z > mtpa_fw_map[index][max_index - 1].torque) {
+		low_right = low_left = max_index - 1;
+	}else {
+		for (int i = 0; i < max_index; i++) {
+			if (z >= mtpa_fw_map[index][i].torque) {
+				low_left = i;
+				low_right = i + 1;
+				if (i == max_index - 1) {
+					low_right = low_left;
+					break;
+				}
+			}
+		}
+	}
+	*left = low_left;
+	*right = low_right;
+}
+
+
+void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
+	bool neg_trq = false;
+	s16 itorque = torque * 10;
+	if (itorque < 0) {
+		neg_trq = true;
+		itorque = -itorque;
+	}
+	int low = 0, high = 0;
+	s16 x1 = 0, x2 = 0;
+	rpm = 2000;//ABS(rpm);
+	s16 irpm = motor_map_rpm_idx(rpm, &low, &high, &x1, &x2);
+
+	_DEBUG("speed %d-%d, %d-%d\n", low, high, x1, x2);
+	int max_trq_idx = TRQ_MAX_IDX;
+	int low_left = max_trq_idx - 1, low_right = max_trq_idx - 1;
+	get_torque_range(itorque, low, max_trq_idx, &low_left, &low_right);
+	_DEBUG("low speed torque %d-%d\n", low_left, low_right);
+	
+	int high_left = max_trq_idx - 1, high_right = max_trq_idx - 1;
+	get_torque_range(itorque, high, max_trq_idx, &high_left, &high_right);
+	_DEBUG("high speed torque %d-%d\n", high_left, high_right);
+	torque_map_t *maps[4];
+	maps[0] = &mtpa_fw_map[low][low_left];
+	maps[1] = &mtpa_fw_map[low][low_right];
+	maps[2] = &mtpa_fw_map[high][high_left];
+	maps[3] = &mtpa_fw_map[high][high_right];
+	float frac_x = 0, d = 0, q = 0;
+	if (x1 != x2) {
+		frac_x = (float)(irpm - x1)/(x2 - x1);
+	}
+	intp_line2(frac_x, itorque, maps, &d, &q);
+	dq_out->d = d / 10.0f;
+	dq_out->q = q / 10.0f;
+
+	if (neg_trq) {
+		dq_out->d = -dq_out->d;
+		dq_out->q = -dq_out->q;
+	}
+}
+#else
+void mpta_fw_lookup(int rpm, s16 torque, DQ_t *dq_out) {
+	dq_out->d = 0;
+	dq_out->q = torque;
+}
+
+#endif
+

Applications/foc/motor/motor_param.h

@@ -2,6 +2,7 @@
 #define _MOTOR_PARAM_H__
 #include "bsp/bsp.h"
 #include "os/os_types.h"
+#include "foc/core/PMSM_FOC_Core.h"
 
 /* 电机外特性map, 每个转速点对应的最大扭矩 */
 typedef struct {
@@ -9,7 +10,15 @@ typedef struct {
 	s16 torque;
 }motor_map_t;
 
+typedef struct{
+	s16 torque;
+	s16 d;
+	s16 q;
+}torque2dq_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);
 
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW1
 #define MOTOR_R   0.010f
@@ -38,7 +47,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 0.0F
-#define CONFIG_MAX_MOTOR_TORQUE 200.0F
+#define CONFIG_MAX_MOTOR_TORQUE 45.0F
 
 #define TRQ_PI_KP 0.05F
 #define TRQ_PI_KI 0.01F

Project/MC100.uvoptx

@@ -393,18 +393,6 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\trq2dq_table.c</PathWithFileName>
-      <FilenameWithoutPath>trq2dq_table.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>2</GroupNumber>
-      <FileNumber>16</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
       <PathWithFileName>..\Applications\foc\core\adrc.c</PathWithFileName>
       <FilenameWithoutPath>adrc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
@@ -412,7 +400,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>16</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -432,7 +420,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -444,7 +432,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -456,7 +444,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -468,7 +456,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -480,7 +468,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -492,7 +480,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -512,7 +500,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -524,7 +512,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -536,7 +524,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -548,7 +536,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -568,7 +556,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -580,7 +568,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -592,7 +580,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>4</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -604,7 +592,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -624,7 +612,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -636,7 +624,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -648,7 +636,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -660,7 +648,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -672,7 +660,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -684,7 +672,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -696,7 +684,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -708,7 +696,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -720,7 +708,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -732,7 +720,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -744,7 +732,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -756,7 +744,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -768,7 +756,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -780,7 +768,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -792,7 +780,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -804,7 +792,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -816,7 +804,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -836,7 +824,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -848,7 +836,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -860,7 +848,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -872,7 +860,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -884,7 +872,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -896,7 +884,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -916,7 +904,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -928,7 +916,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -940,7 +928,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -960,7 +948,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -972,7 +960,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -984,7 +972,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -996,7 +984,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1008,7 +996,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1020,7 +1008,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1032,7 +1020,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1044,7 +1032,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1056,7 +1044,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1068,7 +1056,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1080,7 +1068,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1092,7 +1080,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1104,7 +1092,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>69</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1116,7 +1104,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>70</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1128,7 +1116,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>71</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1140,7 +1128,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>73</FileNumber>
+      <FileNumber>72</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1160,7 +1148,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>73</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1172,7 +1160,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>75</FileNumber>
+      <FileNumber>74</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/MC100.uvprojx

@@ -458,11 +458,6 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\foc_observer.c</FilePath>
             </File>
-            <File>
-              <FileName>trq2dq_table.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\trq2dq_table.c</FilePath>
-            </File>
             <File>
               <FileName>adrc.c</FileName>
               <FileType>1</FileType>

Simulink/motor_data_nihe.m

@@ -0,0 +1,11 @@
+clear;
+y=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','A2:A105'); % torque
+x=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','W2:W105'); % D
+z=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','X2:X105'); % Q
+y=abs(y);
+% Te = 1.5*4*(a*iq + (ld - lq)*idiq);
+ft=fittype('6*(a*iq+(ld-lq)*id*iq)', 'independent',{'id','iq'}, 'coefficients', {'a', 'ld', 'lq'});
+
+Y=fit([x,z],y,ft,'StartPoint',[0,0,0]);
+
+

Simulink/motor_map.m

@@ -0,0 +1,17 @@
+x=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','B2:B105');
+y=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','C2:C105'); 
+z=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','S2:S105');
+x=abs(x);
+[X,Y]=meshgrid([min(x):max(x)],[min(y):max(y)]);
+Z = griddata(x,y,z,X,Y);
+round(Z,1);
+pcolor(X,Y,Z);
+shading interp;hold on;colormap(jet);colorbar;
+[C,H]=contour(X,Y,Z,35,'k');
+A=xlabel('转 速 (rpm)');
+B=ylabel('功率(kW)');
+set(A,'Fontangle','normal','FontName','宋体','FontSize',15);
+set(B,'Fontangle','normal','FontName','宋体','FontSize',15);
+set(findobj('FontSize',10),'FontSize',15);
+clabel(C,H,'fontsize',10,'color','k');
+title('电机效率MAP')