加入磁链识别

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

Applications/app/app.c

@@ -143,12 +143,15 @@ static u32 _app_report_task(void *p) {
 	if (mot_params_lq_ested()) {
 		can_report_motparam(mot_params_get_est_lq(), L_TYPE_Q);
 	}
+	if (mot_params_flux_ested()) {
+		can_report_motparam(mot_params_get_est_flux(), FLUX_TYPE);
+	}
 
 	if (++loop % 10 == 0) {
 		//sys_debug("rst 0x%x\n", get_mcu_reset_source());
-		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, bid %d\n", get_acc_vol(), gpio_board_id());
 		//sys_debug("throttle %f\n", get_throttle_float());
 		//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
@@ -158,11 +161,11 @@ static u32 _app_report_task(void *p) {
 		encoder_log();
 		//motor_debug();
 		//sample_log();
-		PMSM_FOC_LogDebug();
+		//PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
-		sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
-		mc_err_code_log();
+		//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
+		//mc_err_code_log();
 		sys_debug("=====\n");
 	}
 	return 200;

Applications/bsp/gd32/board_mc105_v3.h

@@ -49,11 +49,13 @@
 #define SCHED_TIMER_IRQHandler TIMER5_IRQHandler
 
 #define PWM_DEAD_TIME_NS 400u
+#define PWM_TOFF_DELAY_MAX 240u
+#define PWM_TON_DELAY_MIN 200u
 #define HW_DEAD_TIME_NS  210u
 #define HW_RISE_TIME_NS  150u
 #define HW_NOISE_TIME_NS 300u
 
-#define CONFIG_HW_DeadTime NS_2_TCLK(HW_DEAD_TIME_NS + PWM_DEAD_TIME_NS)/* ����ʱ�� */
+#define CONFIG_HW_DeadTime NS_2_TCLK(HW_DEAD_TIME_NS + PWM_DEAD_TIME_NS + (PWM_TOFF_DELAY_MAX - PWM_TON_DELAY_MIN))/* ����ʱ�� */
 #define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS ����ʱ��*/
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS��������Ŀ�������ʱ�� */
 #define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) *2 * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */

Applications/foc/commands.c

@@ -669,7 +669,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 					}
 				}else if (type == L_TYPE_D || type == L_TYPE_Q) { //ld/lq ind
 					u8 v = decode_u8((u8 *)command->data + 2);
-					u16 freq = decode_u8((u8 *)command->data + 3);
+					u16 freq = decode_u16((u8 *)command->data + 3);
+					sys_debug("ldq ind %d, %d\n", v, freq);
 					if (mc_ind_motor_start(true)) {
 						if (type == L_TYPE_D) {
 							mot_params_ind_ld((float)v, (float)freq);
@@ -677,6 +678,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 							mot_params_ind_lq((float)v, (float)freq);
 						}
 					}
+				}else if (type == FLUX_TYPE) {
+					u8 iq = decode_u8((u8 *)command->data + 2);
+					sys_debug("ind flux iq = %d\n", iq);
+					if (mc_ind_motor_start(true)) {
+						mot_params_ind_flux(0, (float)iq);
+					}
 				}else{ 
 					erroCode = FOC_Param_Err;
 				}

Applications/foc/core/PMSM_FOC_Core.c

@@ -364,14 +364,14 @@ static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
 	static float a_max = 0, b_max = 0, c_max = 0;
 	static u32 _unbalance_cnt = 0;
 	static u32 _unbalance_time = 0;
