SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 7:f674ef860c9c
- Parent:
- 6:fbe401163489
- Child:
- 8:f8b1402c8f3c
--- a/main.cpp Thu Nov 14 14:29:31 2019 +0000
+++ b/main.cpp Thu Nov 14 17:06:09 2019 +0000
@@ -2,22 +2,22 @@
#define dt 0.01f
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define FL_HSB_ID 0xA0 // Rx, 100Hz
-#define FR_HSB_ID 0xA1 // Rx
-#define RL_HSB_ID 0xA2 // Rx
-#define RR_HSB_ID 0xA3 // Rx
+#define FR_HSB_ID 0xA1 // Rx, 100Hz
+#define RL_HSB_ID 0xA2 // Rx, 100Hz
+#define RR_HSB_ID 0xA3 // Rx, 100Hz
#define FL_LSB_ID 0xB0 // Rx, 10Hz
-#define FR_LSB_ID 0xB1 // Rx
-#define RL_LSB_ID 0xB2 // Rx
-#define RR_LSB_ID 0xB3 // Rx
-#define PSU_cmd_ID 0xC4 // Rx, 100Hz
+#define FR_LSB_ID 0xB1 // Rx, 10Hz
+#define RL_LSB_ID 0xB2 // Rx, 10Hz
+#define RR_LSB_ID 0xB3 // Rx, 10Hz
+#define HMI_cmd_ID 0xC4 // Rx, 100Hz
#define FL_CMD_ID 0xC0 // Tx, 100Hz
-#define FR_CMD_ID 0xC1 // Tx
-#define RL_CMD_ID 0xC2 // Tx
-#define RR_CMD_ID 0xC3 // Tx
+#define FR_CMD_ID 0xC1 // Tx, 100Hz
+#define RL_CMD_ID 0xC2 // Tx, 100Hz
+#define RR_CMD_ID 0xC3 // Tx, 100Hz
#define AUX_sense_ID 0xE0 // Tx, 10Hz
-#define Qdrv_stat_ID 0xE1 // Tx
-#define IMU_sense_ID 0xE2 // Tx
+#define Qdrv_stat_ID 0xE1 // Tx, 10Hz
+#define IMU_sense_ID 0xE2 // Tx, 10Hz
#define MODOFL_VDUFLTCode 0x0001U //Drive module timeout after once online
#define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online
@@ -31,7 +31,7 @@
AnalogIn AUX_2(PC_4);
AnalogIn AUX_3(PC_2);
AnalogIn AUX_4(PC_1);
-AnalogIn SD_sense(PB_0); //Shutdown circuit driving end detection
+AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection
CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
Serial pc(USBTX, USBRX, 115200);
Ticker ticker1; //100Hz task
@@ -39,16 +39,8 @@
CANMessage can_msg_Rx;
CANMessage can_msg_Tx;
-//Recive bank for HSB & LFB
+//CAN msg bank
char temp_msg[8] = {0,0,0,0,0,0,0,0};
-char FL_HFB[8]; //data space to speed up can_Rx speed
-char FR_HFB[8];
-char RL_HFB[8];
-char RR_HFB[8];
-char FL_LFB[8];
-char FR_LFB[8];
-char RL_LFB[8];
-char RR_LFB[8];
//CAN to motor drive module, 100Hz
//msg ID: 0xC0~0xC3
@@ -61,20 +53,20 @@
//Module online flag
uint8_t FL_online = 0; // 0 indicate detection timeout
-uint8_t FR_online = 0; // reset value is 10 to hold for 0.1sec
+uint8_t FR_online = 0; // reset value is 3 to hold for 0.03sec
uint8_t RL_online = 0; // -1 per 100Hz task
uint8_t RR_online = 0;
uint8_t PSU_online = 0;
//Feedback data decoded out storage
-float FL_Temp_mtr = 0; // motor temperature degC, 10Hz recieving
-float FR_Temp_mtr = 0;
-float RL_Temp_mtr = 0;
-float RR_Temp_mtr = 0;
-float FL_Temp_ivt = 0; // inverter temperature degC, 10Hz recieving
-float FR_Temp_ivt = 0;
-float RL_Temp_ivt = 0;
-float RR_Temp_ivt = 0;
+float FL_Tmotor = 0; // motor temperature degC, 10Hz recieving
+float FR_Tmotor = 0;
+float RL_Tmotor = 0;
+float RR_Tmotor = 0;
+float FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving
+float FR_Tmodule = 0;
+float RL_Tmodule = 0;
+float RR_Tmodule = 0;
uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving
uint16_t FR_FLT_Run = 0;
uint16_t RL_FLT_Run = 0;
@@ -104,6 +96,8 @@
uint8_t RTD_HMI = 0; // 1 = HMI requesting
uint8_t RST_HMI = 0; // 1 = HMI request once
uint8_t AWD_HMI = 0; // 1 = HMI requesting
+float Trq_HMI = 0.0f; // Nm user request total torque
+float Steer_HMI = 0.0f; // deg steering wheel angel
//10/100Hz tasking
uint8_t HSTick = 5; // High speed tick
@@ -112,6 +106,11 @@
uint8_t LST_EXFL = 0;
uint8_t FLT_print = 0; // Send repeative error message
uint8_t FLT_print_cnt = 0;
+uint16_t AUX_1_u16 = 0x0; // acquired data
+uint16_t AUX_2_u16 = 0x0;
+uint16_t AUX_3_u16 = 0x0;
+uint16_t AUX_4_u16 = 0x0;
+float SDn_voltage = 0.0f;
//VDU states
typedef enum {
@@ -194,8 +193,8 @@
POST();
//Check if state transition only when all module online
- if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
-// if(1){ //Force online check pass only for debug 2019/11/14
+// if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
+ if(1) { //Force online check pass only for debug 2019/11/14
if ( //Check if any error
(FL_DSM == 3U)||
@@ -229,7 +228,7 @@
*/
//Forced RTD_HMI for debug purpose 2019/11/14
-// RTD_HMI = 1; //Should be set if can bus received data
+ RTD_HMI = 1; //Should be set if can bus received data
//Forced RTD_HMI for debug purpose 2019/11/14
RUNT(); //Run test
@@ -242,6 +241,7 @@
(RR_DSM == 3U)||
(VDU_FLT != 0)) {
VDU_STAT = VDU_Fault;
+ printf("Idle 2 Fault\n");
FLT_print = 1;
} else if (RTD_HMI != 0) { //Or command to run threw PSU
//Prepare to send out RTD and start motor
@@ -281,6 +281,7 @@
(RR_DSM == 3U)||
(VDU_FLT != 0)) {
VDU_STAT = VDU_Fault;
+ printf("Run 2 Fault\n");
FLT_print = 1;
} else if (RTD_HMI != 1) { //Or command to stop threw can bus
Run2Idle();
@@ -313,11 +314,12 @@
break;
}
- // Shit out torque distribution
+ // Shit out torque distribution and special command
if(VDU_STAT == VDU_Run) {
//Allow output torque
Tx_Tcmd_CAN1();
} else if(RST_cmd != 0) {
+ //Send out reset cmd once
Tx_CLRerr_CAN1();
} else {
//Force RTD off when not in VDU_Run
@@ -325,7 +327,7 @@
}
}
-
+ // End of high speed loop
// Do low speed state reporting
if (LST_EXFL == 1) {
@@ -334,17 +336,18 @@
// Print out error mesagge if exist, 1Hz repeative
if(FLT_print != 0) {
- FLT_print_cnt += 1;
+ FLT_print_cnt += FLT_print;
if(FLT_print_cnt > 19) {
printf("POST FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Post,FR_FLT_Post,RL_FLT_Post,RR_FLT_Post);
- printf("RUN FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Run,FR_FLT_Run,RL_FLT_Run,RR_FLT_Run);
- printf("VDU Err:0x%04X\n\n", VDU_FLT);
+ printf("RUN FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Run,FR_FLT_Run,RL_FLT_Run,RR_FLT_Run);
+ printf("VDU\n0x%04X\n\n", VDU_FLT);
FLT_print_cnt = 0;
}
}
+ }
+ // End of low speed state reporting
- }
- }
+ } // end of while
}
void Idle2Run(void)
@@ -386,10 +389,17 @@
void Aux_read(void)
{
+ //Capture analog in at once
+ AUX_1_u16 = AUX_1.read_u16();
+ AUX_2_u16 = AUX_2.read_u16();
+ AUX_3_u16 = AUX_3.read_u16();
+ AUX_4_u16 = AUX_4.read_u16();
+ SDn_voltage = 18.81f*SDn_sense.read();
}
void IMU_read(void)
{
+
}
void AWD(void)
@@ -400,45 +410,114 @@
void Rx_CAN1(void)
{
LED = 1;
+ int16_t tmp;
+
if(can1.read(can_msg_Rx)) {
-// switch(can_msg_Rx.id) { //Filtered the input message and do corresponding action
-// case 0xF0:
-// //Internal state address
-// RTD_State = can_msg_Rx.data[7] & 0x01; //get bit "0", result ( 1 = RTD, 0 = OFF )
-// CAN_flag_1 = 1;
-// break;
-//
-// case 0xA0 :
-// //Temperature infrom address 1
-// IGBT_Temp_A = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
-// IGBT_Temp_B = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-// IGBT_Temp_C = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
-// IGBT_Temp = max_val(IGBT_Temp_A,IGBT_Temp_B,IGBT_Temp_C);
-// CAN_flag_1 = 1;
-// break;
-//
-// case 0xA2 :
-// //Temperature infrom address 3
-// Motor_Temp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
-// CAN_flag_1 = 1;
-// break;
-//
-// case 0xA5:
-// Motor_Speed = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-// break;
-//
-// case 0xA6 :
-// //Current infrom address
-// DC_Current = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
-// CAN_flag_1 = 1;
-// break;
-//
-// case 0xA7 :
-// //Voltage infrom address
-// DC_Voltage = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
-// CAN_flag_1 = 1;
-// break;
-// }
+ switch(can_msg_Rx.id) { //Filtered input message
+ // Start of 100Hz msg
+ case FL_HSB_ID:
+ //HSB from FL motor drive
+ FL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ FL_W_ele = tmp*1.0f;
+ tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ FL_Trq_fil3 = tmp * 0.01f;
+ tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ FL_Trq_est = tmp * 0.01f;
+ FL_online = 3;
+ break;
+
+ case FR_HSB_ID:
+ //HSB from FR motor drive
+ FR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ FR_W_ele = tmp*1.0f;
+ tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ FR_Trq_fil3 = tmp * 0.01f;
+ tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ FR_Trq_est = tmp * 0.01f;
+ FR_online = 3;
+ break;
+
+ case RL_HSB_ID:
+ //HSB from RL motor drive
+ RL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ RL_W_ele = tmp*1.0f;
+ tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ RL_Trq_fil3 = tmp * 0.01f;
+ tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ RL_Trq_est = tmp * 0.01f;
+ RL_online = 3;
+ break;
+
+ case RR_HSB_ID:
+ //HSB from RR motor drive
+ RR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ RR_W_ele = tmp*1.0f;
+ tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ RR_Trq_fil3 = tmp * 0.01f;
+ tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ RR_Trq_est = tmp * 0.01f;
+ RR_online = 3;
+ break;
+
+ case HMI_cmd_ID:
+ //HMI command from PSU
+ AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch
+ RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
+ RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch
+ tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ Steer_HMI = tmp * 0.01f;
+ tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ Trq_HMI = tmp * 0.01f;
+ PSU_online = 3;
+ break;
+ // end of 100Hz msg
+
+ // Start of 10Hz msg
+ case FL_LSB_ID:
+ //LSB from FL motor drive
+ tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
+ FL_Tmotor = tmp*0.01f;
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ FL_Tmodule = tmp*0.01f;
+ FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ break;
+
+ case FR_LSB_ID:
+ //LSB from FR motor drive
+ tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
+ FR_Tmotor = tmp*0.01f;
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ FR_Tmodule = tmp*0.01f;
+ FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ break;
+
+ case RL_LSB_ID:
+ //LSB from RL motor drive
+ tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
+ RL_Tmotor = tmp*0.01f;
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ RL_Tmodule = tmp*0.01f;
+ RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ break;
+
+ case RR_LSB_ID:
+ //LSB from RR motor drive
+ tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
+ RL_Tmotor = tmp*0.01f;
+ tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+ RL_Tmodule = tmp*0.01f;
+ RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ break;
+ // end of 10Hz msg
+ }
}
LED = 0;
}
@@ -458,52 +537,45 @@
void Tx_Tcmd_CAN1(void) // 100 Hz
{
- // Need to change ID for real case 2019/11/13
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ int16_t tmp;
+ // Need to change ID for real case 2019/11/14
+ tmp = (int16_t) (FL_Tcmd * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ temp_msg[2] = RTD_cmd;
+ temp_msg[3] = RST_cmd;
can_msg_Tx = CANMessage(FL_HSB_ID,temp_msg,8,CANData,CANStandard); // FL_CMD_ID, now only for testing
+// can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ tmp = (int16_t) (FR_Tcmd * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ temp_msg[2] = RTD_cmd;
+ temp_msg[3] = RST_cmd;
can_msg_Tx = CANMessage(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
+// can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ tmp = (int16_t) (RL_Tcmd * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ temp_msg[2] = RTD_cmd;
+ temp_msg[3] = RST_cmd;
can_msg_Tx = CANMessage(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
+// can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ tmp = (int16_t) (RR_Tcmd * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ temp_msg[2] = RTD_cmd;
+ temp_msg[3] = RST_cmd;
can_msg_Tx = CANMessage(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
+// can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -512,14 +584,14 @@
void Tx_Qdrv_CAN1(void) // 10 Hz
{
// Auxilary sensor reading shitting out
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ temp_msg[0] = AUX_1_u16;
+ temp_msg[1] = AUX_1_u16 >> 8U;
+ temp_msg[2] = AUX_2_u16;
+ temp_msg[3] = AUX_2_u16 >> 8U;
+ temp_msg[4] = AUX_3_u16;
+ temp_msg[5] = AUX_3_u16 >> 8U;
+ temp_msg[6] = AUX_4_u16;
+ temp_msg[7] = AUX_4_u16 >> 8U;
can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -577,7 +649,7 @@
can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
- can1.filter(PSU_cmd_ID,0xFFFF,CANStandard,8);
+ can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8);
can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
}