SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 6:fbe401163489
- Parent:
- 5:8116016abee0
- Child:
- 7:f674ef860c9c
--- a/main.cpp Wed Nov 13 13:56:55 2019 +0000 +++ b/main.cpp Thu Nov 14 14:29:31 2019 +0000 @@ -19,9 +19,19 @@ #define Qdrv_stat_ID 0xE1 // Tx #define IMU_sense_ID 0xE2 // Tx +#define MODOFL_VDUFLTCode 0x0001U //Drive module timeout after once online +#define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online +#define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal + + 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 +AnalogIn AUX_1(PC_0); //Auxilaru analog sensor +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 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message Serial pc(USBTX, USBRX, 115200); Ticker ticker1; //100Hz task @@ -54,94 +64,339 @@ 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; +uint8_t PSU_online = 0; -//Decoded out data storage -float FL_Temp_mtr = 0; // motor temperature degC, 10Hz refresh +//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 refresh +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; -uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz refresh +uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving 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 FL_FLT_Post = 0; // POST fault code, 10Hz recieving 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 FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving 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 FL_Trq_est = 0; // Estimated Torque, 100Hz recieving 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 FL_W_ele = 0; // Estimated W_ele, 100Hz recieving 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 FL_DSM = 0; // DSM state, 100Hz recieving 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 +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 -//100Hz tasking -uint8_t timer_flag = 0; +//10/100Hz tasking +uint8_t HSTick = 5; // High speed tick +uint8_t LSTick = 0; +uint8_t HST_EXFL = 0; // High speed execution flag +uint8_t LST_EXFL = 0; +uint8_t FLT_print = 0; // Send repeative error message +uint8_t FLT_print_cnt = 0; -void Rx_CAN1(void); -void Rx_Decode(void); -void Tx_Tcmd_CAN1(void); -void Tx_Qdrv_CAN1(void); -void Error_CAN1(void); +//VDU states +typedef enum { + VDU_PowerOn = 0U, + VDU_Idle = 1U, + VDU_Run = 2U, + VDU_Fault = 3U +} VDU_STATE_TYPE; +VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; //VDU current state +uint16_t VDU_FLT = 0; //VDU internal fault code -int16_t max_val(int16_t i1, int16_t i2, int16_t i3); +//Prototype +void CAN_init(void); //Initial CAN frequency filter... +void Module_WD(void); //Software watchdog indicate module state +void IMU_read(void); //Update IMU readings by once +void Aux_read(void); //Update AUX reafings by once +void Idle2Run(void); //Initializing before running +void Run2Idle(void); //Initializing before idling +void POST(void); //Check IMU error +void RUNT(void); //Check POST, timeout, ShutDrv voltage error +void AWD(void); //AWD main program +void Rx_CAN1(void); //CAN RX handler +void Tx_CLRerr_CAN1(void); //Send reset cmd to modules +void Tx_Estop_CAN1(void); //Send out heart beat but force RTD off +void Tx_Tcmd_CAN1(void); //Send out heart beat command +void Tx_Qdrv_CAN1(void); //Send out low speed heart beat for logging +void CANpendTX(void); //Helper function for CAN Tx +int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4); void timer1_interrupt(void) { - timer_flag = 1; + HSTick += 1; + LSTick += 1; + if (HSTick > 9) { // 100Hz + HST_EXFL = 1; + HSTick = 0; + } + if (LSTick > 99) { // 10Hz + LST_EXFL = 1; + LSTick = 0; + } } 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); + //Init CAN network + CAN_init(); + + //Start House keeping task + printf("VDU start up, pend for module online\n"); + ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick + while(1) { + + // Do high speed loop + if (HST_EXFL == 1) { + HST_EXFL = 0; + + // Get IMU, Auxs + IMU_read(); + Aux_read(); + Module_WD(); // Module online watch dog + + // Run state machine + switch (VDU_STAT) { + + case VDU_PowerOn: + /* Power on state + * Description: + * Simple start up sequence will be done here + * Do: + * VDU internal POST + * Wait till modules + PSU online + * To VDU_Idle (RTD off): + * Prepare for 4WD main program + * To VDU_Fault: + * Run the error handling service + */ + + //Basic IMU test + 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 ( //Check if any error + (FL_DSM == 3U)|| + (FR_DSM == 3U)|| + (RL_DSM == 3U)|| + (RR_DSM == 3U)|| + (VDU_FLT != 0)) { + VDU_STAT = VDU_Fault; + FLT_print = 1; + } else { + //All module online & POST pass + VDU_STAT = VDU_Idle; + printf("All module online, VDU now Idle\n"); + } + } //Else keep in state VDU_PowerOn + break; + + case VDU_Idle: + /* Controller Idle state + * Description: + * Normal latched state, wait for RTD_HMI set from PSU + * 4WD in running but output mask to 0 + * Do: + * 4WD controller + * Check: + * RUN faults if any + * To VDU_Run: + * Initialize parameters for start up, set RTD_cmd + * To VDU_Fault: + * Run the error handling service + */ + + //Forced RTD_HMI for debug purpose 2019/11/14 +// RTD_HMI = 1; //Should be set if can bus received data + //Forced RTD_HMI for debug purpose 2019/11/14 + + RUNT(); //Run test + AWD(); //AWD main program -// can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 - can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq + if ( //Check if any error + (FL_DSM == 3U)|| + (FR_DSM == 3U)|| + (RL_DSM == 3U)|| + (RR_DSM == 3U)|| + (VDU_FLT != 0)) { + VDU_STAT = VDU_Fault; + FLT_print = 1; + } else if (RTD_HMI != 0) { //Or command to run threw PSU + //Prepare to send out RTD and start motor + Idle2Run(); + VDU_STAT = VDU_Run; + printf("Idle 2 Run\n"); + } //Else keep in state + break; + + case VDU_Run: + /* Controller Run state + * Description: + * Normal latched state, after RTD_HMI is set from PSU + * Same to Idle state except RTD_cmd is set + * Do: + * 4WD controller + * Check: + * RUN faults if any + * To VDU_Idle: + * Initialize parameters for idling, reset RTD_cmd + * To VDU_Fault: + * Run the error handling service + */ + + RUNT(); //Run test + AWD(); //AWD main program + + //Temporary debug posting area 2019/11/14 + //printf("%d,%d\n", Encoder_cnt, Encoder_del); + //printf("%d\n\r", (int16_t)Tmodule);// + //printf("%d\n\r", (int16_t)Vbus); - //House keeping task - ticker1.attach_us(&timer1_interrupt, 10000); //10 ms - while(1) { - if(timer_flag == 1) { + if ( //Check if any error + (FL_DSM == 3U)|| + (FR_DSM == 3U)|| + (RL_DSM == 3U)|| + (RR_DSM == 3U)|| + (VDU_FLT != 0)) { + VDU_STAT = VDU_Fault; + FLT_print = 1; + } else if (RTD_HMI != 1) { //Or command to stop threw can bus + Run2Idle(); + VDU_STAT = VDU_Idle; + printf("Run 2 Idle\n"); + } //Else keep in state + break; + + case VDU_Fault: + /* Controller Fault state + * Description: + * Fault latched state if any faults is detected + * Same to Idle state except keep till RTD_HMI reset + * Do: + * Nothing, like a piece of shit + * Check: + * RUN faults if any + * To VDU_PowerOn: + * Restart VDU + */ + + RUNT(); //Run test + + if (RST_HMI == 1) { //PSU reset to clear error + RST_cmd = 1; + FLT_print = 0; + VDU_STAT = VDU_PowerOn; + printf("Module reset\nVDU restarting...\n"); + } //Else keep in state + break; + } + + // Shit out torque distribution + if(VDU_STAT == VDU_Run) { + //Allow output torque + Tx_Tcmd_CAN1(); + } else if(RST_cmd != 0) { + Tx_CLRerr_CAN1(); + } else { + //Force RTD off when not in VDU_Run + Tx_Estop_CAN1(); + } + + } + + + // Do low speed state reporting + if (LST_EXFL == 1) { + LST_EXFL = 0; 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; + + // Print out error mesagge if exist, 1Hz repeative + if(FLT_print != 0) { + FLT_print_cnt += 1; + 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); + FLT_print_cnt = 0; + } + } + } } } +void Idle2Run(void) +{ + RTD_cmd = 1; +} + +void Run2Idle(void) +{ + RTD_cmd = 0; +} + +void POST(void) +{ + //Check IMU status + if(0) { + VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error + } +} + +void RUNT(void) +{ + POST(); + + //Check timeout + if((FL_online*FR_online*RL_online*RR_online) == 0) { + VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout + } + if(PSU_online == 0) { + VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout + } + + //Check IMU status + //XXX + + //Check ShutDrv voltage + //XXX +} + +void Aux_read(void) +{ +} + +void IMU_read(void) +{ +} + +void AWD(void) +{ + +} + void Rx_CAN1(void) { LED = 1; @@ -188,17 +443,75 @@ LED = 0; } -void Rx_Decode(void) +void Tx_CLRerr_CAN1(void) { + RTD_cmd = 0; //disable as default + Tx_Tcmd_CAN1(); + RST_cmd = 0; //on shot +} + +void Tx_Estop_CAN1(void) +{ + RTD_cmd = 0; //force disable + Tx_Tcmd_CAN1(); } -void Tx_Tcmd_CAN1(void) +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; + can_msg_Tx = CANMessage(FL_HSB_ID,temp_msg,8,CANData,CANStandard); // FL_CMD_ID, now only for testing + 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; + can_msg_Tx = CANMessage(FR_HSB_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; + can_msg_Tx = CANMessage(RL_HSB_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; + can_msg_Tx = CANMessage(RR_HSB_ID,temp_msg,8,CANData,CANStandard); + CANpendTX(); + can1.write(can_msg_Tx); + } -void Tx_Qdrv_CAN1(void) +void Tx_Qdrv_CAN1(void) // 10 Hz { - uint32_t ID = FL_HSB_ID; + // Auxilary sensor reading shitting out temp_msg[0] = 1; temp_msg[1] = 2; temp_msg[2] = 3; @@ -207,38 +520,93 @@ 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 - } + can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard); + CANpendTX(); + can1.write(can_msg_Tx); + + // Qdrive internal state 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; + can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard); + CANpendTX(); + can1.write(can_msg_Tx); - ID = RR_HSB_ID; - can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard); - if(!can1.write(can_msg_Tx)) { - //Report fault + // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use + 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(IMU_sense_ID,temp_msg,8,CANData,CANStandard); + CANpendTX(); + can1.write(can_msg_Tx); +} + +void CANpendTX(void) +{ + //Pend till TX box has empty slot, timeout will generate error + uint32_t timeout = 0; + while(!(CAN1->TSR & CAN_TSR_TME_Msk)) { + //Wait till non empty + timeout += 1; + if(timeout > 10000) { + // Put some timeout error handler + break; + } } } -//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); -//} +void CAN_init(void) +{ + //Set CAN system + 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 +} -int16_t max_val(int16_t i1, int16_t i2, int16_t i3) +void Module_WD(void) +{ + if (FL_online != 0) { + FL_online -= 1; + } + if (FR_online != 0) { + FR_online -= 1; + } + if (RL_online != 0) { + RL_online -= 1; + } + if (RR_online != 0) { + RR_online -= 1; + } + if (PSU_online != 0) { + PSU_online -= 1; + } +} + +int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4) { int16_t max = i1; if(i2 > max) max = i2; if(i3 > max) max = i3; + if(i4 > max) max = i4; return max; -} \ No newline at end of file +} +// pc.printf("SOC: %.2f\n", Module_Total*0.01f);