-	float lowpass = gFoc_Ctrl.in.s_motVelDegreePers * FOC_CTRL_US / 2.0f;
+	float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US / 2.0f;
 	if (lowpass > 1.0f) {
 		lowpass = 1.0f;
 	}
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], lowpass);
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], lowpass);
 	gFoc_Ctrl.in.s_iABCFilter[2] = -(gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
-	if ((gFoc_Ctrl.in.s_angleLast == INVALID_ANGLE) || (gFoc_Ctrl.in.s_motVelDegreePers < 100)) {
+	if ((gFoc_Ctrl.in.s_angleLast == INVALID_ANGLE) || (gFoc_Ctrl.in.s_motVelRadusPers < 100)) {
 		gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
 		a_max = b_max = c_max = 0;
 		_unbalance_cnt = 0;
@@ -530,7 +530,7 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	}
 	gFoc_Ctrl.in.s_motVelocity = enc_vel;
 	LowPass_Filter(gFoc_Ctrl.in.s_motVelocityFiltered, gFoc_Ctrl.in.s_motVelocity, 0.01f);
-	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
+	gFoc_Ctrl.in.s_motVelRadusPers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 
 	PMSM_FOC_Phase_Unbalance();
 
@@ -543,11 +543,13 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	get_phase_vols(gFoc_Ctrl.in.s_vABC);
 
 	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
-
 	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
-	float lowpass = gFoc_Ctrl.in.s_motVelDegreePers * FOC_CTRL_US * 2.0f;
+
+	float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US * 2.0f;
 	if (lowpass > 1.0f) {
 		lowpass = 1.0f;
+	}else if (lowpass <= 0.0001f) {
+		lowpass = 0.001f;
 	}
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, lowpass);
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, lowpass);
@@ -569,7 +571,7 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 #endif
 #ifdef CONFIG_Volvec_Delay_Comp
 	if (gFoc_Ctrl.in.s_motVelocityFiltered >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
-		float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
+		float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelRadusPers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
 		rand_angle(next_angle);
 		SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 	}
