SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 7:f674ef860c9c
- Parent:
- 6:fbe401163489
- Child:
- 8:f8b1402c8f3c
diff -r fbe401163489 -r f674ef860c9c main.cpp --- 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 }