解决除0问题

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

Applications/app/app.c

@@ -25,6 +25,7 @@ extern void PMSM_FOC_LogDebug(void);
 extern void err_code_log(void);
 extern void encoder_log(void);
 extern void thro_torque_log(void);
+extern void eCtrl_debug_log(void);
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
 extern measure_time_t g_meas_MCTask;
@@ -103,17 +104,18 @@ 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("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());
-		sys_debug("target current %f\n", PMSM_FOC_Get()->in.s_targetCurrent);
-		thro_torque_log();
+		//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());
+		//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();
-		PMSM_FOC_LogDebug();
+		//encoder_log();
+		///PMSM_FOC_LogDebug();
+		eCtrl_debug_log();
 		//err_code_log();
 	}
 	return 200;

Applications/foc/core/PMSM_FOC_Core.c

@@ -487,7 +487,8 @@ u8 PMSM_FOC_CtrlMode(void) {
 			float real_trq = PMSM_FOC_Get_Real_dqVector() * 0.9f;
 			eCtrl_reset_Current(min(real_trq, gFoc_Ctrl.in.s_targetTorque));
 			eCtrl_set_TgtCurrent(-PMSM_FOC_GetEbrkTorque());
-#else			
+#else
+			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
 			eCtrl_set_TgtTorque(-PMSM_FOC_GetEbrkTorque());
 #endif
 		}	
@@ -522,11 +523,6 @@ static void crosszero_step_towards(float *value, float target) {
 
 
 /* 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
 	gFoc_Ctrl.pi_power.max = maxTrq;
@@ -575,7 +571,6 @@ 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);
-		PMSM_FOC_FieldWeak();
 	}
 	u32 mask = cpu_enter_critical();
 	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);

Applications/foc/core/e_ctrl.c

@@ -12,6 +12,8 @@ static void _eCtrl_set_TgtTorque(float t);
 static bool _eCtrl_isHwBrk_shutPower(void);
 static void _eCtrl_process_eBrake(void);
 
+#define CURRENT_RAMP_TIME 5*1000
+
 void eCtrl_init(u16 accl_time, u16 dec_time){
 	g_eCtrl.dec_time = dec_time;
 	g_eCtrl.accl_time = accl_time;
@@ -28,11 +30,16 @@ void eCtrl_init(u16 accl_time, u16 dec_time){
 	g_eCtrl.dec_time_shadow = g_eCtrl.dec_time;
 	g_eCtrl.hw_brake = false;
 	g_eCtrl.is_ebrake = false;
-	eRamp_init(&g_eCtrl.current, 10*1000, 10*1000);
+	eRamp_init(&g_eCtrl.current, CURRENT_RAMP_TIME, CURRENT_RAMP_TIME);
 	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
 	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
 }
 
+void eCtrl_debug_log(void) {
+	sys_debug("final %f, step %f, inter %f, shadow %f\n", g_eCtrl.torque.target, g_eCtrl.torque.step_val, g_eCtrl.torque.interpolation, g_eCtrl.torque_shadow);
+	sys_debug("final %f, %f\n", g_eCtrl.torque.acct, g_eCtrl.torque.dect);
+}
+
 void eCtrl_set_TgtCurrent(float c) {
 	g_eCtrl.current_shadow = c;
 }
@@ -74,7 +81,7 @@ bool eCtrl_enable_eBrake(bool enable) {
 }
 
 static void _eCtrl_clear_ramp(void) {
-	eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_time);
+	eRamp_init(&g_eCtrl.current, CURRENT_RAMP_TIME, CURRENT_RAMP_TIME);
 	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
 	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
 	g_eCtrl.current_shadow = 0.0f;
@@ -85,8 +92,6 @@ static void _eCtrl_clear_ramp(void) {
 static void _eCtrl_process_eBrake(void) {
 	if (g_eCtrl.is_ebrake) {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_EBRAKE);
-		eRamp_reset_target(&g_eCtrl.current, 0);
-		g_eCtrl.current_shadow = 0.0f;
 	}else {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
 	}

Applications/foc/core/e_ctrl.h

@@ -45,8 +45,8 @@ static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
 	r->interpolation = 0;
 	r->step_val = 0;
 	r->first_step = 0;
-	r->acct = acc;
-	r->dect = dec;
+	r->acct = (float)acc;
+	r->dect = (float)dec;
 }
 
 static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
@@ -56,8 +56,8 @@ static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
 	r->interpolation = target;
 	r->step_val = 0;
 	r->first_step = 0;
-	r->acct = acc;
-	r->dect = dec;
+	r->acct = (float)acc;
+	r->dect = (float)dec;
 }
 
 static void eRamp_reset_target(e_Ramp *r, float target) {
@@ -69,8 +69,8 @@ static void eRamp_reset_target(e_Ramp *r, float target) {
 	r->first_step = 0;
 }
 static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
-	r->acct = acc;
-	r->dect = dec;
+	r->acct = (float)acc;
+	r->dect = (float)dec;
 }
 
 static void eRamp_set_target(e_Ramp *r, float target) {
@@ -169,6 +169,7 @@ static void eRamp_X2_running(e_Ramp *r) {
 	eRamp_running(r);
 }
 #endif
+
 #if 0
 static void eRamp_set_X2_target(e_Ramp *r, float c) {
 #if 1

Applications/foc/core/thro_torque.c

@@ -88,6 +88,9 @@ static float thro_torque_for_accelerate(void) {
 }
 
 static float thro_torque_for_decelerate(void) {
+	if (_torque.thro_ration_last == 0.0f) {
+		return 0;
+	}
 	float dec_r = _torque.thro_ration / _torque.thro_ration_last;
 	dec_r = fclamp(dec_r, 0.0f, 1.0f);
 	return dec_r * _torque.torque_real;
@@ -150,7 +153,7 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 		_torque.accl = true;
 	}else if (thro_r < _torque.thro_ration) {
 		if (_torque.accl) {
-			_torque.thro_ration_last = _torque.thro_ration + 1e-10f; // add 1e-10f avoid divide zero
+			_torque.thro_ration_last = _torque.thro_ration;
 			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
 		}
 		_torque.accl = false;

Applications/foc/core/trq2dq_table.c

@@ -72,11 +72,18 @@ void trq2dq_lookup_init(void) {
 
 void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
 	if (table_map == NULL) {
-		step_towards(&dq_out->d, 0, 1.0f);
+		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;
+		return;
+	}
 
+	bool neg_torque = torque < 0.0f?1:0;
+	torque = ABS(torque);
 	int low = 0, high = 0;
 
 	low = rpm / RPM_INTVAL - 1;
@@ -113,6 +120,11 @@ void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
 		frac_x = (float)(rpm - x1)/(x2 - x1);
 	}
 	intp_line2(frac_x, torque, maps, &d, &q);
-	step_towards(&dq_out->d, d, 1.0f);
-	dq_out->q = 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;
+	}
 }

Applications/foc/motor/current_ics.c

@@ -36,6 +36,7 @@ void phase_current_calibrate_wait(void) {
 	while(g_cs.is_calibrating_offset || g_cs.is_calibrating_sensor) {
 		wdog_reload();
 	}
+	sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
 }
 
 void phase_current_sensor_start_calibrate(float calibrate_current) {
@@ -114,7 +115,6 @@ bool phase_current_offset(void) {
 			adc_current_sample_config(cs->c_phases);
 		}else {
 			cs->is_calibrating_offset = false;
-			sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
 		}
 
 	}