update，解决高速急停相错误，dq查询异常，导致程序异常

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

Applications/app/nv_storage.c

@@ -43,10 +43,14 @@ static void nv_default_motor_params(void) {
 	m_params.r = MOTOR_R;
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
+#ifdef MOTOR_Flux
+	m_params.flux_linkage = MOTOR_Flux;
+#else
+	m_params.flux_linkage = 0;
+#endif
 	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
 	m_params.est_pll_band = 100;
 	m_params.pos_lock_pll_band = 500;
-	m_params.flux_linkage = 0.0f;
 }
 
 static void nv_default_limter(void) {
@@ -495,8 +499,6 @@ void nv_storage_init(void) {
 		foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
 	}
 #endif
-	foc_params.s_maxDCVol = CONFIG_MAX_DC_VOL;
-	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
-	sys_debug("%d -- %d\n", sizeof(mc_limit_t), sizeof(foc_params_t));
+	sys_debug("%f -- %f, flux: %f\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage);
 }
 

Applications/bsp/can.c

@@ -129,11 +129,6 @@ static void shark_can0_config(void)
   	nvic_irq_enable(USBD_LP_CAN0_RX0_IRQn,CAN_IRQ_PRIORITY,0);	
     /* enable can receive FIFO0 not empty interrupt */
     can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
-
-	//can_filter_mask_mode_init(0x1A0, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO1, 1);
-	//nvic_irq_enable(CAN0_RX1_IRQn,CAN_IRQ_PRIORITY,0);	
-    /* enable can receive FIFO1 not empty interrupt */
-    //can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE1);
 }
 
 

Applications/bsp/pwm.c

@@ -2,6 +2,7 @@
 #include "bsp/pwm.h"
 #include "bsp/adc.h"
 #include "os/os_task.h"
