限速还是回退到限制扭矩，如果限制电压，速度环和扭矩模式切换不平滑

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

Applications/app/app.c

@@ -102,9 +102,9 @@ static u32 _app_report_task(void *p) {
 	can_report_phase_current(0x45);
 	if (++loop % 10 == 0) {
 		//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("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("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());

Applications/app/nv_storage.c

@@ -301,11 +301,6 @@ void nv_set_throttle_vol(float min, float max) {
 	foc_params.n_endThroVol = max;
 }
 
-void nv_set_ebrake_current(float phase_curr, float dc_curr) {
-	foc_params.s_PhaseCurreBrkLim = phase_curr;
-	foc_params.s_iDCeBrkLim = dc_curr;
-}
-
 trq2dq_table_t *_trq2dq_table(int idx) {
 	trq2dq_table_t *tbl = (trq2dq_table_t *)fmc_get_addr(idx);
 

Applications/app/nv_storage.h

@@ -114,7 +114,6 @@ void nv_set_pid(u8 id, pid_conf_t *pid);
 void nv_get_pid(u8 id, pid_conf_t *pid);
 void nv_set_hwbrake_mode(u8 mode);
 void nv_set_throttle_vol(float min, float max);
-void nv_set_ebrake_current(float phase_curr, float dc_curr);
 bool nv_set_gear_config(u8 mode4896, u8 gear, u16 rpm, u16 torque, u16 idc, u16 acc);
 bool nv_get_gear_config(u8 mode4896, u8 gear, u16 *rpm, u16 *torque, u16 *idc, u16 *acc);
 trq2dq_table_t *nv_get_trq2dq_table(void);

Applications/foc/commands.c

@@ -488,9 +488,10 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Set_eBrake_Throld:
 		{
 			if (command->len >= 4) {
-				nv_get_foc_params()->s_iDCeBrkLim = decode_s16((u8 *)command->data);
-				nv_get_foc_params()->s_LimitiDC = decode_s16((u8 *)command->data + 2);
-				nv_save_foc_params();
+
+				float phase_curr = (float)decode_s16((u8 *)command->data);
+				float dc_curr = (float)decode_s16((u8 *)command->data + 2);
+				PMSM_FOC_SeteBrkPhaseCurrent(phase_curr, dc_curr);
 			}
 			break;
 		}

Applications/foc/core/PI_Controller.h

@@ -65,6 +65,18 @@ static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	return sat_out;
 }
 
+static __INLINE float PI_Controller_RunLimit(PI_Controller *pi, float err) {
+	float kp_err = (err) * pi->kp;
+	float ki_err = (err) * pi->ki;
+	float integral = ki_err * pi->DT;
+	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
+	float out = pi->Ui + kp_err;
+	float sat_out = MATH_sat(out, pi->min, pi->max);
+	
+	return sat_out;
+}
+
+
 static __INLINE float PI_Controller_RunWithDiff(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;

Applications/foc/core/PMSM_FOC_Core.c

@@ -308,17 +308,12 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 
 static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	/* update id pi ctrl */
-	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-		gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	}else {
-		gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-		gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	}
+	gFoc_Ctrl.params.f_DCLim = gFoc_Ctrl.in.s_vDC;
+	gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+	gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+	gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+	gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+
 	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id->max) {
 		gFoc_Ctrl.pi_id->max = gFoc_Ctrl.params.maxvDQ.d;
 	}
@@ -521,14 +516,12 @@ static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 
 static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 #if 1
-	gFoc_Ctrl.pi_torque->max = gFoc_Ctrl.in.s_vDC;
+	gFoc_Ctrl.pi_torque->max = maxTrq;
 	gFoc_Ctrl.pi_torque->min = 0;
 
 	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
-	gFoc_Ctrl.params.f_DCLim = PI_Controller_RunWithDiff(gFoc_Ctrl.pi_torque, err);
-	return maxTrq;
+	return PI_Controller_RunLimit(gFoc_Ctrl.pi_torque, err);
 #else
-	gFoc_Ctrl.params.f_DCLim = gFoc_Ctrl.in.s_vDC;
 	return maxTrq;
 #endif
 }
@@ -624,6 +617,15 @@ bool PMSM_FOC_RunTime_Limit(void) {
 	return changed;
 }
 
+
+bool PMSM_FOC_iDC_is_Limited(void) {
+	return (gFoc_Ctrl.protLim.s_iDCLim != HW_LIMIT_NONE);
+}
+
+bool PMSM_FOC_Torque_is_Limited(void) {
+	return (gFoc_Ctrl.protLim.s_TorqueLim != HW_LIMIT_NONE);
+}
+
 void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.rtLim.torqueLimRamp);
 	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
@@ -732,8 +734,9 @@ float PMSM_FOC_GetTorqueLimit(void) {
 	return gFoc_Ctrl.userLim.s_torqueLim;
 }
 
-void PMSM_FOC_SeteBrkPhaseCurrent(float curr) {
-	gFoc_Ctrl.userLim.s_PhaseCurreBrkLim = curr;
+void PMSM_FOC_SeteBrkPhaseCurrent(float phase_curr, float dc_curr) {
+	gFoc_Ctrl.userLim.s_PhaseCurreBrkLim = fclamp(phase_curr, 0, nv_get_foc_params()->s_PhaseCurreBrkLim);
+	gFoc_Ctrl.userLim.s_iDCeBrkLim = fclamp(dc_curr, 0, nv_get_foc_params()->s_iDCeBrkLim);
 }
 
 float PMSM_FOC_GeteBrkPhaseCurrent(void) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -275,7 +275,7 @@ bool PMSM_FOC_Set_epmMode(bool epm);
 bool PMSM_FOC_is_epmMode(void);
 bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir);
 EPM_Dir_t PMSM_FOC_Get_epmDir(void);