@@ -641,7 +643,7 @@ bool PMSM_FOC_Schedule(void) {
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
 	#endif
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
-		float id_ff = id_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
+		float id_ff = id_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_Current(&gFoc_Ctrl.pi_id, err, id_ff);
 #ifdef UPDATE_Lq_By_iq
 		/* update kp&ki from lq for iq PI controller */
@@ -658,7 +660,7 @@ bool PMSM_FOC_Schedule(void) {
 		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
-		float iq_ff = iq_feedforward(gFoc_Ctrl.in.s_motVelDegreePers);
+		float iq_ff = iq_feedforward(gFoc_Ctrl.in.s_motVelRadusPers);
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_Current(&gFoc_Ctrl.pi_iq, err, iq_ff);
 	}else {
 		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;

Applications/foc/core/PMSM_FOC_Core.h

@@ -107,7 +107,7 @@ typedef struct {
 	bool    b_epmMode;
 	bool    b_fwEnable;
 	float   s_motVelocityFiltered; //电机滤波后的转速
-	float   s_motVelDegreePers; //电机的电角速度
+	float   s_motVelRadusPers; //电机的电角速度
 	volatile bool    b_MTPA_calibrate;
 	float   s_manualAngle; //mainly used when calibrate hall/mtpa.
 	float   s_dqAngle; //D轴电流超前角

Applications/foc/motor/mot_params_ind.c

@@ -7,18 +7,22 @@
 
 /*
 参考 MC_Simulink\modules\off_line_params_ind 仿真模型
+必须空载测试
 */
 
 static void _rs_ind_timer_handler(shark_timer_t *);
 static shark_timer_t _rs_ind_timer = TIMER_INIT(_rs_ind_timer, _rs_ind_timer_handler);
 static void _ldq_ind_timer_handler(shark_timer_t *);
 static shark_timer_t _ldq_ind_timer = TIMER_INIT(_ldq_ind_timer, _ldq_ind_timer_handler);
+static void _flux_ind_timer_handler(shark_timer_t *);
+static shark_timer_t _flux_ind_timer = TIMER_INIT(_flux_ind_timer, _flux_ind_timer_handler);
 
 static float rs_id_max, rs_vd_max, rs_vd_now, rs_est_value;
 static s32 rs_meas_time;
-static bool b_rs_ind = false, b_rs_ested = false, b_ldq_ind = false, b_ld_ested = false, b_lq_ested = false;;
+static bool b_rs_ind = false, b_rs_ested = false, b_ldq_ind = false, b_ld_ested = false, b_lq_ested = false, b_flux_ind = false, b_flux_ested = false;
+static u8   rs_ind_step = 0;
 void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
-	if (b_rs_ind || b_ldq_ind) {
+	if (b_rs_ind || b_ldq_ind || b_flux_ind) {
 		return;
 	}
 	b_rs_ind = true;
@@ -27,18 +31,23 @@ void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
 	rs_vd_max = vd_max;
 	rs_vd_now = 2.0f;
 	rs_meas_time = time;
+	PMSM_FOC_Set_MotAngle(0);
 	PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
 
+	rs_ind_step = 1;
 	shark_timer_post(&_rs_ind_timer, 10);
 }
 
 void mot_params_ind_stop(void) {
 	shark_timer_cancel(&_rs_ind_timer);
 	shark_timer_cancel(&_ldq_ind_timer);
+	shark_timer_cancel(&_flux_ind_timer);
 	u32 mask = cpu_enter_critical();
 	b_rs_ind = false;
 	b_ldq_ind = false;
+	b_flux_ind = false;
 	PMSM_FOC_SetOpenVdq(0, 0);
+	PMSM_FOC_Set_Current(0);
 	cpu_exit_critical(mask);
 }
 
@@ -49,43 +58,59 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 		PMSM_FOC_SetOpenVdq(0, 0);
 		return;
 	}
-	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) {
-			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);
-			return;
-		}
-		PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
-	}else {
-		if (ABS(PMSM_FOC_Get()->out.s_FilterIdq.q) > 1.0f) {
-			wait_iq_0_cnt++;
-			if (wait_iq_0_cnt >= 200) {
+
+	switch (rs_ind_step) {
+	case 1:
+		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) {
 				PMSM_FOC_SetOpenVdq(0, 0);
 				b_rs_ind = false;
-				sys_debug("iq is bigger chan 1, %f\n", PMSM_FOC_Get()->out.s_FilterIdq.q);
+				sys_debug("id not reach max id %f\n", PMSM_FOC_Get()->out.s_RealIdq.d);
 				return;
 			}
+			PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
 		}else {
-			float *iabc = PMSM_FOC_Get()->in.s_iABC;
-			float d, q;
-			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);
-			rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
-			if (rs_meas_time-- <= 0) {
-				mc_ind_motor_start(false);
-				mot_params_ind_stop();
-				finish = true;
-				b_rs_ested = true;
-				sys_debug("est rs = %f\n", rs_est_value);
-				sys_debug("vd is %f\n", rs_vd_now);
+			rs_ind_step = 2;
+		}
+		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) {
+				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);
+				return;
 			}
+			PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
+		}else {
+			rs_ind_step = 3;
+		}
+		break;
+	case 3: {
+		float *iabc = PMSM_FOC_Get()->in.s_iABC;
+		float d, q;
+		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);
+		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
+		if (rs_meas_time-- <= 0) {
+			mc_ind_motor_start(false);
+			mot_params_ind_stop();
+			finish = true;
+			b_rs_ested = true;
+			sys_debug("est rs = %f\n", rs_est_value);
+			sys_debug("vd is %f, wait %d\n", rs_vd_now, wait_iq_0_cnt);
 		}
 	}
+	default:
+		break;
+	}
+		
 	if (!finish) {
 		shark_timer_post(&_rs_ind_timer, 10);
 	}
