SHENG-HEN HSIEH
/
VDU_2021
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "main.h" 00003 00004 // define this for newversion test mode 00005 //#define IMU_direct 00006 00007 #ifndef IMU_direct 00008 #include "imu_driver.hpp" 00009 #else 00010 typedef struct AhrsRawData { 00011 uint16_t status; 00012 int16_t rate[3]; 00013 int16_t accel[3]; 00014 uint16_t temperature; 00015 int16_t attitude[3]; 00016 } AhrsData; 00017 #define Read_VG (0x3D) 00018 #define ACCL2F (4000.0f) 00019 #define GYRO2F (100.0f) 00020 #define AHRS2F (90.0f) 00021 #define VG_len 11U 00022 #endif 00023 00024 #define pi 3.14159265359f 00025 #define d2r 0.01745329252f 00026 #define dt 0.01f 00027 00028 #define st2r 0.033f //steer signal to actual rad 00029 00030 DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active 00031 DigitalOut Fault_Ind(PC_12,0); //Indicate fault bt flashing, 1 active 00032 DigitalOut LED(D13, 0); //Internal LED output, general purpose 00033 AnalogIn AUX_1(PC_0); //Auxilaru analog sensor 00034 AnalogIn AUX_2(PC_3); 00035 AnalogIn AUX_3(PC_2); 00036 AnalogIn AUX_4(PC_1); 00037 AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection 00038 AnalogIn Brk_sense(PA_4); //Brake sensor readings 00039 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message 00040 SPI spi2(PB_15, PB_14, PB_13); //1Mbps default, MOSI MISO SCLK, forIMU 00041 #ifndef IMU_direct 00042 ImuDriver <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg); //SPI instance, reset, data ready, slave select 00043 #else 00044 AhrsData ahrsdata; 00045 DigitalOut cs(PB_1,1); 00046 InterruptIn drdy(PB_2); 00047 #endif 00048 Serial pc(USBTX, USBRX, 115200); 00049 Ticker ticker1; //100Hz task 00050 CANMessage can_msg_Rx; 00051 CANMessage can_msg_Tx; 00052 00053 void timer1_interrupt(void) 00054 { 00055 HSTick += 1; 00056 LSTick += 1; 00057 if (HSTick > 9) { // 100Hz 00058 HST_EXFL = 1; 00059 HSTick = 0; 00060 } 00061 if (LSTick > 99) { // 10Hz 00062 LST_EXFL = 1; 00063 LSTick = 0; 00064 } 00065 } 00066 00067 int main() 00068 { 00069 //Init CAN network 00070 CAN_init(); // Note now in Gloable test mode only for testing 2019/11/17 00071 00072 //Start House keeping task 00073 printf("VDU start up, pend for module online\n"); 00074 #ifdef IMU_direct 00075 drdy.fall(&IMU_isr); //IMU interrupt service 00076 spi2.format(16, 3); 00077 //spi2.frequency(); //As default 00078 #endif 00079 00080 wait_ms(100); 00081 ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick 00082 while(1) { 00083 00084 // Do high speed loop 00085 if (HST_EXFL == 1) { 00086 HST_EXFL = 0; 00087 00088 // Get IMU, Auxs, Max Temperature 00089 Cooler(); 00090 IMU_read(); 00091 Aux_read(); 00092 Module_WD(); 00093 00094 // Run state machine 00095 switch (VDU_STAT) { 00096 00097 case VDU_PowerOn: 00098 /* Power on state 00099 * Description: 00100 * Simple start up sequence will be done here 00101 * Do: 00102 * VDU internal POST 00103 * Wait till modules + PSU online 00104 * To VDU_Idle (RTD off): 00105 * Prepare for 4WD main program 00106 * To VDU_Fault: 00107 * Run the error handling service 00108 */ 00109 00110 //Basic IMU test 00111 POST(); 00112 // printf("%d,%d,%d,%d,%d\n",FL_online,FR_online,RL_online,RR_online,PSU_online); 00113 //Check if state transition only when all module online 00114 if (VDU_FLT != 0) { //Check if any error 00115 VDU_STAT = VDU_Fault; 00116 RST_HMI = 0; //Ensure new RST action after error 00117 printf("POST Fault\n"); 00118 FLT_print = 1; 00119 } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) { 00120 // } else if (1) { //2019/11/30 only use for force online debug 00121 //All module online & POST pass 00122 VDU_STAT = VDU_Idle; 00123 printf("All module online, VDU now Idle\n"); 00124 } //Else keep in state VDU_PowerOn 00125 break; 00126 00127 case VDU_Idle: 00128 /* Controller Idle state 00129 * Description: 00130 * Normal latched state, wait for RTD_HMI set from PSU 00131 * 4WD in running but output mask to 0 00132 * Do: 00133 * 4WD controller 00134 * Check: 00135 * RUN faults if any 00136 * To VDU_Run: 00137 * Initialize parameters for start up, set RTD_cmd 00138 * To VDU_Fault: 00139 * Run the error handling service 00140 */ 00141 00142 //Forced RTD_HMI for debug purpose 2019/11/14 00143 // RTD_HMI = 1; //Should be set if can bus received data 00144 //Forced RTD_HMI for debug purpose 2019/11/14 00145 00146 RUNT(); //Run test 00147 AWD(); //AWD main program 00148 00149 if (VDU_FLT != 0) { //Check if any error 00150 VDU_STAT = VDU_Fault; 00151 RST_HMI = 0; //Ensure new RST action after error 00152 printf("Idle 2 Fault\n"); 00153 FLT_print = 1; 00154 } else if (RTD_HMI != 0) { //Or command to run threw PSU 00155 //Prepare to send out RTD and start motor 00156 Idle2Run(); 00157 VDU_STAT = VDU_Run; 00158 printf("Idle 2 Run\n"); 00159 } //Else keep in state 00160 break; 00161 00162 case VDU_Run: 00163 /* Controller Run state 00164 * Description: 00165 * Normal latched state, after RTD_HMI is set from PSU 00166 * Same to Idle state except RTD_cmd is set 00167 * Do: 00168 * 4WD controller 00169 * Check: 00170 * RUN faults if any 00171 * To VDU_Idle: 00172 * Initialize parameters for idling, reset RTD_cmd 00173 * To VDU_Fault: 00174 * Run the error handling service 00175 */ 00176 00177 RUNT(); //Run test 00178 AWD(); //AWD main program 00179 00180 //Temporary debug posting area 2019/11/14 00181 //printf("%d,%d\n", Encoder_cnt, Encoder_del); 00182 //printf("%d\n\r", (int16_t)Tmodule);// 00183 //printf("%d\n\r", (int16_t)Vbus); 00184 00185 if (VDU_FLT != 0) { //Check if any error 00186 VDU_STAT = VDU_Fault; 00187 RST_HMI = 0; //Ensure new RST action after error 00188 printf("Run 2 Fault\n"); 00189 FLT_print = 1; 00190 } else if (RTD_HMI != 1) { //Or command to stop threw can bus 00191 Run2Idle(); 00192 VDU_STAT = VDU_Idle; 00193 printf("Run 2 Idle\n"); 00194 } //Else keep in state 00195 break; 00196 00197 case VDU_Fault: 00198 /* Controller Fault state 00199 * Description: 00200 * Fault latched state if any faults is detected 00201 * Same to Idle state except keep till RTD_HMI reset 00202 * Do: 00203 * Nothing, like a piece of shit 00204 * Check: 00205 * RUN faults if any 00206 * To VDU_PowerOn: 00207 * Restart VDU 00208 */ 00209 00210 RUNT(); //Run test 00211 00212 if (RST_HMI == 1) { //PSU reset to clear error 00213 RST_HMI = 0; 00214 RST_cmd = 1; 00215 FLT_print = 0; //Stop error printing 00216 // FL_online = 5; // 0 indicate detection timeout 00217 // FR_online = 5; // reset value is 3 to hold for 0.03sec 00218 // RL_online = 5; // -1 per 100Hz task 00219 // RR_online = 5; 00220 // PSU_online = 5; 00221 VDU_FLT = 0; 00222 VDU_STAT = VDU_Reset; 00223 printf("VDU rebooting...\n"); 00224 printf("QDrive rebooting...\n"); 00225 } //Else keep in state 00226 break; 00227 00228 case VDU_Reset: 00229 /* Controller Reset state 00230 * Description: 00231 * A state after reset by HMI when Fault latched 00232 * Wait till four driver processing, timeout protected 00233 * Do: 00234 * Nothing, just a soft delay 00235 * Check: 00236 * Timeout condition met 00237 * To VDU_Idle: 00238 * A valid reset 00239 * To VDU_Fault: 00240 * A fail reset 00241 */ 00242 00243 RUNT(); //Run test 00244 if(!RL_DSM) { 00245 // if(!(FL_DSM|FR_DSM|RL_DSM|RR_DSM)) { // 2020/3/13 for real case 00246 printf("...\n"); 00247 VDU_FLT &= ~(DSM_VDUFLTCode); //Clear if fine 00248 } 00249 00250 Reset_to += 1; //Time out check 00251 if (Reset_to > 30) { 00252 Reset_to = 0; 00253 if (VDU_FLT != 0) { //Check if any error 00254 VDU_STAT = VDU_Fault; //Back to fault state wait for next reset 00255 printf("Reset fail 2 Fault\n"); 00256 FLT_print = 1; 00257 } else { //A success reset 00258 VDU_STAT = VDU_Idle; 00259 printf("Reset ok 2 Idle\n"); 00260 } 00261 } //Else keep in state 00262 break; 00263 } 00264 00265 // Shit out torque distribution and special command 00266 if(VDU_STAT == VDU_Run) { 00267 //Allow output torque 00268 Tx_Tcmd_CAN1(); 00269 } else if(RST_cmd) { 00270 //Send out reset cmd once 00271 Tx_CLRerr_CAN1(); 00272 } else { 00273 //Force RTD off when not in VDU_Run 00274 Tx_Estop_CAN1(); 00275 } 00276 00277 //2019/12/18 Add here to test IMU, newer version use inturrupt get data 00278 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.ahrsProcessedData.attitude[0], imu.ahrsProcessedData.attitude[1], imu.ahrsProcessedData.attitude[2]); 00279 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]); 00280 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.accel[0], imu.imuProcessedData.accel[1], imu.imuProcessedData.accel[2]); 00281 // pc.printf("%.3f,%.3f,%.3f\n\r", YR_imu, Ax_imu, Ay_imu); 00282 // pc.printf("%.3f,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu); 00283 // pc.printf("%.3f\n\r", Trq_HMI*100.0f); 00284 // pc.printf("%.3f\n\r", Steer_HMI); 00285 // pc.printf("%.1f\n\r", Vx_wss); 00286 00287 } 00288 // End of high speed loop 00289 00290 // Do low speed state reporting 10 Hz 00291 if (LST_EXFL == 1) { 00292 LST_EXFL = 0; 00293 //Cooling 00294 Cooler(); 00295 00296 //Print low speed data on CAN 00297 Tx_Qdrv_CAN1(); 00298 00299 // Print out error mesagge if exist, 0.5Hz repeative 00300 if(FLT_print != 0) { 00301 //Merge Faults 00302 00303 //USE THIS FOR FULL FUNCTION 00304 FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post; 00305 FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run; 00306 00307 //ONLY FOR TEST TODO 00308 // FLT_Post = RL_FLT_Post; // Use only for single module test 00309 // FLT_Run = RL_FLT_Run; //2020/3/10 00310 00311 //UART 00312 FLT_print_cnt += FLT_print; 00313 if(FLT_print_cnt > 19) { 00314 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); 00315 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); 00316 if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT); 00317 00318 //Only temperature printing 2020/6/30 00319 printf("Tmotor FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor); 00320 printf("Tmodule FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule); 00321 00322 FLT_print_cnt = 0; 00323 } 00324 00325 //LED 00326 if(Ind_refresh) { 00327 // Refresh data for LED indication after run threw 00328 FLT_Post_ind = FLT_Post; 00329 FLT_Run_ind = FLT_Run; 00330 VDU_FLT_ind = VDU_FLT; 00331 Ind_refresh = 0; // Set after run threw all error bits 00332 } 00333 } else { 00334 // Clear & stop LED when no faults 00335 FLT_Post_ind = 0; 00336 FLT_Run_ind = 0; 00337 VDU_FLT_ind = 0; 00338 Repeat = 0U; 00339 Phase = 0U; 00340 Blk_n = 0U; 00341 } 00342 00343 // Blinky output 00344 if (VDU_STAT != VDU_PowerOn) { 00345 // Case after poweron (all module online) or fault(s) occur 00346 Ind_refresh = IndicationKernel( 00347 &Pattern, 00348 &Repeat, 00349 &Phase, 00350 &FLT_Post_ind, 00351 &FLT_Run_ind, 00352 &VDU_FLT_ind); 00353 LED = Indication(Pattern, &Repeat, &Blk_n); 00354 Fault_Ind = LED; 00355 } else { 00356 // Case no fault while waiting for all module online 00357 LED = !LED; //Fast 5Hz blinky indicate the activity 00358 Fault_Ind = LED; 00359 } 00360 00361 00362 } 00363 // End of low speed state reporting 00364 00365 } // end of while 00366 } 00367 00368 void Idle2Run(void) 00369 { 00370 RTD_cmd = 1; 00371 } 00372 00373 void Run2Idle(void) 00374 { 00375 RTD_cmd = 0; 00376 } 00377 00378 void POST(void) 00379 { 00380 //Check IMU status abnormal 00381 // if(fabsf(YR_imu) > 250) { //half turn per sec, strange 00382 // VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error 00383 // } 00384 } 00385 00386 void RUNT(void) 00387 { 00388 //POST 00389 POST(); 00390 00391 //Check module response timeout 00392 if((FL_online*FR_online*RL_online*RR_online) == 0) { 00393 VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout 00394 } 00395 if(PSU_online == 0) { 00396 VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout 00397 } 00398 00399 //Check ShutDrv voltage when running 00400 if(VDU_STAT == VDU_Run) { 00401 if(SDn_voltage < 8.0f) { 00402 //2020/4/5 TODO remove in real case 00403 VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv 00404 } 00405 } 00406 00407 //Check IMU status abnormal add in 2020/10/07 00408 if(fabsf(YR_imu) > 250.0f) { //half turn per sec, strange 00409 VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error 00410 } 00411 } 00412 00413 void Aux_read(void) 00414 { 00415 //Capture analog in at once imu_driver ver 00416 AUX_1_u16 = AUX_1.read_u16() >> 4U; 00417 AUX_2_u16 = AUX_2.read_u16() >> 4U; 00418 AUX_3_u16 = AUX_3.read_u16() >> 4U; 00419 AUX_4_u16 = AUX_4.read_u16() >> 4U; 00420 SDn_voltage = 18.81f*SDn_sense.read(); //Read in Shutdown circuit voltage in output end 00421 Brk_voltage = 5.5f*Brk_sense.read(); 00422 } 00423 00424 #ifdef IMU_direct 00425 void IMU_isr(void) 00426 { 00427 //Start data transfer 00428 uint8_t data_rx = 0; 00429 uint16_t reg_VG = Read_VG<<8U; 00430 cs = 0; 00431 spi2.write(reg_VG); 00432 while(data_rx < VG_len) { 00433 uint16_t spi_data = spi2.write(0x0000); 00434 switch(data_rx) { 00435 case 0: 00436 ahrsdata.status = spi_data; 00437 break; 00438 case 1: 00439 case 2: 00440 case 3: 00441 ahrsdata.rate[data_rx - 1] = spi_data; 00442 break; 00443 case 4: 00444 case 5: 00445 case 6: 00446 ahrsdata.accel[data_rx - 4] = spi_data; 00447 break; 00448 case 7: 00449 ahrsdata.temperature = spi_data; 00450 break; 00451 case 8: 00452 case 9: 00453 case 10: 00454 ahrsdata.attitude[data_rx - 8] = spi_data; 00455 break; 00456 default: 00457 break; 00458 } 00459 ++data_rx; 00460 } 00461 cs = 1; 00462 } 00463 #endif 00464 00465 void IMU_read(void) 00466 { 00467 #ifndef IMU_direct 00468 //Get readings threw back ground, direction not checked 2019/12/20 00469 // Direction checked 2020/10/29 00470 YR_imu = imu.imuProcessedData.rate[2]; 00471 Ax_imu = imu.imuProcessedData.accel[0]; 00472 Ay_imu = imu.imuProcessedData.accel[1]; 00473 Roll_imu = imu.ahrsProcessedData.attitude[0]; 00474 Pitch_imu = imu.ahrsProcessedData.attitude[1]; 00475 Yaw_imu = imu.ahrsProcessedData.attitude[2]; 00476 #else 00477 YR_imu = ahrsdata.rate[2]/GYRO2F; 00478 Ax_imu = ahrsdata.accel[0]/ACCL2F; 00479 Ay_imu = ahrsdata.accel[1]/ACCL2F; 00480 Roll_imu = ahrsdata.attitude[0]/AHRS2F; 00481 Pitch_imu = ahrsdata.attitude[1]/AHRS2F; 00482 #endif 00483 } 00484 00485 void AWD(void) 00486 { 00487 float Tayc_tmp = 0; //active part of yaw control system 00488 float Tlsd_tmp = 0; //limit slip differetial torque 00489 float YdelW_ele = 0; //Ideal L-R electric speed difference 00490 00491 //Get estimate from wheel only 00492 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 00493 00494 //Simple comp filter is fine without GSS onboard 00495 Vx_fil += (Ax_imu-0.01f)*9.807f*0.01f; //-0.01 here is to be calibrated 00496 Vx_fil = Vx_fil*0.98f + Vx_wss*0.02f; //0.98, 0.02 is coefficient 00497 00498 if(AWD_HMI) { 00499 // A simple version is put here for reading 00500 //YR_ref = Steer_HMI*st2r*Vb_est/(Base+EG*Vb_est*Vb_est); 00501 00502 /*Still in test section, ignore*/ 00503 //sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0); 00504 //FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig; 00505 //FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig; 00506 //RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig); 00507 //RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig); 00508 00509 /*A basic static distribution*/ 00510 FL_Tcmd = 0.11f*Trq_HMI; 00511 FR_Tcmd = 0.11f*Trq_HMI; 00512 RL_Tcmd = 0.25f*Trq_HMI; 00513 RR_Tcmd = 0.25f*Trq_HMI; 00514 00515 /*A basic Yaw control*/ 00516 YR_ref = (Steer_HMI*st2r)*Vx_fil/Base; // Center calibration moved to can recieve 00517 Tayc_tmp = (YR_ref - YR_imu*d2r)*10.0f; // map 1 rad/s YR difference to ~ 10Nm 00518 Tayc_tmp = constrain(Tayc_tmp,-5.0f,5.0f); 00519 FL_Tcmd += -Tayc_tmp; 00520 FR_Tcmd += Tayc_tmp; 00521 RL_Tcmd += -Tayc_tmp; 00522 RR_Tcmd += Tayc_tmp; 00523 00524 /*A basic ideal differential controller*/ 00525 YdelW_ele = YR_ref*Track/0.235f*11.0f*4.0f; // dir, rate to speed, wheel radius, reducer, pole pair 00526 //YdelW_ele > 0 when left turning 00527 00528 /*A basic static distribution + differential for test 2020/7/19*/ 00529 //Front differential 00530 Tlsd_tmp = (FL_W_ele - FR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm 00531 Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f); 00532 FL_Tcmd += -Tlsd_tmp; 00533 FR_Tcmd += Tlsd_tmp; 00534 00535 00536 //Rear differential 00537 Tlsd_tmp = (RL_W_ele - RR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm 00538 Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f); 00539 RL_Tcmd += -Tlsd_tmp; 00540 RR_Tcmd += Tlsd_tmp; 00541 00542 } else { 00543 //2020/8/19 for fast test pure rear 00544 //2021/3/4 change front max to 11Nm to test? 00545 Tlsd_tmp = (FL_W_ele - FR_W_ele)*0.01f; // map 1000 rad/s difference to ~ 10Nm 00546 Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f); 00547 if(Tlsd_tmp>0.0f) { //L > R 00548 FL_Tcmd = 0.11f*Trq_HMI - Tlsd_tmp; 00549 FR_Tcmd = 0.11f*Trq_HMI; 00550 } else { //L < R, Tlsd_tmp < 0 00551 FL_Tcmd = 0.11f*Trq_HMI; 00552 FR_Tcmd = 0.11f*Trq_HMI + Tlsd_tmp; 00553 } 00554 //Rear differential 00555 Tlsd_tmp = (RL_W_ele - RR_W_ele)*0.005f; // map 1000 rad/s difference to ~ 5Nm 00556 Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f); 00557 if(Tlsd_tmp>0.0f) { //L > R 00558 RL_Tcmd = 0.25f*Trq_HMI - Tlsd_tmp; 00559 RR_Tcmd = 0.25f*Trq_HMI; 00560 } else { //L < R, Tlsd_tmp < 0 00561 RL_Tcmd = 0.25f*Trq_HMI; 00562 RR_Tcmd = 0.25f*Trq_HMI + Tlsd_tmp; 00563 } 00564 00565 00566 00567 // // A basic static distribution 2020/7/19 00568 // FL_Tcmd = 0.15f*Trq_HMI; 00569 // FR_Tcmd = 0.15f*Trq_HMI; 00570 // RL_Tcmd = 0.25f*Trq_HMI; 00571 // RR_Tcmd = 0.25f*Trq_HMI; 00572 }//last line of: if(AWD_HMI){} 00573 00574 ////Add to force normal drive 00575 // FL_Tcmd = 0.25f*Trq_HMI; 00576 // FR_Tcmd = 0.25f*Trq_HMI; 00577 // RL_Tcmd = 0.25f*Trq_HMI; 00578 // RR_Tcmd = 0.25f*Trq_HMI; 00579 00580 //Add to force rear drive 00581 // FL_Tcmd = 0.2f*Trq_HMI; 00582 // FR_Tcmd = 0.2f*Trq_HMI; 00583 // RL_Tcmd = 0.5f*Trq_HMI; 00584 // RR_Tcmd = 0.5f*Trq_HMI; 00585 00586 //Direction define, move to can sendout 00587 // FL_Tcmd = -1.0f*FL_Tcmd; 00588 // FR_Tcmd = 1.0f*FR_Tcmd; 00589 // RL_Tcmd = -1.0f*RL_Tcmd; 00590 // RR_Tcmd = 1.0f*RR_Tcmd; 00591 00592 //Add to force only one motor drive (DYNO) 00593 // FL_Tcmd = 0.0f*Trq_HMI; 00594 // FR_Tcmd = 0.0f*Trq_HMI; 00595 // RL_Tcmd = 0.1f*Trq_HMI; 00596 // RR_Tcmd = 0.0f*Trq_HMI; 00597 00598 } 00599 00600 void ASL(void) 00601 { 00602 //Filter out each motor W_ele and get approximate accel, compare with IMU 00603 //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel) 00604 //Fill out if enough time 00605 } 00606 00607 void Rx_CAN1(void) 00608 { 00609 // LED = 1; 00610 int16_t tmp; 00611 00612 if(can1.read(can_msg_Rx)) { 00613 switch(can_msg_Rx.id) { //Filtered input message 00614 // Start of 100Hz msg 00615 case FL_HSB_ID://1 00616 //HSB from FL motor drive 00617 FL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT 00618 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00619 FL_W_ele = tmp*-1.0f; 00620 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00621 FL_Trq_fil3 = tmp * -0.01f; 00622 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00623 FL_Trq_est = tmp * -0.01f; 00624 FL_online = 5; 00625 //If fault 00626 if(FL_DSM == 3U) { 00627 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault 00628 } 00629 break; 00630 00631 case FR_HSB_ID://2 00632 //HSB from FR motor drive 00633 FR_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT 00634 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00635 FR_W_ele = tmp*1.0f; 00636 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00637 FR_Trq_fil3 = tmp * 0.01f; 00638 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00639 FR_Trq_est = tmp * 0.01f; 00640 FR_online = 5; 00641 if(FR_DSM == 3U) { 00642 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault 00643 } 00644 break; 00645 00646 case RL_HSB_ID://3 00647 //HSB from RL motor drive 00648 RL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT 00649 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00650 RL_W_ele = tmp*-1.0f; 00651 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00652 RL_Trq_fil3 = tmp * -0.01f; 00653 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00654 RL_Trq_est = tmp * -0.01f; 00655 RL_online = 5; 00656 if(RL_DSM == 3U) { 00657 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault 00658 } 00659 break; 00660 00661 case RR_HSB_ID://4 00662 //HSB from RR motor drive 00663 RR_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT 00664 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00665 RR_W_ele = tmp*1.0f; 00666 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00667 RR_Trq_fil3 = tmp * 0.01f; 00668 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00669 RR_Trq_est = tmp * 0.01f; 00670 RR_online = 5; 00671 if(RR_DSM == 3U) { 00672 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault 00673 } 00674 break; 00675 00676 case HMI_cmd_ID://5 00677 //HMI command from PSU 00678 AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch 00679 RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request 00680 RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch 00681 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00682 Steer_HMI = tmp * 0.01f - 0.0f; //0.7f here is to calibrated center 00683 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00684 Trq_HMI = tmp * 0.01f; 00685 PSU_online = 5; 00686 break; 00687 // end of 100Hz msg 00688 00689 // Start of 10Hz msg 00690 case FL_LSB_ID://6 00691 //LSB from FL motor drive 00692 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6]; 00693 FL_Tmotor = tmp*0.01f; 00694 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00695 FL_Tmodule = tmp*0.01f; 00696 FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00697 FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00698 break; 00699 00700 case FR_LSB_ID://7 00701 //LSB from FR motor drive 00702 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6]; 00703 FR_Tmotor = tmp*0.01f; 00704 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00705 FR_Tmodule = tmp*0.01f; 00706 FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00707 FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00708 break; 00709 00710 case RL_LSB_ID://8 00711 //LSB from RL motor drive 00712 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6]; 00713 RL_Tmotor = tmp*0.01f; 00714 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00715 RL_Tmodule = tmp*0.01f; 00716 RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00717 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00718 break; 00719 00720 case RR_LSB_ID://9 00721 //LSB from RR motor drive 00722 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6]; 00723 RR_Tmotor = tmp*0.01f; 00724 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4]; 00725 RR_Tmodule = tmp*0.01f; 00726 RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2]; 00727 RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0]; 00728 break; 00729 00730 case PedalStat_ID://10 00731 RTD_SW = 0x01&can_msg_Rx.data[1]; 00732 break; 00733 // end of 10Hz msg 00734 } 00735 } 00736 // LED = 0; 00737 } 00738 00739 void Tx_CLRerr_CAN1(void) 00740 { 00741 Tx_Estop_CAN1(); //disable as default 00742 RST_cmd = 0; //clear out one shot 00743 } 00744 00745 void Tx_Estop_CAN1(void) 00746 { 00747 RTD_cmd = 0; //force disable 00748 Tx_Tcmd_CAN1(); 00749 } 00750 00751 void Tx_Tcmd_CAN1(void) // 100 Hz 00752 { 00753 int16_t tmp; 00754 tmp = (int16_t) (FL_Tcmd * -100.0f); 00755 temp_msg[0] = tmp; 00756 temp_msg[1] = tmp >> 8U; 00757 temp_msg[2] = RTD_cmd; 00758 temp_msg[3] = RST_cmd; 00759 // temp_msg[3] = 0U; // 2020/3/4 add to disable HMI reseting Driver 00760 temp_msg[4] = 0U; 00761 temp_msg[5] = 0U; 00762 temp_msg[6] = 0U; 00763 temp_msg[7] = 0U; 00764 can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard); 00765 CANpendTX(); 00766 can1.write(can_msg_Tx); 00767 00768 tmp = (int16_t) (FR_Tcmd * 100.0f); 00769 temp_msg[0] = tmp; 00770 temp_msg[1] = tmp >> 8U; 00771 can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard); 00772 CANpendTX(); 00773 can1.write(can_msg_Tx); 00774 00775 tmp = (int16_t) (RL_Tcmd * -100.0f); 00776 temp_msg[0] = tmp; 00777 temp_msg[1] = tmp >> 8U; 00778 can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard); 00779 CANpendTX(); 00780 can1.write(can_msg_Tx); 00781 00782 tmp = (int16_t) (RR_Tcmd * 100.0f); 00783 temp_msg[0] = tmp; 00784 temp_msg[1] = tmp >> 8U; 00785 can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard); 00786 CANpendTX(); 00787 can1.write(can_msg_Tx); 00788 00789 // IMU attitude readings shitting out 00790 tmp = (int16_t) (YR_imu * 100.0f); 00791 temp_msg[0] = tmp; 00792 temp_msg[1] = tmp >> 8U; 00793 tmp = (int16_t) (Roll_imu * 100.0f); 00794 temp_msg[2] = tmp; 00795 temp_msg[3] = tmp >> 8U; 00796 tmp = (int16_t) (Pitch_imu * 100.0f); 00797 temp_msg[4] = tmp; 00798 temp_msg[5] = tmp >> 8U; 00799 temp_msg[6] = (int8_t)(Ax_imu * 50.0f); 00800 temp_msg[7] = (int8_t)(Ay_imu * 50.0f); 00801 can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard); 00802 CANpendTX(); 00803 can1.write(can_msg_Tx); 00804 00805 // Some internal control variables shitting out 00806 tmp = (int16_t) (Vx_fil * 100.0f); 00807 temp_msg[0] = tmp; 00808 temp_msg[1] = tmp >> 8U; 00809 can_msg_Tx = CANMessage(RegularVar_ID,temp_msg,2,CANData,CANStandard); 00810 CANpendTX(); 00811 can1.write(can_msg_Tx); 00812 } 00813 00814 void Tx_Qdrv_CAN1(void) // 10 Hz 00815 { 00816 //int16_t tmp; 00817 // Auxilary sensor reading shitting out 00818 temp_msg[0] = AUX_1_u16; 00819 temp_msg[1] = AUX_1_u16 >> 8U; 00820 temp_msg[2] = AUX_2_u16; 00821 temp_msg[3] = AUX_2_u16 >> 8U; 00822 temp_msg[4] = AUX_3_u16; 00823 temp_msg[5] = AUX_3_u16 >> 8U; 00824 temp_msg[6] = AUX_4_u16; 00825 temp_msg[7] = AUX_4_u16 >> 8U; 00826 can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard); 00827 CANpendTX(); 00828 can1.write(can_msg_Tx); 00829 00830 // Qdrive internal state shitting out 00831 temp_msg[0] = VDU_FLT; 00832 temp_msg[1] = VDU_FLT >> 8U; 00833 temp_msg[2] = VDU_STAT; 00834 temp_msg[3] = (int8_t)(SDn_voltage*10.0f); 00835 temp_msg[4] = (int8_t)(Brk_voltage*50.0f); 00836 temp_msg[5] = 0U; 00837 temp_msg[6] = 0U; 00838 temp_msg[7] = 0U; 00839 can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard); 00840 CANpendTX(); 00841 can1.write(can_msg_Tx); 00842 } 00843 00844 void CANpendTX(void) 00845 { 00846 //Pend till TX box has empty slot, timeout will generate error 00847 uint32_t timeout = 0; 00848 while(!(CAN1->TSR & CAN_TSR_TME_Msk)) { 00849 //Wait till non empty 00850 timeout += 1; 00851 if(timeout > 10000) { 00852 // Put some timeout error handler 00853 break; 00854 } 00855 } 00856 } 00857 00858 void CAN_init(void) 00859 { 00860 //Set CAN system 00861 SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off 00862 can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode 00863 can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1); 00864 can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2); 00865 can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3); 00866 can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4); 00867 can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5); 00868 can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6); 00869 can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7); 00870 can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring 00871 can1.filter(PedalStat_ID,0xFFFF,CANStandard,9); // PSU online monitoring 00872 // can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 00873 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq 00874 } 00875 00876 void Module_WD(void) 00877 { 00878 //Module online dissipitive indicator 00879 if (FL_online != 0) { 00880 FL_online -= 1; 00881 } 00882 if (FR_online != 0) { 00883 FR_online -= 1; 00884 } 00885 if (RL_online != 0) { 00886 RL_online -= 1; 00887 } 00888 if (RR_online != 0) { 00889 RR_online -= 1; 00890 } 00891 if (PSU_online != 0) { 00892 PSU_online -= 1; 00893 } 00894 } 00895 uint8_t Indication( // Blink indicator in pattern * repeat 00896 uint8_t pattern, 00897 uint16_t*repeat, 00898 uint8_t*blk_n) 00899 { 00900 uint8_t out = 0; 00901 if(*repeat==0) return out; // reject repeat = 0 case, out = 0 00902 out = (pattern>>(*blk_n)) & 1U; // blink from LSB to MSB 00903 if(*blk_n < 7U) { 00904 *blk_n += 1U; 00905 } else { // a full pattern was passed 00906 *blk_n = 0U; 00907 *repeat >>= 1U; 00908 } 00909 return out; 00910 } 00911 00912 uint8_t IndicationKernel( // Generate blink pattern, return 1 if all ind cleared 00913 uint8_t*pattern, 00914 uint16_t*repeat, 00915 uint8_t*phase, 00916 uint16_t*Post_ind, 00917 uint16_t*Run_ind, 00918 uint16_t*VDU_ind) 00919 { 00920 //Blink indicator in pattern 00921 //If FLT_Run = 0b0010-0110, pattern will be --......--...--.. 00922 uint8_t refresh = 0; 00923 if(*repeat!=0) return refresh; // skip straight to Indication() 00924 00925 if(*Post_ind != 0) { 00926 switch(*phase) { 00927 case 0U: 00928 *repeat = Post_rep; 00929 *pattern = L_Pulse; 00930 *phase = 1U; 00931 break; 00932 00933 case 1U: 00934 *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit 00935 *Post_ind &= ~*repeat; // this bit is used out 00936 *pattern = S_Pulse; 00937 *phase = 2U; 00938 break; 00939 00940 case 2U: 00941 *repeat = 1U; 00942 *pattern = N_Pulse; 00943 *phase = 0U; 00944 break; 00945 } 00946 return refresh; 00947 } 00948 00949 if(*Run_ind != 0) { 00950 switch(*phase) { 00951 case 0U: 00952 *repeat = Run_rep; 00953 *pattern = L_Pulse; 00954 *phase = 1U; 00955 break; 00956 00957 case 1U: 00958 *repeat = (*Run_ind)&(-*Run_ind); // extract LSB bit 00959 *Run_ind &= ~*repeat; // this bit is used out 00960 *pattern = S_Pulse; 00961 *phase = 2U; 00962 break; 00963 00964 case 2U: 00965 *repeat = 1U; 00966 *pattern = N_Pulse; 00967 *phase = 0U; 00968 break; 00969 } 00970 return refresh; 00971 } 00972 00973 if(*VDU_ind != 0) { 00974 switch(*phase) { 00975 case 0U: 00976 *repeat = VDU_rep; 00977 *pattern = L_Pulse; 00978 *phase = 1U; 00979 break; 00980 00981 case 1U: 00982 *repeat = (*VDU_ind)&(-*VDU_ind); // extract LSB bit 00983 *VDU_ind &= ~*repeat; // this bit is used out 00984 *pattern = S_Pulse; 00985 *phase = 2U; 00986 break; 00987 00988 case 2U: 00989 *repeat = 1U; 00990 *pattern = N_Pulse; 00991 *phase = 0U; 00992 break; 00993 } 00994 return refresh; 00995 } 00996 00997 // else all XXX_ind is cleared out, set refresh 00998 refresh = 1U; 00999 return refresh; 01000 } 01001 01002 void Cooler(void) 01003 { 01004 //Cooling auto control, unfinish 2019/11/15 01005 Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor); 01006 Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule); 01007 01008 //2020/6/14 only for test use AWD_HMI 01009 if(RTD_SW) { 01010 Aux_Rly = 0; 01011 } else { 01012 Aux_Rly = 1; 01013 } 01014 } 01015 01016 int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4) 01017 { 01018 int16_t max = i1; 01019 if(i2 > max) max = i2; 01020 if(i3 > max) max = i3; 01021 if(i4 > max) max = i4; 01022 return max; 01023 } 01024 01025 float max_fval(float i1, float i2, float i3, float i4) 01026 { 01027 float max = i1; 01028 if(i2 > max) max = i2; 01029 if(i3 > max) max = i3; 01030 if(i4 > max) max = i4; 01031 return max; 01032 }
Generated on Fri Jul 15 2022 08:02:28 by
1.7.2