上报上位机的数据从float改为S16Q5格式

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

Applications/math/fix_math.h

@@ -14,26 +14,26 @@ typedef int32_t   s32q5_t;
 typedef int32_t   s32q19_t;
 
 //#define S16Q4(A) (signed short)((A) * 16.0F)
-#define S16Q5(A) (signed short)((A) * 32.0F)
+#define S16Q5(A) (s16)((A) * 32.0F)
 #define S16Q5toF(A) ((float)(A)/32.0f)
 
-#define S16Q10(A) (signed short)((A) * 1024.0F)
+#define S16Q10(A) (s16)((A) * 1024.0F)
 #define S16Q10toF(A) ((float)(A)/1024.0f)
 
-#define S16Q14(A) (signed short)((A) * 16384.0F)
+#define S16Q14(A) (s16)((A) * 16384.0F)
 #define S16Q14toF(A)  ((float)(A)/16384.0f)
 
-#define S32Q4(A) (signed int)((A) * 16.0F)
+#define S32Q4(A) (s32)((A) * 16.0F)
 #define S32Q4toF(A) ((float)(A)/16.0f)
 
-#define S32Q5(A) (signed int)((A) * 32.0F)
+#define S32Q5(A) (s32)((A) * 32.0F)
 #define S32Q5toF(A) ((float)(A)/32.0f)
 
 
-#define S32Q14(A) (signed int)((A) * 16384.0F)
+#define S32Q14(A) (s32)((A) * 16384.0F)
 #define S32Q14toF(A) ((float)(A)/16384.0f)
 
-#define S32Q19(A) (signed int)((A) * (16384.0F * 32.0F))
+#define S32Q19(A) (s32)((A) * (16384.0F * 32.0F))
 #define S32Q19toF(A) ((float)(A)/(16384.0F * 32.0F))
 
 #define S32Q14_MUL(a, b) (((a)*(b)) >>14)

Applications/prot/can_foc_msg.c

@@ -18,8 +18,8 @@ void can_report_power(u8 can) {
 	s16 rpm = (s16)motor_encoder_get_speed();
 	float vDC = get_vbus_float();
 	float iDC = PMSM_FOC_GetVbusCurrent();	
-	s16 v = (s16)(vDC * 10.0f);
-	s16 i = (s16)(iDC * 10.0f);
+	s16 v = S16Q5(vDC);
+	s16 i = S16Q5(iDC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
 	encode_s16(data + 2, ABS(rpm));
 	encode_s16(data + 4, v);
@@ -29,32 +29,32 @@ void can_report_power(u8 can) {
 
 
 void can_report_phase_current(u8 can) {
-	u8 data[14];
+	u8 data[8];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
-	encode_float(data + 2, PMSM_FOC_Get()->in.s_iABCFilter[0]);
-	encode_float(data + 6, PMSM_FOC_Get()->in.s_iABCFilter[1]);
-	encode_float(data + 10, PMSM_FOC_Get()->in.s_iABCFilter[2]);
+	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->in.s_iABCFilter[0]));
+	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->in.s_iABCFilter[1]));
+	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->in.s_iABCFilter[2]));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
 void can_report_phase_voltage(u8 can) {
-	u8 data[14];
+	u8 data[8];
 	float s_vABC[3];
 	get_phase_vols(s_vABC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
-	encode_float(data + 2, s_vABC[0]);
-	encode_float(data + 6, s_vABC[1]);
-	encode_float(data + 10, s_vABC[2]);
+	encode_s16(data + 2, S16Q5(s_vABC[0]));
+	encode_s16(data + 4, S16Q5(s_vABC[1]));
+	encode_s16(data + 6, S16Q5(s_vABC[2]));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
 void can_report_dq_current(u8 can) {
-	u8 data[10];
+	u8 data[6];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
 	float id = PMSM_FOC_GetDQCurrent()->d;
 	float iq = PMSM_FOC_GetDQCurrent()->q;
-	encode_float(data + 2, id);
-	encode_float(data + 6, iq);
+	encode_s16(data + 2, S16Q5(id));
+	encode_s16(data + 4, S16Q5(iq));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
@@ -97,9 +97,9 @@ void can_report_mpta_values(u8 can) {
 		return;
 	}
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
-	encode_s16(data + 2, (s16)(PMSM_FOC_Get()->out.s_RealCurrent * 10.0f));
-	encode_s16(data + 4, (s16)(PMSM_FOC_Get()->in.s_targetIdq.d * 10.0f));
-	encode_s16(data + 6, (s16)(PMSM_FOC_Get()->in.s_targetIdq.q * 10.0f));
+	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->out.s_RealCurrent));
+	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->in.s_targetIdq.d));
+	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->in.s_targetIdq.q));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }