update

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

Applications/app/app.c

@@ -96,7 +96,7 @@ static u32 _app_report_task(void *p) {
 	can_report_ext_status(0x43);
 	
 	if (++loop % 10 == 0) {
-		sys_debug("modulation %f\n", PMSM_FOC_Get()->out.f_vdqRation);
+		sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->in.s_targetTorque);
 		encoder_log();
 		err_code_log();
 	}
@@ -106,7 +106,7 @@ static u32 _app_report_task(void *p) {
 //static float _angle = 0.0f;
 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_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
 #if 0
 	if (!_mc_start) {
 		_mc_start = true;

Applications/app/nv_storage.c

@@ -58,7 +58,7 @@ static void nv_default_foc_params(void) {
 	foc_params.n_acc_time = CONFIG_ACC_TIME;
 	foc_params.n_dec_time = CONFIG_DEC_TIME;
 	foc_params.n_ebrk_time = CONFIG_EBRK_RAMP_TIME;
-	
+	foc_params.n_FwEnable  = CONFIG_DEFAULT_FW_ENABLE;
 	foc_params.pid_conf[PID_D_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Ld);
 	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
 	foc_params.pid_conf[PID_D_id].kb = 0;
@@ -83,6 +83,10 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
 	foc_params.pid_conf[PID_Lock_id].kb = 0;
 
+	foc_params.pid_conf[PID_FW_id].kp = 0;
+	foc_params.pid_conf[PID_FW_id].ki = 0.01f;
+	foc_params.pid_conf[PID_FW_id].kb = 0;
+
 }
 
 void nv_save_motor_params(void) {

Applications/app/nv_storage.h

@@ -24,6 +24,7 @@ typedef struct {
 	u32   n_acc_time;
 	u32   n_dec_time;
 	u32   n_ebrk_time;
+	u8    n_FwEnable;
 	pid_conf_t pid_conf[PID_Max_id];
 	u16   crc16;
 }foc_params_t;

Applications/foc/core/PMSM_FOC_Core.c

@@ -154,6 +154,11 @@ static void PMSM_FOC_Conf_PID(void) {
 	gFoc_Ctrl.pi_lock->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
 	gFoc_Ctrl.pi_lock->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
 
+	gFoc_Ctrl.pi_fw->kp = nv_get_foc_params()->pid_conf[PID_FW_id].kp;
+	gFoc_Ctrl.pi_fw->ki = nv_get_foc_params()->pid_conf[PID_FW_id].ki;
+	gFoc_Ctrl.pi_fw->kb = nv_get_foc_params()->pid_conf[PID_FW_id].kb;
+	gFoc_Ctrl.pi_fw->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	PI_Controller_max(gFoc_Ctrl.pi_fw, 0, -CONFIG_MAX_FW_D_CURR);
 }
 
 static void PMSM_FOC_UserInit(void) {
@@ -207,10 +212,12 @@ void PMSM_FOC_CoreInit(void) {
 		shark_task_create(PMSM_FOC_Debug_Task, NULL);
 		g_focinit = true;
 	}
+	
 	gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
 	gFoc_Ctrl.params.n_PhaseFilterCeof = (CONFIG_CURR_LP_PARAM>1.0f?1.0f:CONFIG_CURR_LP_PARAM);
 	gFoc_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
 	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);
 
 	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
@@ -258,6 +265,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	//sample current
 	phase_current_get(gFoc_Ctrl.in.s_iABC);
 	get_phase_vols(gFoc_Ctrl.in.s_vABC);
+#if 0
 	gFoc_Ctrl.in.s_vABC[0] -= gFoc_Ctrl.in.s_vDC/2.0f;
 	gFoc_Ctrl.in.s_vABC[1] -= gFoc_Ctrl.in.s_vDC/2.0f;
 	gFoc_Ctrl.in.s_vABC[2] -= gFoc_Ctrl.in.s_vDC/2.0f;
@@ -265,7 +273,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	Clark(gFoc_Ctrl.in.s_vABC[0], gFoc_Ctrl.in.s_vABC[1], gFoc_Ctrl.in.s_vABC[2], &vAB);
 	
 	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealVdq);
-
+#endif
 #ifdef PHASE_LFP
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
@@ -382,7 +390,7 @@ void PMSM_FOC_Schedule(void) {
 			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
 				plot_3data16(FtoS16(gFoc_Ctrl.in.s_iABC[0]), FtoS16(gFoc_Ctrl.in.s_iABC[1]), FtoS16(gFoc_Ctrl.in.s_iABC[2]));
 			}else if (gFoc_Ctrl.plot_type == Plot_Phase_vol) {
-				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_vABC[0]), FtoS16x10(gFoc_Ctrl.in.s_vABC[1]), FtoS16x10(gFoc_Ctrl.in.s_vABC[2]));
+				plot_3data16(FtoS16(gFoc_Ctrl.in.s_vABC[0]), FtoS16(gFoc_Ctrl.in.s_vABC[1]), FtoS16(gFoc_Ctrl.in.s_vABC[2]));
 			}
 		}
 	}
