SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
main.cpp
- Committer:
- open4416
- Date:
- 2019-11-13
- Revision:
- 5:8116016abee0
- Parent:
- 4:f05bef255292
- Child:
- 6:fbe401163489
File content as of revision 5:8116016abee0:
#include "mbed.h" #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 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 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 AUX_sense_ID 0xE0 // Tx, 10Hz #define Qdrv_stat_ID 0xE1 // Tx #define IMU_sense_ID 0xE2 // Tx DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active DigitalOut Fault_Ind(PC_10,0); //Indicate fault bt flashing, 1 active DigitalOut LED(D13, 0); //Internal LED output, general purpose CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message Serial pc(USBTX, USBRX, 115200); Ticker ticker1; //100Hz task CANMessage can_msg_Rx; CANMessage can_msg_Tx; //Recive bank for HSB & LFB 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 float FL_Tcmd = 0; // *100 before sent in int16_t float FR_Tcmd = 0; float RL_Tcmd = 0; float RR_Tcmd = 0; uint8_t RTD_cmd = 0; // start inverter switching cmd uint8_t RST_cmd = 0; // send out once to reset module fault //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 RL_online = 0; // -1 per 100Hz task uint8_t RR_online = 0; //Decoded out data storage float FL_Temp_mtr = 0; // motor temperature degC, 10Hz refresh float FR_Temp_mtr = 0; float RL_Temp_mtr = 0; float RR_Temp_mtr = 0; float FL_Temp_ivt = 0; // inverter temperature degC, 10Hz refresh float FR_Temp_ivt = 0; float RL_Temp_ivt = 0; float RR_Temp_ivt = 0; uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz refresh uint16_t FR_FLT_Run = 0; uint16_t RL_FLT_Run = 0; uint16_t RR_FLT_Run = 0; uint16_t FL_FLT_Post = 0; // POST fault code, 10Hz refresh uint16_t FR_FLT_Post = 0; uint16_t RL_FLT_Post = 0; uint16_t RR_FLT_Post = 0; float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz refresh float FR_Trq_fil3 = 0; float RL_Trq_fil3 = 0; float RR_Trq_fil3 = 0; float FL_Trq_est = 0; // Estimated Torque, 100Hz refresh float FR_Trq_est = 0; float RL_Trq_est = 0; float RR_Trq_est = 0; float FL_W_ele = 0; // Estimated W_ele, 100Hz refresh float FR_W_ele = 0; float RL_W_ele = 0; float RR_W_ele = 0; uint8_t FL_DSM = 0; // DSM state, 100Hz refresh uint8_t FR_DSM = 0; uint8_t RL_DSM = 0; uint8_t RR_DSM = 0; CAN_HandleTypeDef hcan1; //From Pedal Box msg uint8_t RTD_HMI = 0; //1 = requesting uint8_t AWD_HMI = 0; //1 = requesting //100Hz tasking uint8_t timer_flag = 0; void Rx_CAN1(void); void Rx_Decode(void); void Tx_Tcmd_CAN1(void); void Tx_Qdrv_CAN1(void); void Error_CAN1(void); int16_t max_val(int16_t i1, int16_t i2, int16_t i3); void timer1_interrupt(void) { timer_flag = 1; } int main() { SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1); can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2); can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3); can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4); 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.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq //House keeping task ticker1.attach_us(&timer1_interrupt, 10000); //10 ms while(1) { if(timer_flag == 1) { Tx_Qdrv_CAN1(); // pc.printf("SOC: %.2f\n", Module_Total*0.01f); // pc.printf("Min: %.3f\n", Module_Min*0.0001f); // pc.printf("Max: %.3f\n", Module_Max*0.0001f); // pc.printf("Max1: %.3f\n", Module_Max_BMS1*0.0001f); // pc.printf("Max2: %.3f\n", Module_Max_BMS2*0.0001f); // pc.printf("Temp: %.1f\n", Max_temp*0.01f); timer_flag = 0; } } } void Rx_CAN1(void) { LED = 1; 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; // } } LED = 0; } void Rx_Decode(void) { } void Tx_Tcmd_CAN1(void) { } void Tx_Qdrv_CAN1(void) { uint32_t ID = FL_HSB_ID; 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; can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard); if(!can1.write(can_msg_Tx)) { //Report fault } ID = RR_HSB_ID; can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard); if(!can1.write(can_msg_Tx)) { //Report fault } } //void Send2CAN1(void) //{ // uint32_t ID = 0xF1; // temp_msg[0] = Module_Total; // temp_msg[1] = Module_Total >> 8; // temp_msg[2] = Module_Min; // temp_msg[3] = Module_Min >> 8; // temp_msg[4] = Module_Max; // temp_msg[5] = Module_Max >> 8; // temp_msg[6] = Max_temp; // temp_msg[7] = Max_temp >> 8; // send_CAN1(); // can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard); // can1.write(can_msg_Tx); //} int16_t max_val(int16_t i1, int16_t i2, int16_t i3) { int16_t max = i1; if(i2 > max) max = i2; if(i3 > max) max = i3; return max; }