@@ -106,18 +131,20 @@ static u16   n_ind_ld, n_samples;
 static float ld_est_value, lq_est_value;
 static s32   ldq_est_wait_cnt = 0;
 void mot_params_ind_inductance(float v, float freq, u16 l_type) {
-	if (b_ldq_ind || b_rs_ind) {
+	if (b_ldq_ind || b_rs_ind || b_flux_ind) {
+		return;
+	}
+	if (!b_rs_ested) { //必须先识别相电阻
 		return;
 	}
-	
 	hj_v = v;
 	hj_freq = freq;
 	hj_n = (float)FOC_PWM_FS / hj_freq;
-	hj_samples = hj_n * 20;
+	hj_samples = hj_n * 50;
 	K_terms = (s32) (0.5f + hj_samples*hj_freq/(float)FOC_PWM_FS);
 	Vdead = PMSM_FOC_Get()->in.s_vDC * 0.5f * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
 	hj_w = 360.0f / hj_n;
-
+	sys_debug("hj %f, %f, %f, %f, %f, %f, %f\n", hj_v, hj_freq, hj_n, hj_samples, K_terms, Vdead, hj_w);
 	float fft_angle = 360.0f / hj_samples * K_terms;
 	SinCos_Lut(fft_angle, &hj_image, &hj_real);
 	hj_real = hj_real * 2.0f;
@@ -134,7 +161,9 @@ void mot_params_ind_inductance(float v, float freq, u16 l_type) {
 	i_samples = os_alloc(sizeof(float) * hj_samples);
 	if (v_samples != NULL && i_samples != NULL) {
 		b_ldq_ind = true;
-		shark_timer_post(&_ldq_ind_timer, 50);
+		shark_timer_post(&_ldq_ind_timer, 10);
+	}else {
+		sys_debug("alloc error\n");
 	}
 }
 
@@ -150,7 +179,7 @@ static void _ldq_ind_timer_handler(shark_timer_t *t) {
 			mot_params_ind_stop();
 			sys_debug("ldq ind timeout %d\n", ldq_est_wait_cnt);
 		}else {
-			shark_timer_post(&_ldq_ind_timer, 50);
+			shark_timer_post(&_ldq_ind_timer, 10);
 		}
 	}
 }