@@ -435,6 +443,11 @@ u8 PMSM_FOC_CtrlMode(void) {
 }
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
+static __INLINE void PMSM_FOC_FieldWeak(void) {
+	if (!gFoc_Ctrl.in.b_fwEnable) {
+		return;
+	}
+}
 static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 #if 1
 	PI_Ctrl_Power.max = maxTrq;
@@ -463,6 +476,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)) {
 		torque_get_idq(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_motRPM, &gFoc_Ctrl.in.s_targetIdq);
+		PMSM_FOC_FieldWeak();
 	}
 	u32 mask = cpu_enter_critical();
 	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
@@ -558,7 +572,10 @@ void PMSM_FOC_Slow_Task(void) {
 }
 
 float PMSM_FOC_Get_Real_Torque(void) {
-	return sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
+	if (gFoc_Ctrl.out.s_RealCurrent == 0) {
+		gFoc_Ctrl.out.s_RealCurrent = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
+	}
+	return gFoc_Ctrl.out.s_RealCurrent;
 }
 
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
@@ -606,9 +623,27 @@ void PMSM_FOC_SpeedLimit(float speedLimit) {
 		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
 	}
 	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
+	eRamp_set_time(&gFoc_Ctrl.rtLim.rpmLimRamp, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 	eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
 }
 
+/* 扭矩模式下的限速，主要是处理转把对应的最高速 */
+void PMSM_FOC_Torque_SpeedLimit(float speedLimit) {
+	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
+		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
+	}
+	if (speedLimit > gFoc_Ctrl.userLim.s_motRPMLim) {
+		speedLimit = gFoc_Ctrl.userLim.s_motRPMLim;
+	}
+	eRamp_set_time(&gFoc_Ctrl.rtLim.rpmLimRamp, 1000, 1000);
+	if (gFoc_Ctrl.in.s_motRPM == 0) {
+		eRamp_reset_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit/1.3f);//1.3的系数防止限速过冲
+		eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
+	}else {
+		eRamp_set_step_target(&gFoc_Ctrl.rtLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
+	}
+}
+
 float PMSM_FOC_GetSpeedLimit(void) {
 	return gFoc_Ctrl.userLim.s_motRPMLim;
 }
