1. 模拟转把处理双转把错误问题

2. 无感挡位需要小于1档的扭矩
3. 使用采集的相电压计算母线电流

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

Applications/app/factory.c

@@ -67,7 +67,7 @@ void can_process_factory_message(can_message_t *can_message){
 				rsplen += 10;
 			}else if (item == 4) { // u phase detect
 				int count = 20;
-				s16 uvw[3] = {0, 0, 0};
+				float uvw[3] = {0, 0, 0};
 				s16 uvw_total[3] = {0, 0, 0};
 				if (can_message->len < 2) {
 					response[2] = 2; //长度错误
@@ -77,10 +77,10 @@ void can_process_factory_message(can_message_t *can_message){
 				gpio_phase_u_detect(detect?true:false);
 				while(count-- > 0) {
 					delay_us(100);
-					sample_uvw_phases_raw(uvw);
-					uvw_total[0] += uvw[0];
-					uvw_total[1] += uvw[1];
-					uvw_total[2] += uvw[2];
+					get_uvw_phases_raw(uvw);
+					uvw_total[0] += (s16)(uvw[0] * 100);
+					uvw_total[1] += (s16)(uvw[1] * 100);
+					uvw_total[2] += (s16)(uvw[2] * 100);
 				}
 				encode_u16(response + 3, uvw_total[0]/20);
 				encode_u16(response + 5, uvw_total[1]/20);

Applications/foc/core/PMSM_FOC_Core.c

@@ -489,6 +489,20 @@ static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
 #endif
 }
 
+static __INLINE void Phase_Voltage_update(float lowpass) {
+	float v_ABC[3];
+	get_uvw_phases_raw(v_ABC);
+	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[0], v_ABC[0], lowpass);
+	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[1], v_ABC[1], lowpass);
+	LowPass_Filter(gFoc_Ctrl.in.s_SamplePhaseV[2], v_ABC[2], lowpass);
+	/* phase voltage = phase-phase voltage / sqrt(3), 1.4是滤波器幅值补偿系数 */
+	float phase_vAN = (gFoc_Ctrl.in.s_SamplePhaseV[0] - gFoc_Ctrl.in.s_SamplePhaseV[1]) * ONE_BY_SQRT3 * 1.4f;
+	float phase_vBN = (gFoc_Ctrl.in.s_SamplePhaseV[1] - gFoc_Ctrl.in.s_SamplePhaseV[2]) * ONE_BY_SQRT3 * 1.4f;
+	float phase_vCN = (gFoc_Ctrl.in.s_SamplePhaseV[2] - gFoc_Ctrl.in.s_SamplePhaseV[0]) * ONE_BY_SQRT3 * 1.4f;
+	Clark(phase_vAN, phase_vBN, phase_vCN, &gFoc_Ctrl.out.s_SampleAB);
+	Park(&gFoc_Ctrl.out.s_SampleAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_SamplevDQ);
+}
+
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
@@ -540,23 +554,24 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 
 	gFoc_Ctrl.in.s_vDC = get_vbus_float();
 	
-	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_motVelRadusPers * FOC_CTRL_US * 2.0f;
-	if (lowpass > 1.0f) {
-		lowpass = 1.0f;
-	}else if (lowpass <= 0.0001f) {
-		lowpass = 0.001f;
+	float lowpass = gFoc_Ctrl.in.s_motVelRadusPers * FOC_CTRL_US;
+	float iqLowPass = lowpass * 2.0f;
+	if (iqLowPass > 1.0f) {
+		iqLowPass = 1.0f;
+	}else if (iqLowPass <= 0.0001f) {
+		iqLowPass = 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);
-
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, iqLowPass);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, iqLowPass);
 	/* 使用低通后的dq电流重新变换得到abc电流,给死区补偿使用 */
 	RevPark(&gFoc_Ctrl.out.s_FilterIdq, gFoc_Ctrl.in.s_motAngle, &iAB);
 	RevClark(&iAB, gFoc_Ctrl.in.s_iABC_DT);
+	
+	Phase_Voltage_update(lowpass);
+
 #ifdef CONFIG_START_LINE_DTC_CURRENT
 	gFoc_Ctrl.out.s_OutVdqDTC.d = 0;
 	gFoc_Ctrl.out.s_OutVdqDTC.q = 0;
