加入直流母线电流的计算和获取

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

Applications/foc/core/foc_core.c

@@ -178,6 +178,21 @@ u8 PMSM_FOC_GetErrCode(void) {
 	return pmsm_foc.error_code;
 }
 
+//获取母线电流
+float PMSM_FOC_Get_iDC(void) {
+	s32 vd = pmsm_foc.FOC_Out->f_Vdq[0];
+	s32 vq = pmsm_foc.FOC_Out->f_Vdq[1];
+	s32 id = pmsm_foc.FOC_Out->f_Idq[0];
+	s32 iq = pmsm_foc.FOC_Out->f_Idq[1];
+    /*
+		根据公式（等幅值变换，功率不等）:
+		iDC x vDC = 2/3(iq x vq + id x vd);
+	*/
+	s32 m_pow = (vd * id + vq * iq); //s32q10
+	s16 iDC = m_pow / pmsm_foc.FOC_In->vDC; //s16q5
+	return S16Q5toF(iDC) * 0.667f;
+}
+
 void PMSM_FOC_Brake(bool brake) {
 	pmsm_foc.b_brake_in = brake;
 	if (!pmsm_foc.FOC_In->b_motEna) {

Applications/foc/core/foc_core.h

@@ -25,6 +25,7 @@ void PMSM_FOC_SetErrCode(u8 code);
 u8 PMSM_FOC_GetErrCode(void);
 bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
+float PMSM_FOC_Get_iDC(void);
 
 #endif /* _FOC_CORE_H__ */