@@ -906,8 +941,8 @@ u32 PMSM_FOC_GetCriticalError(void) {
 void PMSM_FOC_Set_PlotType(Plot_t t) {
 	gFoc_Ctrl.plot_type = t;
 }
-//获取母线电流
-float PMSM_FOC_Calc_iDC(void) {
+//获取母线电流和实际输出电流矢量大小
+void PMSM_FOC_Calc_Current(void) {
 	float vd = gFoc_Ctrl.out.s_OutVdq.d;
 	float vq = gFoc_Ctrl.out.s_OutVdq.q;
 
@@ -924,7 +959,9 @@ float PMSM_FOC_Calc_iDC(void) {
 	}
 	float raw_idc = m_pow / get_vbus_float() * (1.0f - 0.2f * id_thr/100.0f);// * 1.5f * 0.66f; //s16q5
 	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);
-	return gFoc_Ctrl.out.s_FilteriDC;
+
+	gFoc_Ctrl.out.s_RealCurrent = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
+
 }
 
 void PMSM_FOC_Brake(bool brake) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -95,6 +95,7 @@ typedef struct {
 	bool    b_AutoHold;
 	bool    b_eBrake;
 	bool    b_epmMode;
+	bool    b_fwEnable;
 	volatile bool    b_MTPA_calibrate;
 	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
 }FOC_InP;
@@ -147,6 +148,7 @@ typedef struct {
 	DQ_t  s_RealVdq;
 	DQ_t  s_FilterIdq;
 	float s_FilteriDC;
+	float s_RealCurrent;
 	float f_vdqRation;
 	s16   test_sample;
 	float sin;
@@ -249,7 +251,7 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm);
 float PMSM_FOC_GetSpeed(void);
 bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
-float PMSM_FOC_Calc_iDC(void);
+void PMSM_FOC_Calc_Current(void);
 void PMSM_FOC_AutoHold(bool lock);
 void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
 void PMSM_FOC_Set_Angle(float angle);
@@ -284,6 +286,7 @@ void PMSM_FOC_RT_PhaseCurrLim(float lim);
 void PMSM_FOC_RT_LimInit(void);
 float PMSM_FOC_Get_Real_Torque(void);
 void PMSM_FOC_Reset_Torque(void);
+void PMSM_FOC_Torque_SpeedLimit(float speedLimit);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/torque.c

@@ -72,12 +72,12 @@ float throttle_to_torque(float f_throttle) {
 	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
 }
 
-#define REAL_DQ_CEOF 1.1f
-
+#define REAL_DQ_CEOF 0.9f
+#define FINAL_DQ_CEFO 1.1F
+#define START_RPM_FOR_AUDOHOLD 10.0F
 #if 1
 void torque_mode_process(void) {
 	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
-//	float pre_trq = g_trq_mn.torque_prev;
 	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
 		g_trq_mn.thro_value = 0;
 		g_trq_mn.torque_prev = 0;
@@ -86,14 +86,14 @@ void torque_mode_process(void) {
 	}
 	
 	if (!mc_throttle_released()) {
-		if (PMSM_FOC_GetSpeed() <= 10.0f) {
-			ref_trq  = MAX(eCtrl_get_FinalTorque() * REAL_DQ_CEOF, ref_trq );
+		if (PMSM_FOC_GetSpeed() <= START_RPM_FOR_AUDOHOLD) {
+			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() * 0.9f;
+		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);

Applications/foc/foc_config.h

@@ -4,7 +4,7 @@
 
 #define CONFIG_DEFAULT_IDC_LIM 45
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
-#define CONFIG_DEFAULT_RPM_LIM       6000
+#define CONFIG_DEFAULT_RPM_LIM       3500
 #define CONFIG_DEFAULT_LOCK_PHASE_CURR_LIM 100
 
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 50
@@ -14,7 +14,7 @@
 #define CONFIG_SVM_MODULATION       1.0f//(1.0F/SQRT3_BY_2)
 #define CONFIG_BRK_SHUT_POWER_ENABLE 1
 #define CONFIG_AUTOHOLD_ENABLE 1
-
+#define CONFIG_DEFAULT_FW_ENABLE 1
 /* 转把 */
 #define CONFIG_THROTTLE_LOW_VALUE 1.2f /* 转把最小值 */
 #define CONFIG_THROTTLE_MAX_VALUE 3.8f /* 转把最大值 */
@@ -51,5 +51,7 @@
 
 #define CURRENT_LOOP_RAMP_COUNT 300
 
+#define CONFIG_TORQUE_MODE_MIN_RPM 600
+
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor.c

@@ -653,8 +653,8 @@ void Sched_MC_mTask(void) {
 
 	/*保护功能*/
 	PMSM_FOC_RunTime_Limit();
-	/* 母线电流计算 */
-	PMSM_FOC_Calc_iDC();
+	/* 母线电流，实际采集的相电流矢量大小的计算 */
+	PMSM_FOC_Calc_Current();
 
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
 		return;

Applications/math/fast_math.h

@@ -43,6 +43,26 @@ static void fast_norm_angle(float *angle) {
 	}
 }
 
+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;
+}
+
+static __INLINE void step_towards(float *value, float goal, float step) {
+    if (*value < goal) {
+        if ((*value + step) < goal) {
+            *value += step;
+        } else {
+            *value = goal;
+        }
+    } else if (*value > goal) {
+        if ((*value - step) > goal) {
+            *value -= step;
+        } else {
+            *value = goal;
+        }
+    }
+}
+
 static void normal_sincosf(float angle, float *sin, float *cos) {
 	*sin = arm_sin_f32(angle);
 	*cos = arm_cos_f32(angle);