@@ -175,7 +204,7 @@ void mot_params_high_freq_inject(void) {
 	float s, c;
 	SinCos_Lut(hj_angle, &s, &c);
 	float vd = 0, vq = 0;
-	if (n_ind_ld == 1) {
+	if (n_ind_ld == L_TYPE_D) {
 		vd = hj_v * c;
 	}else {
 		vq = hj_v * c;
@@ -187,12 +216,12 @@ bool mot_params_hj_sample_vi(float vd, float vq, float id, float iq) {
 	if (!b_ldq_ind) {
 		return true;
 	}
-	if (n_samples >= 1 && (n_samples) <= (hj_samples + 1)) {
+	if ((n_samples >= 1) && (n_samples <= hj_samples)) {
 		if (n_ind_ld == L_TYPE_D) {
-			v_samples[n_samples - 1] = vd;
+			v_samples[n_samples - 1] = vd * TWO_BY_THREE;
 			i_samples[n_samples - 1] = id;
 		}else {
-			v_samples[n_samples - 1] = vq;
+			v_samples[n_samples - 1] = vq * TWO_BY_THREE;
 			i_samples[n_samples - 1] = iq;
 		}
 	}
@@ -220,18 +249,20 @@ void mot_params_calc_inductance(void) {
 	}
 	goertzel_dft(v_samples, &v_real, &v_image, &v_mag);
 	goertzel_dft(i_samples, &i_real, &i_image, &i_mag);
-
+	sys_debug("v %f, %f, %f\n", v_mag/(hj_samples*0.5f), v_real, v_image);
+	sys_debug("i %f, %f, %f\n", i_mag/(hj_samples*0.5f), i_real, i_image);
 	v_mag -= Vdead * hj_samples*0.5f;
 	
-	float z_angle = fast_atan_2(i_real, i_image) - fast_atan_2(v_real, v_image);
+	float z_angle = fast_atan_2(i_image, i_real) - fast_atan_2(v_image, v_real);
 	float s,c;
-	SinCos_Lut(z_angle, &s, &c);
+	SinCos_Lut(pi_2_degree(z_angle), &s, &c);
 
 	float z_mag = v_mag / (i_mag + 0.0000001f);
 	float z_real = z_mag * c;
 	float z_image = z_mag * s;
-	float Ri = SQ(z_real - b_rs_ested) + SQ(z_image)/(z_real - b_rs_ested + 0.0000001f);
-	float l = Ri * (z_real - rs_est_value)/(hj_freq * 2 * PI * z_image + 0.0000001f);
+	float Rs = rs_est_value;
+	float Ri = (SQ(z_real - Rs) + SQ(z_image))/(z_real - Rs + 0.0000001f);
+	float l = Ri * (z_real - Rs)/(hj_freq * 2 * PI * z_image + 0.0000001f) * 0.83f; //0.83f just for v3 board
 	if (n_ind_ld == L_TYPE_D) {
 		ld_est_value = l;
 		b_ld_ested = true;
@@ -243,6 +274,45 @@ 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 void _flux_ind_timer_handler(shark_timer_t *t) {
+	float We = PMSM_FOC_Get()->in.s_motVelRadusPers;
+	float delta = We - 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;
+		float flux = vq / We;
+		flux_est_value = LowPass_Filter(flux_est_value, flux, 0.1f);
+		flux_do_cnt ++;
+	}else {
+		flux_wait_cnt ++;
+	}
+	if ((flux_wait_cnt >= 100) || (flux_do_cnt >= 400)) {
+		b_flux_ind = false;
+		if (flux_wait_cnt >= 100) {
+			sys_debug("ind flux error\n");
+		}else {
+			b_flux_ested = true;
+			sys_debug("ind_flux finish, %f\n", flux_est_value);
+		}
+	}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) {
+		return;
+	}
+	b_flux_ind = true;
+	flux_wait_cnt = 0;
+	flux_do_cnt = 0;
+	flux_est_value = 0;
+	mc_set_foc_mode(CTRL_MODE_CURRENT);
+	PMSM_FOC_Set_Current(iq);
+	shark_timer_post(&_flux_ind_timer, 10);
+	motVelRadusPers = PMSM_FOC_Get()->in.s_motVelRadusPers;
+}
 
 float mot_params_get_est_ld(void) {
 	return ld_est_value;
@@ -259,3 +329,11 @@ bool mot_params_lq_ested(void) {
 	return b_lq_ested;
 }
 
+bool mot_params_flux_ested(void) {
+	return b_flux_ested;
+}
+float mot_params_get_est_flux(void) {
+	return flux_est_value;
+}
+
+

Applications/foc/motor/mot_params_ind.h

@@ -7,10 +7,12 @@
 #define R_TYPE   1
 #define L_TYPE_D 2
 #define L_TYPE_Q 3
+#define FLUX_TYPE 4
 
 void mot_params_ind_rs(float vd_max, float id_max, s32 time);
 void mot_params_ind_ld(float v, float freq);
 void mot_params_ind_lq(float v, float freq);
+void mot_params_ind_flux(float id, float iq);
 void mot_params_high_freq_inject(void);
 float mot_params_get_est_rs(void);
 bool mot_params_hj_sample_vi(float vd, float vq, float id, float iq);
@@ -21,5 +23,7 @@ void mot_params_calc_inductance(void);
 bool mot_params_rs_ested(void);
 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);
 #endif /* MOT_PARAMS_IND_H__ */
 

Applications/foc/motor/motor.c

@@ -725,7 +725,6 @@ bool mc_ind_motor_start(bool start) {
 		adc_start_convert();
 		pwm_enable_channel();
 		phase_current_calibrate_wait();
-		PMSM_FOC_Set_MotAngle(0);
 		PMSM_FOC_SetOpenVdq_Immediate(0, 0);
 		motor.b_ind_start = start;
 	}else {

Applications/prot/can_foc_msg.c

@@ -143,7 +143,7 @@ void can_report_ext_status(u8 can) {
 void can_report_motparam(float v, u8 type) {
 	u8 data[8];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_MotPara_Report));
-	encode_u8(data + 1, type);
+	encode_u8(data + 2, type);
 	encode_float(data + 3, v);
 	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
 }