SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
main.cpp
- Committer:
- open4416
- Date:
- 2021-02-21
- Revision:
- 25:3c6e83b449b2
- Parent:
- 24:518ec8a4fb6d
- Child:
- 26:ad4fbceeb4ae
File content as of revision 25:3c6e83b449b2:
#include "mbed.h" #include "main.h" // define this for newversion test mode //#define IMU_direct #ifndef IMU_direct #include "imu_driver.hpp" #else typedef struct AhrsRawData { uint16_t status; int16_t rate[3]; int16_t accel[3]; uint16_t temperature; int16_t attitude[3]; } AhrsData; #define Read_VG (0x3D) #define ACCL2F (4000.0f) #define GYRO2F (100.0f) #define AHRS2F (90.0f) #define VG_len 11U #endif #define pi 3.14159265359f #define d2r 0.01745329252f #define dt 0.01f #define st2r 0.033f //steer signal to actual rad DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active DigitalOut Fault_Ind(PC_12,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_3); AnalogIn AUX_3(PC_2); AnalogIn AUX_4(PC_1); AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection AnalogIn Brk_sense(PA_4); //Brake sensor readings CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message SPI spi2(PB_15, PB_14, PB_13); //1Mbps default, MOSI MISO SCLK, forIMU #ifndef IMU_direct ImuDriver <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg); //SPI instance, reset, data ready, slave select #else AhrsData ahrsdata; DigitalOut cs(PB_1,1); InterruptIn drdy(PB_2); #endif Serial pc(USBTX, USBRX, 115200); Ticker ticker1; //100Hz task CANMessage can_msg_Rx; CANMessage can_msg_Tx; void timer1_interrupt(void) { HSTick += 1; LSTick += 1; if (HSTick > 9) { // 100Hz HST_EXFL = 1; HSTick = 0; } if (LSTick > 99) { // 10Hz LST_EXFL = 1; LSTick = 0; } } int main() { //Init CAN network CAN_init(); // Note now in Gloable test mode only for testing 2019/11/17 //Start House keeping task printf("VDU start up, pend for module online\n"); #ifdef IMU_direct drdy.fall(&IMU_isr); //IMU interrupt service spi2.format(16, 3); //spi2.frequency(); //As default #endif wait_ms(100); 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, Max Temperature Cooler(); IMU_read(); Aux_read(); Module_WD(); // 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(); // printf("%d,%d,%d,%d,%d\n",FL_online,FR_online,RL_online,RR_online,PSU_online); //Check if state transition only when all module online if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; RST_HMI = 0; //Ensure new RST action after error printf("POST Fault\n"); FLT_print = 1; } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) { // } else if (1) { //2019/11/30 only use for force online debug //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 if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; RST_HMI = 0; //Ensure new RST action after error 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 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); if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; RST_HMI = 0; //Ensure new RST action after error printf("Run 2 Fault\n"); 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_HMI = 0; RST_cmd = 1; FLT_print = 0; //Stop error printing // FL_online = 5; // 0 indicate detection timeout // FR_online = 5; // reset value is 3 to hold for 0.03sec // RL_online = 5; // -1 per 100Hz task // RR_online = 5; // PSU_online = 5; VDU_FLT = 0; VDU_STAT = VDU_Reset; printf("VDU rebooting...\n"); printf("QDrive rebooting...\n"); } //Else keep in state break; case VDU_Reset: /* Controller Reset state * Description: * A state after reset by HMI when Fault latched * Wait till four driver processing, timeout protected * Do: * Nothing, just a soft delay * Check: * Timeout condition met * To VDU_Idle: * A valid reset * To VDU_Fault: * A fail reset */ RUNT(); //Run test if(!RL_DSM) { // if(!(FL_DSM|FR_DSM|RL_DSM|RR_DSM)) { // 2020/3/13 for real case printf("...\n"); VDU_FLT &= ~(DSM_VDUFLTCode); //Clear if fine } Reset_to += 1; //Time out check if (Reset_to > 30) { Reset_to = 0; if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; //Back to fault state wait for next reset printf("Reset fail 2 Fault\n"); FLT_print = 1; } else { //A success reset VDU_STAT = VDU_Idle; printf("Reset ok 2 Idle\n"); } } //Else keep in state break; } // Shit out torque distribution and special command if(VDU_STAT == VDU_Run) { //Allow output torque Tx_Tcmd_CAN1(); } else if(RST_cmd) { //Send out reset cmd once Tx_CLRerr_CAN1(); } else { //Force RTD off when not in VDU_Run Tx_Estop_CAN1(); } //2019/12/18 Add here to test IMU, newer version use inturrupt get data // pc.printf("%.3f,%.3f,%.3f\n\r", imu.ahrsProcessedData.attitude[0], imu.ahrsProcessedData.attitude[1], imu.ahrsProcessedData.attitude[2]); // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]); // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.accel[0], imu.imuProcessedData.accel[1], imu.imuProcessedData.accel[2]); // pc.printf("%.3f,%.3f,%.3f\n\r", YR_imu, Ax_imu, Ay_imu); // pc.printf("%.3f,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu); // pc.printf("%.3f\n\r", Trq_HMI*100.0f); // pc.printf("%.3f\n\r", Steer_HMI); // pc.printf("%.1f\n\r", Vx_wss); } // End of high speed loop // Do low speed state reporting 10 Hz if (LST_EXFL == 1) { LST_EXFL = 0; //Cooling Cooler(); //Print low speed data on CAN Tx_Qdrv_CAN1(); // Print out error mesagge if exist, 0.5Hz repeative if(FLT_print != 0) { //Merge Faults //USE THIS FOR FULL FUNCTION FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post; FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run; //ONLY FOR TEST TODO // FLT_Post = RL_FLT_Post; // Use only for single module test // FLT_Run = RL_FLT_Run; //2020/3/10 //UART FLT_print_cnt += FLT_print; if(FLT_print_cnt > 19) { if(FLT_Post!=0)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); if(FLT_Run!=0)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); if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT); //Only temperature printing 2020/6/30 printf("Tmotor FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor); printf("Tmodule FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule); FLT_print_cnt = 0; } //LED if(Ind_refresh) { // Refresh data for LED indication after run threw FLT_Post_ind = FLT_Post; FLT_Run_ind = FLT_Run; VDU_FLT_ind = VDU_FLT; Ind_refresh = 0; // Set after run threw all error bits } } else { // Clear & stop LED when no faults FLT_Post_ind = 0; FLT_Run_ind = 0; VDU_FLT_ind = 0; Repeat = 0U; Phase = 0U; Blk_n = 0U; } // Blinky output if (VDU_STAT != VDU_PowerOn) { // Case after poweron (all module online) or fault(s) occur Ind_refresh = IndicationKernel( &Pattern, &Repeat, &Phase, &FLT_Post_ind, &FLT_Run_ind, &VDU_FLT_ind); LED = Indication(Pattern, &Repeat, &Blk_n); Fault_Ind = LED; } else { // Case no fault while waiting for all module online LED = !LED; //Fast 5Hz blinky indicate the activity Fault_Ind = LED; } } // End of low speed state reporting } // end of while } void Idle2Run(void) { RTD_cmd = 1; } void Run2Idle(void) { RTD_cmd = 0; } void POST(void) { //Check IMU status abnormal // if(fabsf(YR_imu) > 250) { //half turn per sec, strange // VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error // } } void RUNT(void) { //POST POST(); //Check module response 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 ShutDrv voltage when running if(VDU_STAT == VDU_Run) { if(SDn_voltage < 8.0f) { //2020/4/5 TODO remove in real case VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv } } //Check IMU status abnormal add in 2020/10/07 if(fabsf(YR_imu) > 250.0f) { //half turn per sec, strange VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error } } void Aux_read(void) { //Capture analog in at once imu_driver ver AUX_1_u16 = AUX_1.read_u16() >> 4U; AUX_2_u16 = AUX_2.read_u16() >> 4U; AUX_3_u16 = AUX_3.read_u16() >> 4U; AUX_4_u16 = AUX_4.read_u16() >> 4U; SDn_voltage = 18.81f*SDn_sense.read(); //Read in Shutdown circuit voltage in output end Brk_voltage = 5.5f*Brk_sense.read(); } #ifdef IMU_direct void IMU_isr(void) { //Start data transfer uint8_t data_rx = 0; uint16_t reg_VG = Read_VG<<8U; cs = 0; spi2.write(reg_VG); while(data_rx < VG_len) { uint16_t spi_data = spi2.write(0x0000); switch(data_rx) { case 0: ahrsdata.status = spi_data; break; case 1: case 2: case 3: ahrsdata.rate[data_rx - 1] = spi_data; break; case 4: case 5: case 6: ahrsdata.accel[data_rx - 4] = spi_data; break; case 7: ahrsdata.temperature = spi_data; break; case 8: case 9: case 10: ahrsdata.attitude[data_rx - 8] = spi_data; break; default: break; } ++data_rx; } cs = 1; } #endif void IMU_read(void) { #ifndef IMU_direct //Get readings threw back ground, direction not checked 2019/12/20 // Direction checked 2020/10/29 YR_imu = imu.imuProcessedData.rate[2]; Ax_imu = imu.imuProcessedData.accel[0]; Ay_imu = imu.imuProcessedData.accel[1]; Roll_imu = imu.ahrsProcessedData.attitude[0]; Pitch_imu = imu.ahrsProcessedData.attitude[1]; Yaw_imu = imu.ahrsProcessedData.attitude[2]; #else YR_imu = ahrsdata.rate[2]/GYRO2F; Ax_imu = ahrsdata.accel[0]/ACCL2F; Ay_imu = ahrsdata.accel[1]/ACCL2F; Roll_imu = ahrsdata.attitude[0]/AHRS2F; Pitch_imu = ahrsdata.attitude[1]/AHRS2F; #endif } void AWD(void) { float Tayc_tmp = 0; //active part of yaw control system float Tlsd_tmp = 0; //limit slip differetial torque float YdelW_ele = 0; //Ideal L-R electric speed difference //Get estimate from wheel only Vx_wss = (FL_W_ele+FR_W_ele+RL_W_ele+RR_W_ele)*0.25f/4.0f/11.0f*0.235f; // average, pole pair, reducer, wheel radius //Simple comp filter is fine without GSS onboard Vx_fil += (Ax_imu-0.01f)*9.807f*0.01f; //-0.01 here is to be calibrated Vx_fil = Vx_fil*0.98f + Vx_wss*0.02f; //0.98, 0.02 is coefficient if(AWD_HMI) { // A simple version is put here for reading //YR_ref = Steer_HMI*st2r*Vb_est/(Base+EG*Vb_est*Vb_est); /*Still in test section, ignore*/ //sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0); //FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig; //FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig; //RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig); //RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig); /*A basic static distribution*/ FL_Tcmd = 0.11f*Trq_HMI; FR_Tcmd = 0.11f*Trq_HMI; RL_Tcmd = 0.25f*Trq_HMI; RR_Tcmd = 0.25f*Trq_HMI; /*A basic Yaw control*/ YR_ref = (Steer_HMI*st2r)*Vx_fil/Base; // Center calibration moved to can recieve Tayc_tmp = (YR_ref - YR_imu*d2r)*10.0f; // map 1 rad/s YR difference to ~ 10Nm Tayc_tmp = constrain(Tayc_tmp,-5.0f,5.0f); FL_Tcmd += -Tayc_tmp; FR_Tcmd += Tayc_tmp; RL_Tcmd += -Tayc_tmp; RR_Tcmd += Tayc_tmp; /*A basic ideal differential controller*/ YdelW_ele = YR_ref*Track/0.235f*11.0f*4.0f; // dir, rate to speed, wheel radius, reducer, pole pair //YdelW_ele > 0 when left turning /*A basic static distribution + differential for test 2020/7/19*/ //Front differential Tlsd_tmp = (FL_W_ele - FR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f); FL_Tcmd += -Tlsd_tmp; FR_Tcmd += Tlsd_tmp; //Rear differential Tlsd_tmp = (RL_W_ele - RR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f); RL_Tcmd += -Tlsd_tmp; RR_Tcmd += Tlsd_tmp; } else { //2020/8/19 for fast test pure rear Tlsd_tmp = (FL_W_ele - FR_W_ele)*0.01f; // map 1000 rad/s difference to ~ 10Nm Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f); if(Tlsd_tmp>0.0f) { //L > R FL_Tcmd = 0.15f*Trq_HMI - Tlsd_tmp; FR_Tcmd = 0.15f*Trq_HMI; } else { //L < R, Tlsd_tmp < 0 FL_Tcmd = 0.15f*Trq_HMI; FR_Tcmd = 0.15f*Trq_HMI + Tlsd_tmp; } //Rear differential Tlsd_tmp = (RL_W_ele - RR_W_ele)*0.005f; // map 1000 rad/s difference to ~ 5Nm Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f); if(Tlsd_tmp>0.0f) { //L > R RL_Tcmd = 0.25f*Trq_HMI - Tlsd_tmp; RR_Tcmd = 0.25f*Trq_HMI; } else { //L < R, Tlsd_tmp < 0 RL_Tcmd = 0.25f*Trq_HMI; RR_Tcmd = 0.25f*Trq_HMI + Tlsd_tmp; } // // A basic static distribution 2020/7/19 // FL_Tcmd = 0.15f*Trq_HMI; // FR_Tcmd = 0.15f*Trq_HMI; // RL_Tcmd = 0.25f*Trq_HMI; // RR_Tcmd = 0.25f*Trq_HMI; }//last line of: if(AWD_HMI){} ////Add to force normal drive // FL_Tcmd = 0.25f*Trq_HMI; // FR_Tcmd = 0.25f*Trq_HMI; // RL_Tcmd = 0.25f*Trq_HMI; // RR_Tcmd = 0.25f*Trq_HMI; //Add to force rear drive // FL_Tcmd = 0.2f*Trq_HMI; // FR_Tcmd = 0.2f*Trq_HMI; // RL_Tcmd = 0.5f*Trq_HMI; // RR_Tcmd = 0.5f*Trq_HMI; //Direction define, move to can sendout // FL_Tcmd = -1.0f*FL_Tcmd; // FR_Tcmd = 1.0f*FR_Tcmd; // RL_Tcmd = -1.0f*RL_Tcmd; // RR_Tcmd = 1.0f*RR_Tcmd; } void ASL(void) { //Filter out each motor W_ele and get approximate accel, compare with IMU //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel) //Fill out if enough time } void Rx_CAN1(void) { // LED = 1; int16_t tmp; if(can1.read(can_msg_Rx)) { switch(can_msg_Rx.id) { //Filtered input message // Start of 100Hz msg case FL_HSB_ID://1 //HSB from FL motor drive FL_DSM = can_msg_Rx.data[6] & 0x03; //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 = 5; //If fault if(FL_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } break; case FR_HSB_ID://2 //HSB from FR motor drive FR_DSM = can_msg_Rx.data[6] & 0x03; //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 = 5; if(FR_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } break; case RL_HSB_ID://3 //HSB from RL motor drive RL_DSM = can_msg_Rx.data[6] & 0x03; //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 = 5; if(RL_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } break; case RR_HSB_ID://4 //HSB from RR motor drive RR_DSM = can_msg_Rx.data[6] & 0x03; //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 = 5; if(RR_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } break; case HMI_cmd_ID://5 //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 - 0.0f; //0.7f here is to calibrated center tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; Trq_HMI = tmp * 0.01f; PSU_online = 5; break; // end of 100Hz msg // Start of 10Hz msg case FL_LSB_ID://6 //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://7 //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://8 //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://9 //LSB from RR motor drive tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6]; RR_Tmotor = tmp*0.01f; tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; RR_Tmodule = tmp*0.01f; RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; break; case PedalStat_ID://10 RTD_SW = 0x01&can_msg_Rx.data[1]; break; // end of 10Hz msg } } // LED = 0; } void Tx_CLRerr_CAN1(void) { Tx_Estop_CAN1(); //disable as default RST_cmd = 0; //clear out one shot } void Tx_Estop_CAN1(void) { RTD_cmd = 0; //force disable Tx_Tcmd_CAN1(); } void Tx_Tcmd_CAN1(void) // 100 Hz { int16_t tmp; 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; // temp_msg[3] = 0U; // 2020/3/4 add to disable HMI reseting Driver temp_msg[4] = 0U; temp_msg[5] = 0U; temp_msg[6] = 0U; temp_msg[7] = 0U; can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); tmp = (int16_t) (FR_Tcmd * 100.0f); temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); tmp = (int16_t) (RL_Tcmd * -100.0f); temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); tmp = (int16_t) (RR_Tcmd * 100.0f); temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); // IMU attitude readings shitting out tmp = (int16_t) (YR_imu * 100.0f); temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; tmp = (int16_t) (Roll_imu * 100.0f); temp_msg[2] = tmp; temp_msg[3] = tmp >> 8U; tmp = (int16_t) (Pitch_imu * 100.0f); temp_msg[4] = tmp; temp_msg[5] = tmp >> 8U; temp_msg[6] = (int8_t)(Ax_imu * 50.0f); temp_msg[7] = (int8_t)(Ay_imu * 50.0f); can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); // Some internal control variables shitting out tmp = (int16_t) (Vx_fil * 100.0f); temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; can_msg_Tx = CANMessage(RegularVar_ID,temp_msg,2,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); } void Tx_Qdrv_CAN1(void) // 10 Hz { //int16_t tmp; // Auxilary sensor reading shitting out 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); // Qdrive internal state shitting out temp_msg[0] = VDU_FLT; temp_msg[1] = VDU_FLT >> 8U; temp_msg[2] = VDU_STAT; temp_msg[3] = (int8_t)(SDn_voltage*10.0f); temp_msg[4] = (int8_t)(Brk_voltage*50.0f); temp_msg[5] = 0U; temp_msg[6] = 0U; temp_msg[7] = 0U; can_msg_Tx = CANMessage(Qdrv_stat_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 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(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring can1.filter(PedalStat_ID,0xFFFF,CANStandard,9); // PSU online monitoring // can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq } void Module_WD(void) { //Module online dissipitive indicator 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; } } uint8_t Indication( // Blink indicator in pattern * repeat uint8_t pattern, uint16_t*repeat, uint8_t*blk_n) { uint8_t out = 0; if(*repeat==0) return out; // reject repeat = 0 case, out = 0 out = (pattern>>(*blk_n)) & 1U; // blink from LSB to MSB if(*blk_n < 7U) { *blk_n += 1U; } else { // a full pattern was passed *blk_n = 0U; *repeat >>= 1U; } return out; } uint8_t IndicationKernel( // Generate blink pattern, return 1 if all ind cleared uint8_t*pattern, uint16_t*repeat, uint8_t*phase, uint16_t*Post_ind, uint16_t*Run_ind, uint16_t*VDU_ind) { //Blink indicator in pattern //If FLT_Run = 0b0010-0110, pattern will be --......--...--.. uint8_t refresh = 0; if(*repeat!=0) return refresh; // skip straight to Indication() if(*Post_ind != 0) { switch(*phase) { case 0U: *repeat = Post_rep; *pattern = L_Pulse; *phase = 1U; break; case 1U: *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit *Post_ind &= ~*repeat; // this bit is used out *pattern = S_Pulse; *phase = 2U; break; case 2U: *repeat = 1U; *pattern = N_Pulse; *phase = 0U; break; } return refresh; } if(*Run_ind != 0) { switch(*phase) { case 0U: *repeat = Run_rep; *pattern = L_Pulse; *phase = 1U; break; case 1U: *repeat = (*Run_ind)&(-*Run_ind); // extract LSB bit *Run_ind &= ~*repeat; // this bit is used out *pattern = S_Pulse; *phase = 2U; break; case 2U: *repeat = 1U; *pattern = N_Pulse; *phase = 0U; break; } return refresh; } if(*VDU_ind != 0) { switch(*phase) { case 0U: *repeat = VDU_rep; *pattern = L_Pulse; *phase = 1U; break; case 1U: *repeat = (*VDU_ind)&(-*VDU_ind); // extract LSB bit *VDU_ind &= ~*repeat; // this bit is used out *pattern = S_Pulse; *phase = 2U; break; case 2U: *repeat = 1U; *pattern = N_Pulse; *phase = 0U; break; } return refresh; } // else all XXX_ind is cleared out, set refresh refresh = 1U; return refresh; } void Cooler(void) { //Cooling auto control, unfinish 2019/11/15 Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor); Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule); //2020/6/14 only for test use AWD_HMI if(RTD_SW) { Aux_Rly = 0; } else { Aux_Rly = 1; } } int16_t max_uval(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; } float max_fval(float i1, float i2, float i3, float i4) { float max = i1; if(i2 > max) max = i2; if(i3 > max) max = i3; if(i4 > max) max = i4; return max; }