@@ -696,7 +711,7 @@ bool PMSM_FOC_Schedule(void) {
 }
 
 void PMSM_FOC_LogDebug(void) {
-	sys_debug("DC curr %f --- %f, - %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_FilteriDC, gFoc_Ctrl.userLim.s_iDCLim);
+	sys_debug("DC curr %f --- %f,  %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_FilteriDC, gFoc_Ctrl.out.s_CalciDC2);
 	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
 	sys_debug("unbalance: %d, %f, %f, %f, %f\n", phase_unbalance_cnt, phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
 	if (obser_vel != 111111) {
@@ -1435,22 +1450,28 @@ void PMSM_FOC_Set_PlotType(Plot_t t) {
 }
 //获取母线电流和实际输出电流矢量大小
 void PMSM_FOC_Calc_Current(void) {
-	float vd = gFoc_Ctrl.out.s_OutVdq.d + gFoc_Ctrl.out.s_OutVdqDTC.d * 0.667f;
-	float vq = gFoc_Ctrl.out.s_OutVdq.q + gFoc_Ctrl.out.s_OutVdqDTC.q * 0.667f;
+	float vd = gFoc_Ctrl.out.s_OutVdq.d - gFoc_Ctrl.out.s_OutVdqDTC.d * TWO_BY_THREE;
+	float vq = gFoc_Ctrl.out.s_OutVdq.q - gFoc_Ctrl.out.s_OutVdqDTC.q * TWO_BY_THREE;
 
 	float id = gFoc_Ctrl.out.s_FilterIdq.d;
 	float iq = gFoc_Ctrl.out.s_FilterIdq.q;
     /*
 		根据公式（等幅值变换，功率不等）:
-		iDC x vDC = 2/3(iq x vq + id x vd);
+		iDC x vDC = 3/2(iq x vq + id x vd);
 	*/
-	float m_pow = (vd * id + vq * iq); //s32q10
+	float m_pow = (vd * id + vq * iq);
 	float raw_idc = 0.0f;
 	float v_dc = get_vbus_float();
 	if (v_dc != 0.0f) {
-		raw_idc = m_pow / v_dc;// * 1.5f * 0.66f; //s16q5
+		raw_idc = m_pow / v_dc;
 	}
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.02f);
+
+	m_pow = (gFoc_Ctrl.out.s_SamplevDQ.d * id + gFoc_Ctrl.out.s_SamplevDQ.q * iq) * 1.5f;
+	if (v_dc != 0.0f) {
+		raw_idc = m_pow / v_dc;
+	}
+	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, raw_idc, 0.02f);
 #ifdef VBUS_I_CHAN
 	raw_idc = get_vbus_current();
 	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);

Applications/foc/core/PMSM_FOC_Core.h

@@ -88,7 +88,7 @@ typedef struct {
 	float 	s_iABC[3];
 	float 	s_iABCFilter[3];
 	float   s_iABC_DT[3]; //abc phase current for deadtime compesition
-	float   s_vABC[3];
+	float   s_SamplePhaseV[3]; //相对地电压
 	float   s_motVelocity;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
 	float   s_angleLast;
@@ -162,6 +162,8 @@ typedef struct {
 	DQ_t  s_RealIdq;
 	DQ_t  s_RealVdq;
 	DQ_t  s_FilterIdq;
+	AB_t  s_SampleAB;
+	DQ_t  s_SamplevDQ;
 	float s_FilteriDC;
 	float s_CalciDC;
 	float s_CalciDC2;

Applications/foc/motor/motor.c

@@ -59,7 +59,7 @@ static mc_gear_t sensorless_gear = {
 	.n_max_idc = CONFIG_SENSORLESS_MAX_IDC,
 	.n_zero_accl = 1500,
 	.n_accl_time = 1500,
-	.n_torque = {100, 100, 100, 100, 0, 0, 0, 0, 0, 0},
+	.n_torque = {100, 80, 60, 0, 0, 0, 0, 0, 0, 0},
 };
 static runtime_node_t mc_error;
 
@@ -93,7 +93,7 @@ static void MC_Check_MosVbusThrottle(void) {
 
 	gpio_phase_u_detect(false);
 	float abc[3];
-	get_phase_vols(abc);
+	get_phase_vols_filtered(abc);
 	int vbus_vol = get_vbus_int();
 	if (vbus_vol > nv_get_foc_params()->s_maxDCVol) {
 		mc_set_critical_error(FOC_CRIT_OV_Vol_Err);
@@ -275,7 +275,10 @@ bool mc_critical_need_stop(void) {
 bool mc_critical_can_not_run(void) {
 	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err);
 	u32 err = motor.n_CritiCalErrMask & mask;
-	bool crit_err = (err != 0) || mc_critical_need_stop() || throttle_is_all_error();
+	bool crit_err = (err != 0) || mc_critical_need_stop();
+	if (!motor.b_ignor_throttle) {
+		crit_err = crit_err || throttle_is_all_error();
+	}
 	return crit_err;
 }
 
@@ -298,6 +301,7 @@ bool mc_unsafe_critical_error(void) {
 #endif
 	if (motor.b_ignor_throttle) {
 		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
+		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
 	}
 	return (err != 0);
 }
@@ -665,6 +669,7 @@ void mc_set_throttle_r(bool use, u8 r) {
 	motor.b_ignor_throttle = use;
 	if (motor.b_ignor_throttle) {
 		mc_clr_critical_error(FOC_CRIT_THRO_Err);
+		mc_clr_critical_error(FOC_CRIT_THRO2_Err);
 	}
 }
 

Applications/foc/samples.c

@@ -90,7 +90,7 @@ void samples_init(void){
 }
 
 
-void get_phase_vols(float *uvw) {
+void get_phase_vols_filtered(float *uvw) {
 	uvw[0] = _uvw_phase[0].filted_value;
 	uvw[1] = _uvw_phase[1].filted_value;
 	uvw[2] = _uvw_phase[2].filted_value;
@@ -171,7 +171,7 @@ static u32 sample_task(void *param) {
 	sample_vbus();
 	sample_ibus();
 	sample_throttle();
-	sample_uvw_phase();
+	//sample_uvw_phase();
 	sample_motor_temp();
 	sample_mos_temp();
 	sample_vref();
@@ -242,26 +242,26 @@ void sample_throttle(void){
 
 }
 
-void sample_uvw_phases_raw(s16 *uvw_raw) {
+void get_uvw_phases_raw(float *uvw_raw) {
 	u16 uvw[3];
 	adc_get_uvw_phaseV(uvw);
-	uvw_raw[0] = (s16)((float)uvw[0] * UVW_VOL_CEOF * 100.0f);
-	uvw_raw[1] = (s16)((float)uvw[1] * UVW_VOL_CEOF * 100.0f);
-	uvw_raw[2] = (s16)((float)uvw[2] * UVW_VOL_CEOF * 100.0f);
+	uvw_raw[0] = ((float)uvw[0] * UVW_VOL_CEOF);
+	uvw_raw[1] = ((float)uvw[1] * UVW_VOL_CEOF);
+	uvw_raw[2] = ((float)uvw[2] * UVW_VOL_CEOF);
 }
 
 void sample_uvw_phase(void) {
 #ifdef U_VOL_ADC_CHAN
-	u16 uvw[3];
-	adc_get_uvw_phaseV(uvw);
+	float uvw[3];
+	get_uvw_phases_raw(uvw);
 
-	_uvw_phase[0].value = (float)uvw[0] * UVW_VOL_CEOF;
+	_uvw_phase[0].value = uvw[0];
 	LowPass_Filter(_uvw_phase[0].filted_value, _uvw_phase[0].value, _uvw_phase[0].lowpass);
 
-	_uvw_phase[1].value = (float)uvw[1] * UVW_VOL_CEOF;
+	_uvw_phase[1].value = uvw[1];
 	LowPass_Filter(_uvw_phase[1].filted_value, _uvw_phase[1].value, _uvw_phase[1].lowpass);
 
-	_uvw_phase[2].value = (float)uvw[2] * UVW_VOL_CEOF;
+	_uvw_phase[2].value = uvw[2];
 	LowPass_Filter(_uvw_phase[2].filted_value, _uvw_phase[2].value, _uvw_phase[2].lowpass);
 #endif
 }

Applications/foc/samples.h

@@ -6,7 +6,7 @@ void samples_init(void);
 int get_vbus_int(void);
 float get_vbus_float(void);
 float get_throttle_float(void);
-void get_phase_vols(float *uvw);
+void get_phase_vols_filtered(float *uvw);
 void sample_uvw_phase(void);
 void sample_vbus(void);
 void sample_ibus(void);
@@ -24,7 +24,7 @@ float get_adc_vref(void);
 float get_thro2_5v_float(void);
 float get_thro_5v_float(void);
 float get_throttle2_float(void);
-void sample_uvw_phases_raw(s16 *uvw_raw);
+void get_uvw_phases_raw(float *uvw_raw);
 float sample_ibus_raw(void);
 float sample_acc_vol_raw(void);
 float sample_vbus_raw(void);

Applications/prot/can_foc_msg.c

@@ -42,7 +42,7 @@ void can_report_phase_current(u8 can) {
 void can_report_phase_voltage(u8 can) {
 	u8 data[8];
 	float s_vABC[3];
-	get_phase_vols(s_vABC);
+	get_phase_vols_filtered(s_vABC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
 	encode_s16(data + 2, S16Q5(s_vABC[0]));
 	encode_s16(data + 4, S16Q5(s_vABC[1]));

Project/MC105_V3_Z100.uvoptx

@@ -1119,7 +1119,7 @@
 
   <Group>
     <GroupName>StartUp</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>