+#include "libs/logger.h"
 /*
 以下主要是在某一相电路无法采集的时候，需要对这相的pwm挖坑处理
 timer 分配:
@@ -56,6 +57,7 @@ void pwm_3phase_sides(bool hon, bool lon) {
     gpio_init(PWM_V_N_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_V_N_PIN);
     gpio_init(PWM_W_N_GROUP,GPIO_MODE_OUT_PP,GPIO_OSPEED_50MHZ,PWM_W_N_PIN);
 
+	sys_debug("pwm_3phase_sides\n");
 	/* 开上桥或者下桥之前先关闭下桥或者上桥 */
 	if (hon) {
 		_pwm_gpio_config();

Applications/foc/commands.c

@@ -214,7 +214,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Set_Ctrl_Mode:
 		{
 			u8 mode = decode_u8(command->data);
-			sys_debug("mode = %d\n", mode);
+			sys_debug("ctl mode = %d, len = %d\n", mode, command->len);
 			if (!mc_set_foc_mode(mode)) {
 				erroCode = PMSM_FOC_GetErrCode();
 			}

Applications/foc/core/PMSM_FOC_Core.c

@@ -219,6 +219,8 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.params.n_PhaseFilterCeof = CONFIG_CURR_LP_CEOF;
 	gFoc_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
 	gFoc_Ctrl.params.lq = nv_get_motor_params()->lq;
+	gFoc_Ctrl.params.ld = nv_get_motor_params()->ld;
+	gFoc_Ctrl.params.flux = nv_get_motor_params()->flux_linkage;
 	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
 	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
@@ -303,6 +305,21 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 	return 1;
 }
 //#define UPDATE_Lq_By_iq
+
+static __INLINE void id_feedforward(float eW) {
+#ifdef MOTOR_Flux
+	gFoc_Ctrl.in.s_targetVdq.d += -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
+	gFoc_Ctrl.in.s_targetVdq.d = fclamp(gFoc_Ctrl.in.s_targetVdq.d, gFoc_Ctrl.pi_id.min, gFoc_Ctrl.pi_id.max);
+#endif
+}
+
+static __INLINE void iq_feedforward(float eW) {
+#ifdef MOTOR_Flux
+	gFoc_Ctrl.in.s_targetVdq.q += (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
+	gFoc_Ctrl.in.s_targetVdq.q = fclamp(gFoc_Ctrl.in.s_targetVdq.q, gFoc_Ctrl.pi_iq.min, gFoc_Ctrl.pi_iq.max);
+#endif
+}
+
 void PMSM_FOC_Schedule(void) {
 
 	gFoc_Ctrl.ctrl_count++;
@@ -320,8 +337,10 @@ void PMSM_FOC_Schedule(void) {
 	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
 	#endif
+		float eW = gFoc_Ctrl.in.s_motRPM / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
+		id_feedforward(eW);
 #ifdef UPDATE_Lq_By_iq
 		/* update kp&ki from lq for iq PI controller */
 		float lq = motor_get_lq_from_iq((s16)gFoc_Ctrl.out.s_FilterIdq.q);
@@ -338,6 +357,7 @@ void PMSM_FOC_Schedule(void) {
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(&gFoc_Ctrl.pi_iq, err);
+		iq_feedforward(eW);
 	}else {
 		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
 		float max_vd = max_Vdc * SQRT3_BY_2;
@@ -389,7 +409,7 @@ void PMSM_FOC_Schedule(void) {
 }
 
 void PMSM_FOC_LogDebug(void) {
-	sys_debug("DC curr %f\n", gFoc_Ctrl.out.s_CalciDC);
+	sys_debug("DC curr %f, Lq %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.params.lq);
 }
 
 /*called in media task */

Applications/foc/core/PMSM_FOC_Core.h

@@ -75,6 +75,8 @@ typedef struct {
 	u8 	  n_poles;
 	float n_modulation;
 	float lq;
+	float ld;
+	float flux;
 	float n_PhaseFilterCeof;
 }FOC_Params;
 

Applications/foc/core/foc_observer.c

@@ -22,6 +22,7 @@ void foc_observer_init(void) {
 
 #define RPM_2_degree(rpm)  ((rpm) * 60.0f * nv_get_motor_params()->poles * FOC_CTRL_US)
 
+#if 0
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 	float prev_enc_angle = foc_obser.enc_angle;
 	float prev_enc_speed = foc_obser.enc_speed;
@@ -75,7 +76,18 @@ float foc_observer_speed(void) {
 	}
 	return foc_obser.enc_speed;
 }
+#else
+float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
+	foc_obser.enc_angle = motor_encoder_get_angle();
+	foc_obser.enc_speed = motor_encoder_get_speed();
+	return foc_obser.enc_angle;
+}
+
+float foc_observer_speed(void) {
+	return foc_obser.enc_speed;
+}
 
+#endif
 bool  foc_observer_is_encoder(void) {
 	return !foc_obser.smo_used;
 }

Applications/foc/limit.c

@@ -17,11 +17,11 @@ 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;

Applications/foc/motor/A1_motor_config.c

@@ -1,5 +1,5 @@
-static torque_map_t mtpa_fw_map[10][10] = {
- { // 转速：0-3000
+static torque_map_t mtpa_fw_map[11][10] = {
+ { // 转速：0-2000
  		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
 		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 497, -130, 480 }, // 电流矢量V：49.72927, D轴电流：-13, Q轴电流：48
@@ -10,8 +10,21 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2998, -2120, 2120 }, // 电流矢量V：299.8133, D轴电流：-212, Q轴电流：212
 		{ 3493, -2470, 2470 }, // 电流矢量V：349.3108, D轴电流：-247, Q轴电流：247
 		{ 4002, -2830, 2830 }, // 电流矢量V：400.2224, D轴电流：-283, Q轴电流：283
+
+	}, { // 转速：-3000
+ 		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
+		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 497, -170, 470 }, // 电流矢量V：49.72927, D轴电流：-13, Q轴电流：48
+		{ 993, -510, 850 }, // 电流矢量V：99.31767, D轴电流：-42, Q轴电流：90
+		{ 1500, -850, 1230 }, // 电流矢量V：150.0833, D轴电流：-75, Q轴电流：130
+		{ 2000, -1140, 1630 }, // 电流矢量V：200.3023, D轴电流：-115, Q轴电流：164
+		{ 2500, -1460, 2030 }, // 电流矢量V：250, D轴电流：-150, Q轴电流：200
+		{ 2998, -1920, 2290 }, // 电流矢量V：299.8133, D轴电流：-212, Q轴电流：212
+		{ 3493, -2340, 2600 }, // 电流矢量V：349.3108, D轴电流：-247, Q轴电流：247
+		{ 4002, -2820, 2830 }, // 电流矢量V：400.2224, D轴电流：-283, Q轴电流：283
 		
-	},  { // 转速：-4000
+	},
+	{ // 转速：-4000
 		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
 		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 500, -170, 470 }, // 电流矢量V：49.98, D轴电流：-17, Q轴电流：47
@@ -25,8 +38,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		
 	}, { // 转速：-4500
 		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
-		{ 497, -130, 480 }, // 电流矢量V：49.72927, D轴电流：-13, Q轴电流：48
+		{ 199, -52, 193 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 500, -130, 480 }, // 电流矢量V：49.72927, D轴电流：-13, Q轴电流：48
 		{ 1003, -350, 940 }, // 电流矢量V：100.3045, D轴电流：-35, Q轴电流：94
 		{ 1494, -700, 1320 }, // 电流矢量V：149.4122, D轴电流：-70, Q轴电流：132
 		{ 1969, -1090, 1640 }, // 电流矢量V：196.9188, D轴电流：-109, Q轴电流：164
@@ -37,7 +50,7 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		
 	}, { // 转速：-5000
 		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 199, -68,  188}, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 500, -170, 470 }, // 电流矢量V：49.98, D轴电流：-17, Q轴电流：47
 		{ 993, -420, 900 }, // 电流矢量V：99.31767, D轴电流：-42, Q轴电流：90
 		{ 1501, -860, 1230 }, // 电流矢量V：150.0833, D轴电流：-86, Q轴电流：123
@@ -49,7 +62,7 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		
 	}, { // 转速：-5500
 		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 199, -68, 188 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 500, -170, 470 }, // 电流矢量V：49.98, D轴电流：-17, Q轴电流：47
 		{ 993, -420, 900 }, // 电流矢量V：99.31767, D轴电流：-42, Q轴电流：90
 		{ 1492, -1040, 1070 }, // 电流矢量V：149.2146, D轴电流：-104, Q轴电流：107
@@ -60,8 +73,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2293, -1960, 1190 }, // 电流矢量V：229.2968, D轴电流：-196, Q轴电流：119
 		
 	}, { // 转速：-6000
-		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 0, -10, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
+		{ 199, -68, 188 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 500, -170, 470 }, // 电流矢量V：49.98, D轴电流：-17, Q轴电流：47
 		{ 1003, -630, 780 }, // 电流矢量V：100.2646, D轴电流：-63, Q轴电流：78
 		{ 1496, -1180, 920 }, // 电流矢量V：149.6262, D轴电流：-118, Q轴电流：92
@@ -72,8 +85,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2201, -1950, 1020 }, // 电流矢量V：220.0659, D轴电流：-195, Q轴电流：102
 		
 	}, { // 转速：-6500
-		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -17, 198 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
+		{ 0, -30, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
+		{ 199, -68, 188 }, // 电流矢量V：19.87285, D轴电流：-1.7, Q轴电流：19.8
 		{ 500, -170, 470 }, // 电流矢量V：49.98, D轴电流：-17, Q轴电流：47
 		{ 983, -690, 700 }, // 电流矢量V：98.29038, D轴电流：-69, Q轴电流：70
 		{ 1407, -1150, 810 }, // 电流矢量V：140.6627, D轴电流：-115, Q轴电流：81
@@ -84,8 +97,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2080, -1875, 900 }, // 电流矢量V：207.9814, D轴电流：-187.5, Q轴电流：90
 		
 	}, { // 转速：-7000
-		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -68, 187 }, // 电流矢量V：19.89799, D轴电流：-6.8, Q轴电流：18.7
+		{ 0, -60, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
+		{ 199, -124, 157 }, // 电流矢量V：19.89799, D轴电流：-6.8, Q轴电流：18.7
 		{ 498, -310, 390 }, // 电流矢量V：49.81968, D轴电流：-31, Q轴电流：39
 		{ 976, -770, 600 }, // 电流矢量V：97.6166, D轴电流：-77, Q轴电流：60
 		{ 1482, -1290, 730 }, // 电流矢量V：148.2228, D轴电流：-129, Q轴电流：73
@@ -96,8 +109,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2126, -1970, 800 }, // 电流矢量V：212.6241, D轴电流：-197, Q轴电流：80
 		
 	}, { // 转速：-7500
-		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -86, 180 }, // 电流矢量V：19.94893, D轴电流：-8.6, Q轴电流：18
+		{ 0, -125, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
+		{ 199, -151, 130 }, // 电流矢量V：19.94893, D轴电流：-8.6, Q轴电流：18
 		{ 502, -378, 330 }, // 电流矢量V：50.17808, D轴电流：-37.8, Q轴电流：33
 		{ 990, -830, 540 }, // 电流矢量V：99.0202, D轴电流：-83, Q轴电流：54
 		{ 1498, -1340, 670 }, // 电流矢量V：149.8166, D轴电流：-134, Q轴电流：67
@@ -108,8 +121,8 @@ static torque_map_t mtpa_fw_map[10][10] = {
 		{ 2026, -1890, 730 }, // 电流矢量V：202.608, D轴电流：-189, Q轴电流：73
 		
 	}, { // 转速：-8000
-		{ 0, 0, 0 }, // 电流矢量V：0, D轴电流：0, Q轴电流：0
-		{ 199, -86, 180 }, // 电流矢量V：19.94893, D轴电流：-8.6, Q轴电流：18
+		{ 0, -250, 0 }, // 电流矢量V：0, D轴电流：-24, Q轴电流：4.5
+		{ 325, -280, 165 }, // 电流矢量V：19.94893, D轴电流：-8.6, Q轴电流：18
 		{ 655, -560, 340 }, // 电流矢量V：65.51336, D轴电流：-56, Q轴电流：34
 		{ 1190, -1060, 540 }, // 电流矢量V：118.9622, D轴电流：-106, Q轴电流：54
 		{ 1677, -1550, 640 }, // 电流矢量V：167.6932, D轴电流：-155, Q轴电流：64
@@ -121,18 +134,18 @@ static torque_map_t mtpa_fw_map[10][10] = {
 	},
 };
 
-static motor_map_t mot_map[] = {
-	{0, 400},
-	{3000, 400},
-	{4000, 322},
-	{4500, 276},
-	{5000, 246},
-	{5500, 230},
-	{6000, 220},
-	{6500, 208},
-	{7000, 208},
-	{7500, 203},
-	{8000, 193},
+static motor_map_t mot_map[] = {
+	{0, 400},
+	{3000, 400},
+	{4000, 322},
+	{4500, 276},
+	{5000, 246},
+	{5500, 230},
+	{6000, 220},
+	{6500, 208},
+	{7000, 208},
+	{7500, 203},
+	{8000, 193},
 };
 
 static iq_lq_map_t iq_lq_map[] = {

Applications/foc/motor/motor.c

@@ -413,6 +413,11 @@ bool mc_set_foc_mode(u8 mode) {
 	if (!motor.b_start) {
 		return false;
 	}
+	if ((mode == CTRL_MODE_OPEN) && (ABS(PMSM_FOC_GetSpeed()) > CONFIG_ZERO_SPEED_RPM)) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
+
 	u32 mask = cpu_enter_critical();
 	bool ret = false;
 	if (PMSM_FOC_SetCtrlMode(mode)) {
@@ -911,6 +916,7 @@ static void _pwm_brake_prot_timer_handler(shark_timer_t *t){
 	pwm_brake_enable(true);
 	sys_debug("MC protect error\n");
 }
+static float err_vbus, err_id,err_iq, ctl_mode;
 
 void MC_Protect_IRQHandler(void){
 	pwm_brake_enable(false);
@@ -918,6 +924,10 @@ void MC_Protect_IRQHandler(void){
 	if (!motor.b_start) {
 		return;
 	}
+	err_vbus = get_vbus_float();
+	err_id = PMSM_FOC_Get()->idq_ctl[0].s_Cp;
+	err_iq = PMSM_FOC_Get()->idq_ctl[1].s_Cp;
+	ctl_mode = PMSM_FOC_Get()->out.n_RunMode;
 	mc_set_critical_error(FOC_CRIT_Phase_Err);
 	_mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
@@ -926,6 +936,10 @@ void MC_Protect_IRQHandler(void){
 	pwm_up_enable(true);
 }
 
+void motor_debug(void) {
+	sys_debug("err: %f, %f, %f, %d\n", err_vbus, err_id, err_iq, ctl_mode);
+}
+
 void TIMER_UP_IRQHandler(void){
 	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
 		motor_encoder_update();

Applications/foc/motor/motor_param.c

@@ -5,10 +5,10 @@
 
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 #define MOT_HAVE_MAPS
-#define MOT_USE_PHASE_I //表示使用电流矢量和RPM查表，获取D轴电流，iq = 开根号(电流矢量的平方 + D轴电流的平方)
-#define RPM_MAX_IDX 10
+#define MOT_USE_PHASE_I //表示使用电流矢量和RPM查表，获取D轴电流，iq = 开根号(电流矢量的平方 - D轴电流的平方)
+#define RPM_MAX_IDX 11
 #define TRQ_MAX_IDX 10
-static int map_rpm[] = {3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000};
+static int map_rpm[] = {2000, 3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000};
 #include "foc/motor/A1_motor_config.c"
 
 #endif
@@ -98,7 +98,7 @@ static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float
 	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);
+	_DEBUG("z: %d, low --> %d %d\n", z, 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);
 	}
@@ -113,7 +113,19 @@ static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float
 	float c2 = (1.0f - frac_z2) * map[2]->d + frac_z2 * map[3]->d; //第二行插值
 	*d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
 #ifdef MOT_USE_PHASE_I
-	*q = sqrtf(z*z - (*d)*(*d));
+	if (z != 0) {
+		if (z >= ABS(*d)) {
+			*q = sqrtf(z*z - (*d)*(*d));
+		}else {
+			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;
+		}
+	}else {
+		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;
+	}
 #else
 	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;
@@ -175,12 +187,16 @@ void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 		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;
+	if (itorque != 0) {
+		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 {
+		step_towards(&dq_out->d, d/10.0f, 0.5f);
+		step_towards(&dq_out->q, q/10.0f, 0.5f);
 	}
 }
 #else

Applications/foc/motor/motor_param.h

@@ -45,7 +45,7 @@ float motor_get_lq_from_iq(s16 iq);
 #define MOTOR_R   0.011f
 #define MOTOR_Ld (0.000140F*0.5f)
 #define MOTOR_Lq (0.000356f*0.5f)
-
+#define MOTOR_Flux (0.013f)
 //#define MOTOR_R   0.033f
 //#define MOTOR_Ld (0.000168f * 0.5f)//(0.000140F*0.5f)
 //#define MOTOR_Lq (0.000330f * 0.5f)//(0.000340f*0.5f)

Simulink/foc_adrc.m

@@ -13,7 +13,7 @@ Ts_Spd_ctl          = 1/f_speed_ctrl;
 n_polePairs  = 4;        % [-] Number of motor pole pairs
 PM           = 0.01688;   % Permanent magnet flux linkage, 
 Ld           = 0.07e-3;% d-axis inductance, 
-Lq           = 0.150e-3; % q-axis inductance,
+Lq           = 0.09e-3; % q-axis inductance,
 Rs           = 0.011;   % Stator resistance,
 J            = 0.03945; % Moment of inertia,
 bandwith     = 5000;