-void PMSM_FOC_SeteBrkPhaseCurrent(float curr);
+void PMSM_FOC_SeteBrkPhaseCurrent(float phase_curr, float dc_curr);
 float PMSM_FOC_GeteBrkPhaseCurrent(void);
 void PMSM_FOC_PhaseCurrLim(float lim);
 float PMSM_FOC_GetPhaseCurrLim(void);
@@ -293,6 +293,8 @@ void PMSM_FOC_RT_LimInit(void);
 float PMSM_FOC_Get_Real_Torque(void);
 void PMSM_FOC_Reset_Torque(void);
 void PMSM_FOC_SpeedDirectLimit(float limit);
+bool PMSM_FOC_iDC_is_Limited(void);
+bool PMSM_FOC_Torque_is_Limited(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/e_ctrl.c

@@ -97,7 +97,7 @@ static void _eCtrl_process_eBrake(void) {
 	}
 }
 
-bool eCtrl_is_eBrk_enabled(void) {
+bool eCtrl_is_eBrk_Running(void) {
 	return g_eCtrl.is_ebrake;
 }
 
@@ -191,7 +191,7 @@ void eCtrl_brake_signal(bool hw_brake) {
 		}
 	}
 	if (_eCtrl_isHwBrk_shutPower()) {
-		if (!eCtrl_is_eBrk_enabled() && !eCtrl_enable_eBrake(true)) {
+		if (!eCtrl_is_eBrk_Running() && !eCtrl_enable_eBrake(true)) {
 			_eCtrl_clear_ramp();
 		}
 	}

Applications/foc/core/e_ctrl.h

@@ -209,7 +209,7 @@ static void eRamp_set_X2_target(e_Ramp *r, float c) {
 void eCtrl_init(u16 accl_time, u16 dec_time);
 void eCtrl_set_ebrk_time(u16 ebrk_time);
 void eCtrl_brake_signal(bool hw_brake);
-bool eCtrl_is_eBrk_enabled(void);
+bool eCtrl_is_eBrk_Running(void);
 void eCtrl_set_TgtCurrent(float c);
 void eCtrl_set_TgtTorque(float t);
 void eCtrl_set_TgtSpeed(float s);

Applications/foc/foc_config.h

@@ -57,7 +57,7 @@
 
 #define CONFIG_TORQUE_MODE_MIN_RPM 600
 
-#define CONFIG_MAX_NEG_TORQUE 2.0F
+#define CONFIG_MAX_NEG_TORQUE 0.0F
 
 #ifdef CONFIG_SMO_OBSERVER
 	#define CONFIG_SMO_MIN_SPEED    1000 //RPM

Applications/foc/motor/motor.c

@@ -157,7 +157,7 @@ static void mc_gear_vmode_changed(void) {
 	}else {
 		gears = &nv_get_gear_configs()->gears_48[0];
 	}
-	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");
+	//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_SpeedLimit(gears[motor.n_gear].u_maxRPM);
 	PMSM_FOC_DCCurrLimit(gears[motor.n_gear].u_maxIdc);
 	PMSM_FOC_TorqueLimit(gears[motor.n_gear].u_maxTorque);
@@ -462,6 +462,26 @@ void mc_get_running_status(u8 *data) {
 	data[0] |= (motor.b_lock_motor) << 7; //motor locked
 }
 
+u16 mc_get_running_status2(void) {
+	u16 data = 0;
+	data = motor.n_gear;
+	data |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
+	data |= (motor.b_break?1:0) << 3;
+	data |= (PMSM_FOC_Get()->in.b_cruiseEna?1:0) << 4;
+	data |= (motor.b_start?1:0) << 5;
+	data |= (mc_is_epm()?1:0) << 6;
+	data |= (motor.b_lock_motor) << 7; //motor locked
+	data |= (eCtrl_is_eBrk_Running()?1:0) << 8; //能量回收运行标志
+	data |= ((motor.n_CritiCalErrMask != 0)?1:0) << 9;
+	data |= (fan_pwm_is_running()?1:0) << 10; //风扇是否运行
+	data |= (motor.b_runStall?1:0) << 11; //是否堵转
+	data |= (PMSM_FOC_iDC_is_Limited()?1:0) << 12; //是否欠压限制母线电流
+	data |= (PMSM_FOC_Torque_is_Limited()?1:0) << 13; //是否高温限扭矩
+	data |= (motor.b_is96Mode?1:0) << 14; //是否高压模式
+	return data;
+}
+
+
 static float _force_angle = 0.0f;
 static int   _force_wait = 2000;
 /* 开环，强制给定电角度和DQ的电压 */

Applications/foc/motor/motor.h

@@ -71,6 +71,7 @@ void mc_clr_critical_error(u8 err);
 u32 mc_get_critical_error(void);
 void mc_set_fan_duty(u8 duty);
 void mc_force_run_open(s16 vd, s16 vq);
+u16 mc_get_running_status2(void);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL

Applications/prot/can_foc_msg.c

@@ -90,6 +90,17 @@ void can_report_foc_status(u8 can) {
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
+/* 采用组播方式上报,目的7E */
+void can_mcast_foc_status2(u8 can) {
+	u8 data[8];
+	encode_u16(data, mc_get_running_status2());
+	encode_u16(data + 2, ABS((s16)(motor_encoder_get_speed())));
+	float vDC = get_vbus_float();
+	encode_s16(data + 4, (s16)(vDC*10));
+	float iDC = PMSM_FOC_GetVbusCurrent();
+	encode_s16(data + 6, (s16)(iDC*10));
+	shark_can0_send_ext_message(get_indicator_can_id(0x7E), data, sizeof(data));
+}
 
 void can_report_mpta_values(u8 can) {
 	u8 data[8];