优化参数识别逻辑

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

Applications/foc/commands.c

@@ -651,6 +651,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_MotPara_Ind:
 		{
+			_pc_connect = true;
+			set_log_level(MOD_SYSTEM, L_debug);
 			bool start = decode_u8((u8 *)command->data)?true:false;
 			if (!start) {
 				mc_ind_motor_start(start);
@@ -665,7 +667,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 					u8 time = decode_u8((u8 *)command->data + 4);
 					sys_debug("rs ind %d, %d, %d\n", v_max, i_max, time);
 					if (mc_ind_motor_start(true)) {
-						mot_params_ind_rs((float)v_max, (float)i_max, (s32)time * 10);
+						mot_params_ind_rs((float)v_max, (float)i_max, (s32)time);
 					}
 				}else if (type == L_TYPE_D || type == L_TYPE_Q) { //ld/lq ind
 					u8 v = decode_u8((u8 *)command->data + 2);

Applications/foc/motor/mot_params_ind.c

@@ -46,9 +46,9 @@ void mot_params_ind_stop(void) {
 	b_rs_ind = false;
 	b_ldq_ind = false;
 	b_flux_ind = false;
+	cpu_exit_critical(mask);
 	PMSM_FOC_SetOpenVdq(0, 0);
 	PMSM_FOC_Set_Current(0);
-	cpu_exit_critical(mask);
 }
 
 static void _rs_ind_timer_handler(shark_timer_t *t) {
@@ -73,21 +73,22 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 			PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
 		}else {
 			rs_ind_step = 2;
+			sys_debug("id reach the set\n");
 		}
 		break;
 	case 2:
-		if (PMSM_FOC_Get()->out.s_RealIdq.d < rs_id_max) {
-			rs_vd_now += 0.1f;
-			wait_iq_0_cnt = 0;
-			if (rs_vd_now >= rs_vd_max) {
+		if (ABS(PMSM_FOC_Get()->out.s_FilterIdq.q) > 5.0f) {
+			wait_iq_0_cnt++;
+			if (wait_iq_0_cnt >= 200) {
 				PMSM_FOC_SetOpenVdq(0, 0);
 				b_rs_ind = false;
-				sys_debug("id not reach max id %f\n", PMSM_FOC_Get()->out.s_RealIdq.d);
+				sys_debug("iq is larger %f\n", PMSM_FOC_Get()->out.s_FilterIdq.q);
 				return;
 			}
-			PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
 		}else {
+			wait_iq_0_cnt = 0;
 			rs_ind_step = 3;
+			sys_debug("start rs calc, %d\n", rs_meas_time);
 		}
 		break;
 	case 3: {
@@ -96,7 +97,7 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 		PMSM_FOC_ABC2Dq(SIGN(iabc[0]), SIGN(iabc[1]), SIGN(iabc[2]), &d, &q);
 		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * PMSM_FOC_Get()->in.s_vDC * 1.5f;
 		float vd = (rs_vd_now - dtc) * TWO_BY_THREE;
-		float rs = vd / (PMSM_FOC_Get()->out.s_RealIdq.d + 0.00000000001f);
+		float rs = vd / (PMSM_FOC_Get()->out.s_RealIdq.d + 0.0001f);
 		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
 		if (rs_meas_time-- <= 0) {
 			mc_ind_motor_start(false);
@@ -275,9 +276,11 @@ void mot_params_calc_inductance(void) {
 	b_ldq_ind = false;
 }
 static float motVelRadusPers, flux_wait_cnt = 0, flux_do_cnt = 0, flux_est_value = 0;
+static bool _pending_flux_mc_stop = false;
 static void _flux_ind_timer_handler(shark_timer_t *t) {
 	float We = PMSM_FOC_Get()->in.s_motVelRadusPers;
 	float delta = We - motVelRadusPers;
+	motVelRadusPers = PMSM_FOC_Get()->in.s_motVelRadusPers;
 	if (We > 100 && ABS(delta) < 40) {
 		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * PMSM_FOC_Get()->in.s_vDC * 1.5f;
 		float vq = (PMSM_FOC_Get()->out.s_OutVdq.q - dtc) * TWO_BY_THREE;
@@ -287,27 +290,33 @@ static void _flux_ind_timer_handler(shark_timer_t *t) {
 	}else {
 		flux_wait_cnt ++;
 	}
-	if ((flux_wait_cnt >= 100) || (flux_do_cnt >= 400)) {
+	if ((flux_wait_cnt >= 500) || (flux_do_cnt >= 400)) {
 		b_flux_ind = false;
-		if (flux_wait_cnt >= 100) {
+		if (flux_wait_cnt >= 500) {
 			sys_debug("ind flux error\n");
 		}else {
 			b_flux_ested = true;
 			sys_debug("ind_flux finish, %f\n", flux_est_value);
 		}
+		mot_params_ind_stop();
+		if (PMSM_FOC_GetSpeed() < CONFIG_ZERO_SPEED_RPM) {
+			mc_ind_motor_start(false);
+		}else {
+			_pending_flux_mc_stop = true;
+		}
 	}else {
 		shark_timer_post(&_flux_ind_timer, 10);
 	}
 }
 
 void mot_params_ind_flux(float id, float iq) {
-	if (b_rs_ind || b_ldq_ind || b_flux_ind) {
+	if (b_rs_ind || b_ldq_ind || b_flux_ind || _pending_flux_mc_stop) {
 		return;
 	}
 	b_flux_ind = true;
 	flux_wait_cnt = 0;
 	flux_do_cnt = 0;
-	flux_est_value = 0;
+	b_flux_ested = false;
 	mc_set_foc_mode(CTRL_MODE_CURRENT);
 	PMSM_FOC_Set_Current(iq);
 	shark_timer_post(&_flux_ind_timer, 10);
@@ -336,4 +345,13 @@ float mot_params_get_est_flux(void) {
 	return flux_est_value;
 }
 
+void mot_params_flux_stop(void) {
+	if (_pending_flux_mc_stop && (PMSM_FOC_GetSpeed() < CONFIG_ZERO_SPEED_RPM)) {
+		mc_ind_motor_start(false);
+		_pending_flux_mc_stop = false;
+	}
+}
 
+bool mot_params_flux_pending(void) {
+	return _pending_flux_mc_stop;
+}

Applications/foc/motor/mot_params_ind.h

@@ -25,5 +25,8 @@ bool mot_params_ld_ested(void);
 bool mot_params_lq_ested(void);
 bool mot_params_flux_ested(void);
 float mot_params_get_est_flux(void);
+void mot_params_flux_stop(void);
+bool mot_params_flux_pending(void);
+
 #endif /* MOT_PARAMS_IND_H__ */
 

Applications/foc/motor/motor.c

@@ -476,7 +476,7 @@ bool mc_set_foc_mode(u8 mode) {
 	if (mode == motor.mode) {
 		return true;
 	}
-	if (!motor.b_start) {
+	if (!motor.b_start && !motor.b_ind_start) {
 		return false;
 	}
 	if (mc_critical_can_not_run()) {
@@ -712,7 +712,7 @@ bool mc_ind_motor_start(bool start) {
 	if (start) {
 		motor.b_ignor_throttle = true;
 		MC_Check_MosVbusThrottle();
-		if (mc_unsafe_critical_error()) {
+		if (mc_unsafe_critical_error() || mot_params_flux_pending()) {
 			PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
 			return false;
 		}
@@ -737,9 +737,9 @@ bool mc_ind_motor_start(bool start) {
 		adc_stop_convert();
 		pwm_stop();
 		PMSM_FOC_Stop();
+		motor.mode = CTRL_MODE_OPEN;
 		pwm_up_enable(true);
 		motor.b_ignor_throttle = false;
-		
 	}
 	
 	return true;
@@ -1439,6 +1439,9 @@ void Sched_MC_mTask(void) {
 		eCtrl_Running();
 		PMSM_FOC_Slow_Task();
 		mc_motor_runstop();
+		if (motor.b_ind_start) {
+			mot_params_flux_stop();
+		}
 		mc_TaskEnd;
 		return;
 	}