SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@26:ad4fbceeb4ae, 2021-02-21 (annotated)

Committer:
open4416
Date:
Sun Feb 21 04:25:00 2021 +0000
Revision:
26:ad4fbceeb4ae
Parent:
25:3c6e83b449b2
Child:
27:52260996ce32
Add DYNO part

Who changed what in which revision?

UserRevisionLine numberNew contents of line
open4416 0:c4747ebbe0b4 1 #include "mbed.h"
open4416 8:f8b1402c8f3c 2 #include "main.h"
open4416 19:d68f21173c23 3
open4416 19:d68f21173c23 4 // define this for newversion test mode
open4416 19:d68f21173c23 5 //#define IMU_direct
open4416 19:d68f21173c23 6
open4416 19:d68f21173c23 7 #ifndef IMU_direct
open4416 9:c99eeafa6fa3 8 #include "imu_driver.hpp"
open4416 19:d68f21173c23 9 #else
open4416 19:d68f21173c23 10 typedef struct AhrsRawData {
open4416 19:d68f21173c23 11 uint16_t status;
open4416 19:d68f21173c23 12 int16_t rate[3];
open4416 19:d68f21173c23 13 int16_t accel[3];
open4416 19:d68f21173c23 14 uint16_t temperature;
open4416 19:d68f21173c23 15 int16_t attitude[3];
open4416 19:d68f21173c23 16 } AhrsData;
open4416 19:d68f21173c23 17 #define Read_VG (0x3D)
open4416 19:d68f21173c23 18 #define ACCL2F (4000.0f)
open4416 19:d68f21173c23 19 #define GYRO2F (100.0f)
open4416 19:d68f21173c23 20 #define AHRS2F (90.0f)
open4416 19:d68f21173c23 21 #define VG_len 11U
open4416 19:d68f21173c23 22 #endif
open4416 19:d68f21173c23 23
open4416 8:f8b1402c8f3c 24 #define pi 3.14159265359f
open4416 8:f8b1402c8f3c 25 #define d2r 0.01745329252f
open4416 2:c7a3a8c1aeed 26 #define dt 0.01f
open4416 6:fbe401163489 27
open4416 25:3c6e83b449b2 28 #define st2r 0.033f //steer signal to actual rad
open4416 25:3c6e83b449b2 29
open4416 5:8116016abee0 30 DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
open4416 9:c99eeafa6fa3 31 DigitalOut Fault_Ind(PC_12,0); //Indicate fault bt flashing, 1 active
open4416 2:c7a3a8c1aeed 32 DigitalOut LED(D13, 0); //Internal LED output, general purpose
open4416 6:fbe401163489 33 AnalogIn AUX_1(PC_0); //Auxilaru analog sensor
open4416 13:ac024885d0bf 34 AnalogIn AUX_2(PC_3);
open4416 6:fbe401163489 35 AnalogIn AUX_3(PC_2);
open4416 6:fbe401163489 36 AnalogIn AUX_4(PC_1);
open4416 7:f674ef860c9c 37 AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection
open4416 13:ac024885d0bf 38 AnalogIn Brk_sense(PA_4); //Brake sensor readings
open4416 9:c99eeafa6fa3 39 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
open4416 9:c99eeafa6fa3 40 SPI spi2(PB_15, PB_14, PB_13); //1Mbps default, MOSI MISO SCLK, forIMU
open4416 19:d68f21173c23 41 #ifndef IMU_direct
open4416 14:bcf5cb4d08a5 42 ImuDriver <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg); //SPI instance, reset, data ready, slave select
open4416 19:d68f21173c23 43 #else
open4416 19:d68f21173c23 44 AhrsData ahrsdata;
open4416 19:d68f21173c23 45 DigitalOut cs(PB_1,1);
open4416 19:d68f21173c23 46 InterruptIn drdy(PB_2);
open4416 19:d68f21173c23 47 #endif
open4416 9:c99eeafa6fa3 48 Serial pc(USBTX, USBRX, 115200);
open4416 9:c99eeafa6fa3 49 Ticker ticker1; //100Hz task
open4416 5:8116016abee0 50 CANMessage can_msg_Rx;
open4416 5:8116016abee0 51 CANMessage can_msg_Tx;
open4416 2:c7a3a8c1aeed 52
open4416 2:c7a3a8c1aeed 53 void timer1_interrupt(void)
open4416 2:c7a3a8c1aeed 54 {
open4416 6:fbe401163489 55 HSTick += 1;
open4416 6:fbe401163489 56 LSTick += 1;
open4416 6:fbe401163489 57 if (HSTick > 9) { // 100Hz
open4416 6:fbe401163489 58 HST_EXFL = 1;
open4416 6:fbe401163489 59 HSTick = 0;
open4416 6:fbe401163489 60 }
open4416 6:fbe401163489 61 if (LSTick > 99) { // 10Hz
open4416 6:fbe401163489 62 LST_EXFL = 1;
open4416 6:fbe401163489 63 LSTick = 0;
open4416 6:fbe401163489 64 }
open4416 2:c7a3a8c1aeed 65 }
open4416 0:c4747ebbe0b4 66
open4416 0:c4747ebbe0b4 67 int main()
open4416 0:c4747ebbe0b4 68 {
open4416 6:fbe401163489 69 //Init CAN network
open4416 9:c99eeafa6fa3 70 CAN_init(); // Note now in Gloable test mode only for testing 2019/11/17
open4416 6:fbe401163489 71
open4416 6:fbe401163489 72 //Start House keeping task
open4416 6:fbe401163489 73 printf("VDU start up, pend for module online\n");
open4416 19:d68f21173c23 74 #ifdef IMU_direct
open4416 19:d68f21173c23 75 drdy.fall(&IMU_isr); //IMU interrupt service
open4416 19:d68f21173c23 76 spi2.format(16, 3);
open4416 19:d68f21173c23 77 //spi2.frequency(); //As default
open4416 19:d68f21173c23 78 #endif
open4416 21:e01a019fae2f 79
open4416 23:100661e2ad70 80 wait_ms(100);
open4416 6:fbe401163489 81 ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
open4416 6:fbe401163489 82 while(1) {
open4416 6:fbe401163489 83
open4416 6:fbe401163489 84 // Do high speed loop
open4416 6:fbe401163489 85 if (HST_EXFL == 1) {
open4416 6:fbe401163489 86 HST_EXFL = 0;
open4416 6:fbe401163489 87
open4416 9:c99eeafa6fa3 88 // Get IMU, Auxs, Max Temperature
open4416 9:c99eeafa6fa3 89 Cooler();
open4416 6:fbe401163489 90 IMU_read();
open4416 6:fbe401163489 91 Aux_read();
open4416 9:c99eeafa6fa3 92 Module_WD();
open4416 6:fbe401163489 93
open4416 6:fbe401163489 94 // Run state machine
open4416 6:fbe401163489 95 switch (VDU_STAT) {
open4416 6:fbe401163489 96
open4416 6:fbe401163489 97 case VDU_PowerOn:
open4416 6:fbe401163489 98 /* Power on state
open4416 6:fbe401163489 99 * Description:
open4416 6:fbe401163489 100 * Simple start up sequence will be done here
open4416 6:fbe401163489 101 * Do:
open4416 6:fbe401163489 102 * VDU internal POST
open4416 6:fbe401163489 103 * Wait till modules + PSU online
open4416 6:fbe401163489 104 * To VDU_Idle (RTD off):
open4416 6:fbe401163489 105 * Prepare for 4WD main program
open4416 6:fbe401163489 106 * To VDU_Fault:
open4416 6:fbe401163489 107 * Run the error handling service
open4416 6:fbe401163489 108 */
open4416 6:fbe401163489 109
open4416 6:fbe401163489 110 //Basic IMU test
open4416 6:fbe401163489 111 POST();
open4416 22:4f764f7cd0b3 112 // printf("%d,%d,%d,%d,%d\n",FL_online,FR_online,RL_online,RR_online,PSU_online);
open4416 6:fbe401163489 113 //Check if state transition only when all module online
open4416 14:bcf5cb4d08a5 114 if (VDU_FLT != 0) { //Check if any error
open4416 14:bcf5cb4d08a5 115 VDU_STAT = VDU_Fault;
open4416 20:e9daae390513 116 RST_HMI = 0; //Ensure new RST action after error
open4416 14:bcf5cb4d08a5 117 printf("POST Fault\n");
open4416 14:bcf5cb4d08a5 118 FLT_print = 1;
open4416 14:bcf5cb4d08a5 119 } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) {
open4416 14:bcf5cb4d08a5 120 // } else if (1) { //2019/11/30 only use for force online debug
open4416 14:bcf5cb4d08a5 121 //All module online & POST pass
open4416 14:bcf5cb4d08a5 122 VDU_STAT = VDU_Idle;
open4416 14:bcf5cb4d08a5 123 printf("All module online, VDU now Idle\n");
open4416 6:fbe401163489 124 } //Else keep in state VDU_PowerOn
open4416 6:fbe401163489 125 break;
open4416 6:fbe401163489 126
open4416 6:fbe401163489 127 case VDU_Idle:
open4416 6:fbe401163489 128 /* Controller Idle state
open4416 6:fbe401163489 129 * Description:
open4416 6:fbe401163489 130 * Normal latched state, wait for RTD_HMI set from PSU
open4416 6:fbe401163489 131 * 4WD in running but output mask to 0
open4416 6:fbe401163489 132 * Do:
open4416 6:fbe401163489 133 * 4WD controller
open4416 6:fbe401163489 134 * Check:
open4416 6:fbe401163489 135 * RUN faults if any
open4416 6:fbe401163489 136 * To VDU_Run:
open4416 6:fbe401163489 137 * Initialize parameters for start up, set RTD_cmd
open4416 6:fbe401163489 138 * To VDU_Fault:
open4416 6:fbe401163489 139 * Run the error handling service
open4416 6:fbe401163489 140 */
open4416 6:fbe401163489 141
open4416 6:fbe401163489 142 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 13:ac024885d0bf 143 // RTD_HMI = 1; //Should be set if can bus received data
open4416 6:fbe401163489 144 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 6:fbe401163489 145
open4416 6:fbe401163489 146 RUNT(); //Run test
open4416 6:fbe401163489 147 AWD(); //AWD main program
open4416 1:8a9ac822aab7 148
open4416 14:bcf5cb4d08a5 149 if (VDU_FLT != 0) { //Check if any error
open4416 6:fbe401163489 150 VDU_STAT = VDU_Fault;
open4416 20:e9daae390513 151 RST_HMI = 0; //Ensure new RST action after error
open4416 7:f674ef860c9c 152 printf("Idle 2 Fault\n");
open4416 6:fbe401163489 153 FLT_print = 1;
open4416 6:fbe401163489 154 } else if (RTD_HMI != 0) { //Or command to run threw PSU
open4416 6:fbe401163489 155 //Prepare to send out RTD and start motor
open4416 6:fbe401163489 156 Idle2Run();
open4416 6:fbe401163489 157 VDU_STAT = VDU_Run;
open4416 6:fbe401163489 158 printf("Idle 2 Run\n");
open4416 6:fbe401163489 159 } //Else keep in state
open4416 6:fbe401163489 160 break;
open4416 6:fbe401163489 161
open4416 6:fbe401163489 162 case VDU_Run:
open4416 6:fbe401163489 163 /* Controller Run state
open4416 6:fbe401163489 164 * Description:
open4416 6:fbe401163489 165 * Normal latched state, after RTD_HMI is set from PSU
open4416 6:fbe401163489 166 * Same to Idle state except RTD_cmd is set
open4416 6:fbe401163489 167 * Do:
open4416 6:fbe401163489 168 * 4WD controller
open4416 6:fbe401163489 169 * Check:
open4416 6:fbe401163489 170 * RUN faults if any
open4416 6:fbe401163489 171 * To VDU_Idle:
open4416 6:fbe401163489 172 * Initialize parameters for idling, reset RTD_cmd
open4416 6:fbe401163489 173 * To VDU_Fault:
open4416 6:fbe401163489 174 * Run the error handling service
open4416 6:fbe401163489 175 */
open4416 6:fbe401163489 176
open4416 6:fbe401163489 177 RUNT(); //Run test
open4416 6:fbe401163489 178 AWD(); //AWD main program
open4416 6:fbe401163489 179
open4416 6:fbe401163489 180 //Temporary debug posting area 2019/11/14
open4416 6:fbe401163489 181 //printf("%d,%d\n", Encoder_cnt, Encoder_del);
open4416 6:fbe401163489 182 //printf("%d\n\r", (int16_t)Tmodule);//
open4416 6:fbe401163489 183 //printf("%d\n\r", (int16_t)Vbus);
open4416 0:c4747ebbe0b4 184
open4416 14:bcf5cb4d08a5 185 if (VDU_FLT != 0) { //Check if any error
open4416 6:fbe401163489 186 VDU_STAT = VDU_Fault;
open4416 20:e9daae390513 187 RST_HMI = 0; //Ensure new RST action after error
open4416 7:f674ef860c9c 188 printf("Run 2 Fault\n");
open4416 6:fbe401163489 189 FLT_print = 1;
open4416 6:fbe401163489 190 } else if (RTD_HMI != 1) { //Or command to stop threw can bus
open4416 6:fbe401163489 191 Run2Idle();
open4416 6:fbe401163489 192 VDU_STAT = VDU_Idle;
open4416 6:fbe401163489 193 printf("Run 2 Idle\n");
open4416 6:fbe401163489 194 } //Else keep in state
open4416 6:fbe401163489 195 break;
open4416 6:fbe401163489 196
open4416 6:fbe401163489 197 case VDU_Fault:
open4416 6:fbe401163489 198 /* Controller Fault state
open4416 6:fbe401163489 199 * Description:
open4416 6:fbe401163489 200 * Fault latched state if any faults is detected
open4416 6:fbe401163489 201 * Same to Idle state except keep till RTD_HMI reset
open4416 6:fbe401163489 202 * Do:
open4416 6:fbe401163489 203 * Nothing, like a piece of shit
open4416 6:fbe401163489 204 * Check:
open4416 6:fbe401163489 205 * RUN faults if any
open4416 6:fbe401163489 206 * To VDU_PowerOn:
open4416 6:fbe401163489 207 * Restart VDU
open4416 6:fbe401163489 208 */
open4416 6:fbe401163489 209
open4416 6:fbe401163489 210 RUNT(); //Run test
open4416 6:fbe401163489 211
open4416 6:fbe401163489 212 if (RST_HMI == 1) { //PSU reset to clear error
open4416 18:780366f2534c 213 RST_HMI = 0;
open4416 6:fbe401163489 214 RST_cmd = 1;
open4416 14:bcf5cb4d08a5 215 FLT_print = 0; //Stop error printing
open4416 22:4f764f7cd0b3 216 // FL_online = 5; // 0 indicate detection timeout
open4416 22:4f764f7cd0b3 217 // FR_online = 5; // reset value is 3 to hold for 0.03sec
open4416 22:4f764f7cd0b3 218 // RL_online = 5; // -1 per 100Hz task
open4416 22:4f764f7cd0b3 219 // RR_online = 5;
open4416 22:4f764f7cd0b3 220 // PSU_online = 5;
open4416 22:4f764f7cd0b3 221 VDU_FLT = 0;
open4416 23:100661e2ad70 222 VDU_STAT = VDU_Reset;
open4416 14:bcf5cb4d08a5 223 printf("VDU rebooting...\n");
open4416 21:e01a019fae2f 224 printf("QDrive rebooting...\n");
open4416 6:fbe401163489 225 } //Else keep in state
open4416 6:fbe401163489 226 break;
open4416 23:100661e2ad70 227
open4416 23:100661e2ad70 228 case VDU_Reset:
open4416 23:100661e2ad70 229 /* Controller Reset state
open4416 23:100661e2ad70 230 * Description:
open4416 23:100661e2ad70 231 * A state after reset by HMI when Fault latched
open4416 23:100661e2ad70 232 * Wait till four driver processing, timeout protected
open4416 23:100661e2ad70 233 * Do:
open4416 23:100661e2ad70 234 * Nothing, just a soft delay
open4416 23:100661e2ad70 235 * Check:
open4416 23:100661e2ad70 236 * Timeout condition met
open4416 23:100661e2ad70 237 * To VDU_Idle:
open4416 23:100661e2ad70 238 * A valid reset
open4416 23:100661e2ad70 239 * To VDU_Fault:
open4416 23:100661e2ad70 240 * A fail reset
open4416 23:100661e2ad70 241 */
open4416 23:100661e2ad70 242
open4416 23:100661e2ad70 243 RUNT(); //Run test
open4416 23:100661e2ad70 244 if(!RL_DSM) {
open4416 23:100661e2ad70 245 // if(!(FL_DSM|FR_DSM|RL_DSM|RR_DSM)) { // 2020/3/13 for real case
open4416 23:100661e2ad70 246 printf("...\n");
open4416 23:100661e2ad70 247 VDU_FLT &= ~(DSM_VDUFLTCode); //Clear if fine
open4416 23:100661e2ad70 248 }
open4416 23:100661e2ad70 249
open4416 23:100661e2ad70 250 Reset_to += 1; //Time out check
open4416 23:100661e2ad70 251 if (Reset_to > 30) {
open4416 23:100661e2ad70 252 Reset_to = 0;
open4416 23:100661e2ad70 253 if (VDU_FLT != 0) { //Check if any error
open4416 23:100661e2ad70 254 VDU_STAT = VDU_Fault; //Back to fault state wait for next reset
open4416 23:100661e2ad70 255 printf("Reset fail 2 Fault\n");
open4416 23:100661e2ad70 256 FLT_print = 1;
open4416 23:100661e2ad70 257 } else { //A success reset
open4416 23:100661e2ad70 258 VDU_STAT = VDU_Idle;
open4416 23:100661e2ad70 259 printf("Reset ok 2 Idle\n");
open4416 23:100661e2ad70 260 }
open4416 23:100661e2ad70 261 } //Else keep in state
open4416 23:100661e2ad70 262 break;
open4416 6:fbe401163489 263 }
open4416 6:fbe401163489 264
open4416 7:f674ef860c9c 265 // Shit out torque distribution and special command
open4416 6:fbe401163489 266 if(VDU_STAT == VDU_Run) {
open4416 6:fbe401163489 267 //Allow output torque
open4416 6:fbe401163489 268 Tx_Tcmd_CAN1();
open4416 23:100661e2ad70 269 } else if(RST_cmd) {
open4416 7:f674ef860c9c 270 //Send out reset cmd once
open4416 6:fbe401163489 271 Tx_CLRerr_CAN1();
open4416 6:fbe401163489 272 } else {
open4416 6:fbe401163489 273 //Force RTD off when not in VDU_Run
open4416 6:fbe401163489 274 Tx_Estop_CAN1();
open4416 6:fbe401163489 275 }
open4416 6:fbe401163489 276
open4416 14:bcf5cb4d08a5 277 //2019/12/18 Add here to test IMU, newer version use inturrupt get data
open4416 14:bcf5cb4d08a5 278 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.ahrsProcessedData.attitude[0], imu.ahrsProcessedData.attitude[1], imu.ahrsProcessedData.attitude[2]);
open4416 14:bcf5cb4d08a5 279 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]);
open4416 14:bcf5cb4d08a5 280 // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.accel[0], imu.imuProcessedData.accel[1], imu.imuProcessedData.accel[2]);
open4416 19:d68f21173c23 281 // pc.printf("%.3f,%.3f,%.3f\n\r", YR_imu, Ax_imu, Ay_imu);
open4416 20:e9daae390513 282 // pc.printf("%.3f,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu);
open4416 22:4f764f7cd0b3 283 // pc.printf("%.3f\n\r", Trq_HMI*100.0f);
open4416 25:3c6e83b449b2 284 // pc.printf("%.3f\n\r", Steer_HMI);
open4416 25:3c6e83b449b2 285 // pc.printf("%.1f\n\r", Vx_wss);
open4416 14:bcf5cb4d08a5 286
open4416 6:fbe401163489 287 }
open4416 7:f674ef860c9c 288 // End of high speed loop
open4416 6:fbe401163489 289
open4416 9:c99eeafa6fa3 290 // Do low speed state reporting 10 Hz
open4416 6:fbe401163489 291 if (LST_EXFL == 1) {
open4416 6:fbe401163489 292 LST_EXFL = 0;
open4416 18:780366f2534c 293 //Cooling
open4416 8:f8b1402c8f3c 294 Cooler();
open4416 18:780366f2534c 295
open4416 18:780366f2534c 296 //Print low speed data on CAN
open4416 5:8116016abee0 297 Tx_Qdrv_CAN1();
open4416 6:fbe401163489 298
open4416 18:780366f2534c 299 // Print out error mesagge if exist, 0.5Hz repeative
open4416 6:fbe401163489 300 if(FLT_print != 0) {
open4416 18:780366f2534c 301 //Merge Faults
open4416 22:4f764f7cd0b3 302
open4416 22:4f764f7cd0b3 303 //USE THIS FOR FULL FUNCTION
open4416 25:3c6e83b449b2 304 FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post;
open4416 25:3c6e83b449b2 305 FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run;
open4416 22:4f764f7cd0b3 306
open4416 22:4f764f7cd0b3 307 //ONLY FOR TEST TODO
open4416 25:3c6e83b449b2 308 // FLT_Post = RL_FLT_Post; // Use only for single module test
open4416 25:3c6e83b449b2 309 // FLT_Run = RL_FLT_Run; //2020/3/10
open4416 18:780366f2534c 310
open4416 18:780366f2534c 311 //UART
open4416 7:f674ef860c9c 312 FLT_print_cnt += FLT_print;
open4416 6:fbe401163489 313 if(FLT_print_cnt > 19) {
open4416 18:780366f2534c 314 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);
open4416 18:780366f2534c 315 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);
open4416 18:780366f2534c 316 if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT);
open4416 24:518ec8a4fb6d 317
open4416 24:518ec8a4fb6d 318 //Only temperature printing 2020/6/30
open4416 24:518ec8a4fb6d 319 printf("Tmotor FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
open4416 24:518ec8a4fb6d 320 printf("Tmodule FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
open4416 24:518ec8a4fb6d 321
open4416 6:fbe401163489 322 FLT_print_cnt = 0;
open4416 6:fbe401163489 323 }
open4416 18:780366f2534c 324
open4416 18:780366f2534c 325 //LED
open4416 18:780366f2534c 326 if(Ind_refresh) {
open4416 18:780366f2534c 327 // Refresh data for LED indication after run threw
open4416 18:780366f2534c 328 FLT_Post_ind = FLT_Post;
open4416 18:780366f2534c 329 FLT_Run_ind = FLT_Run;
open4416 18:780366f2534c 330 VDU_FLT_ind = VDU_FLT;
open4416 18:780366f2534c 331 Ind_refresh = 0; // Set after run threw all error bits
open4416 18:780366f2534c 332 }
open4416 18:780366f2534c 333 } else {
open4416 18:780366f2534c 334 // Clear & stop LED when no faults
open4416 18:780366f2534c 335 FLT_Post_ind = 0;
open4416 18:780366f2534c 336 FLT_Run_ind = 0;
open4416 18:780366f2534c 337 VDU_FLT_ind = 0;
open4416 18:780366f2534c 338 Repeat = 0U;
open4416 18:780366f2534c 339 Phase = 0U;
open4416 18:780366f2534c 340 Blk_n = 0U;
open4416 6:fbe401163489 341 }
open4416 14:bcf5cb4d08a5 342
open4416 18:780366f2534c 343 // Blinky output
open4416 18:780366f2534c 344 if (VDU_STAT != VDU_PowerOn) {
open4416 18:780366f2534c 345 // Case after poweron (all module online) or fault(s) occur
open4416 18:780366f2534c 346 Ind_refresh = IndicationKernel(
open4416 18:780366f2534c 347 &Pattern,
open4416 18:780366f2534c 348 &Repeat,
open4416 18:780366f2534c 349 &Phase,
open4416 18:780366f2534c 350 &FLT_Post_ind,
open4416 18:780366f2534c 351 &FLT_Run_ind,
open4416 18:780366f2534c 352 &VDU_FLT_ind);
open4416 18:780366f2534c 353 LED = Indication(Pattern, &Repeat, &Blk_n);
open4416 18:780366f2534c 354 Fault_Ind = LED;
open4416 18:780366f2534c 355 } else {
open4416 18:780366f2534c 356 // Case no fault while waiting for all module online
open4416 18:780366f2534c 357 LED = !LED; //Fast 5Hz blinky indicate the activity
open4416 18:780366f2534c 358 Fault_Ind = LED;
open4416 18:780366f2534c 359 }
open4416 24:518ec8a4fb6d 360
open4416 24:518ec8a4fb6d 361
open4416 7:f674ef860c9c 362 }
open4416 7:f674ef860c9c 363 // End of low speed state reporting
open4416 6:fbe401163489 364
open4416 7:f674ef860c9c 365 } // end of while
open4416 0:c4747ebbe0b4 366 }
open4416 0:c4747ebbe0b4 367
open4416 6:fbe401163489 368 void Idle2Run(void)
open4416 6:fbe401163489 369 {
open4416 6:fbe401163489 370 RTD_cmd = 1;
open4416 6:fbe401163489 371 }
open4416 6:fbe401163489 372
open4416 6:fbe401163489 373 void Run2Idle(void)
open4416 6:fbe401163489 374 {
open4416 6:fbe401163489 375 RTD_cmd = 0;
open4416 6:fbe401163489 376 }
open4416 6:fbe401163489 377
open4416 6:fbe401163489 378 void POST(void)
open4416 6:fbe401163489 379 {
open4416 8:f8b1402c8f3c 380 //Check IMU status abnormal
open4416 25:3c6e83b449b2 381 // if(fabsf(YR_imu) > 250) { //half turn per sec, strange
open4416 25:3c6e83b449b2 382 // VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
open4416 25:3c6e83b449b2 383 // }
open4416 6:fbe401163489 384 }
open4416 6:fbe401163489 385
open4416 6:fbe401163489 386 void RUNT(void)
open4416 6:fbe401163489 387 {
open4416 8:f8b1402c8f3c 388 //POST
open4416 6:fbe401163489 389 POST();
open4416 6:fbe401163489 390
open4416 8:f8b1402c8f3c 391 //Check module response timeout
open4416 6:fbe401163489 392 if((FL_online*FR_online*RL_online*RR_online) == 0) {
open4416 6:fbe401163489 393 VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout
open4416 6:fbe401163489 394 }
open4416 6:fbe401163489 395 if(PSU_online == 0) {
open4416 6:fbe401163489 396 VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout
open4416 6:fbe401163489 397 }
open4416 6:fbe401163489 398
open4416 20:e9daae390513 399 //Check ShutDrv voltage when running
open4416 22:4f764f7cd0b3 400 if(VDU_STAT == VDU_Run) {
open4416 8:f8b1402c8f3c 401 if(SDn_voltage < 8.0f) {
open4416 24:518ec8a4fb6d 402 //2020/4/5 TODO remove in real case
open4416 25:3c6e83b449b2 403 VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv
open4416 8:f8b1402c8f3c 404 }
open4416 8:f8b1402c8f3c 405 }
open4416 25:3c6e83b449b2 406
open4416 25:3c6e83b449b2 407 //Check IMU status abnormal add in 2020/10/07
open4416 25:3c6e83b449b2 408 if(fabsf(YR_imu) > 250.0f) { //half turn per sec, strange
open4416 25:3c6e83b449b2 409 VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
open4416 25:3c6e83b449b2 410 }
open4416 6:fbe401163489 411 }
open4416 6:fbe401163489 412
open4416 6:fbe401163489 413 void Aux_read(void)
open4416 6:fbe401163489 414 {
open4416 19:d68f21173c23 415 //Capture analog in at once imu_driver ver
open4416 13:ac024885d0bf 416 AUX_1_u16 = AUX_1.read_u16() >> 4U;
open4416 13:ac024885d0bf 417 AUX_2_u16 = AUX_2.read_u16() >> 4U;
open4416 13:ac024885d0bf 418 AUX_3_u16 = AUX_3.read_u16() >> 4U;
open4416 13:ac024885d0bf 419 AUX_4_u16 = AUX_4.read_u16() >> 4U;
open4416 8:f8b1402c8f3c 420 SDn_voltage = 18.81f*SDn_sense.read(); //Read in Shutdown circuit voltage in output end
open4416 13:ac024885d0bf 421 Brk_voltage = 5.5f*Brk_sense.read();
open4416 6:fbe401163489 422 }
open4416 6:fbe401163489 423
open4416 19:d68f21173c23 424 #ifdef IMU_direct
open4416 19:d68f21173c23 425 void IMU_isr(void)
open4416 19:d68f21173c23 426 {
open4416 19:d68f21173c23 427 //Start data transfer
open4416 19:d68f21173c23 428 uint8_t data_rx = 0;
open4416 19:d68f21173c23 429 uint16_t reg_VG = Read_VG<<8U;
open4416 19:d68f21173c23 430 cs = 0;
open4416 19:d68f21173c23 431 spi2.write(reg_VG);
open4416 19:d68f21173c23 432 while(data_rx < VG_len) {
open4416 19:d68f21173c23 433 uint16_t spi_data = spi2.write(0x0000);
open4416 19:d68f21173c23 434 switch(data_rx) {
open4416 19:d68f21173c23 435 case 0:
open4416 19:d68f21173c23 436 ahrsdata.status = spi_data;
open4416 19:d68f21173c23 437 break;
open4416 19:d68f21173c23 438 case 1:
open4416 19:d68f21173c23 439 case 2:
open4416 19:d68f21173c23 440 case 3:
open4416 19:d68f21173c23 441 ahrsdata.rate[data_rx - 1] = spi_data;
open4416 19:d68f21173c23 442 break;
open4416 19:d68f21173c23 443 case 4:
open4416 19:d68f21173c23 444 case 5:
open4416 19:d68f21173c23 445 case 6:
open4416 19:d68f21173c23 446 ahrsdata.accel[data_rx - 4] = spi_data;
open4416 19:d68f21173c23 447 break;
open4416 19:d68f21173c23 448 case 7:
open4416 19:d68f21173c23 449 ahrsdata.temperature = spi_data;
open4416 19:d68f21173c23 450 break;
open4416 19:d68f21173c23 451 case 8:
open4416 19:d68f21173c23 452 case 9:
open4416 19:d68f21173c23 453 case 10:
open4416 19:d68f21173c23 454 ahrsdata.attitude[data_rx - 8] = spi_data;
open4416 19:d68f21173c23 455 break;
open4416 19:d68f21173c23 456 default:
open4416 19:d68f21173c23 457 break;
open4416 19:d68f21173c23 458 }
open4416 19:d68f21173c23 459 ++data_rx;
open4416 19:d68f21173c23 460 }
open4416 19:d68f21173c23 461 cs = 1;
open4416 19:d68f21173c23 462 }
open4416 19:d68f21173c23 463 #endif
open4416 19:d68f21173c23 464
open4416 6:fbe401163489 465 void IMU_read(void)
open4416 6:fbe401163489 466 {
open4416 19:d68f21173c23 467 #ifndef IMU_direct
open4416 18:780366f2534c 468 //Get readings threw back ground, direction not checked 2019/12/20
open4416 25:3c6e83b449b2 469 // Direction checked 2020/10/29
open4416 14:bcf5cb4d08a5 470 YR_imu = imu.imuProcessedData.rate[2];
open4416 14:bcf5cb4d08a5 471 Ax_imu = imu.imuProcessedData.accel[0];
open4416 14:bcf5cb4d08a5 472 Ay_imu = imu.imuProcessedData.accel[1];
open4416 14:bcf5cb4d08a5 473 Roll_imu = imu.ahrsProcessedData.attitude[0];
open4416 14:bcf5cb4d08a5 474 Pitch_imu = imu.ahrsProcessedData.attitude[1];
open4416 19:d68f21173c23 475 Yaw_imu = imu.ahrsProcessedData.attitude[2];
open4416 19:d68f21173c23 476 #else
open4416 19:d68f21173c23 477 YR_imu = ahrsdata.rate[2]/GYRO2F;
open4416 19:d68f21173c23 478 Ax_imu = ahrsdata.accel[0]/ACCL2F;
open4416 19:d68f21173c23 479 Ay_imu = ahrsdata.accel[1]/ACCL2F;
open4416 19:d68f21173c23 480 Roll_imu = ahrsdata.attitude[0]/AHRS2F;
open4416 19:d68f21173c23 481 Pitch_imu = ahrsdata.attitude[1]/AHRS2F;
open4416 19:d68f21173c23 482 #endif
open4416 6:fbe401163489 483 }
open4416 6:fbe401163489 484
open4416 6:fbe401163489 485 void AWD(void)
open4416 6:fbe401163489 486 {
open4416 25:3c6e83b449b2 487 float Tayc_tmp = 0; //active part of yaw control system
open4416 25:3c6e83b449b2 488 float Tlsd_tmp = 0; //limit slip differetial torque
open4416 25:3c6e83b449b2 489 float YdelW_ele = 0; //Ideal L-R electric speed difference
open4416 25:3c6e83b449b2 490
open4416 25:3c6e83b449b2 491 //Get estimate from wheel only
open4416 25:3c6e83b449b2 492 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
open4416 25:3c6e83b449b2 493
open4416 25:3c6e83b449b2 494 //Simple comp filter is fine without GSS onboard
open4416 25:3c6e83b449b2 495 Vx_fil += (Ax_imu-0.01f)*9.807f*0.01f; //-0.01 here is to be calibrated
open4416 25:3c6e83b449b2 496 Vx_fil = Vx_fil*0.98f + Vx_wss*0.02f; //0.98, 0.02 is coefficient
open4416 25:3c6e83b449b2 497
open4416 25:3c6e83b449b2 498 if(AWD_HMI) {
open4416 25:3c6e83b449b2 499 // A simple version is put here for reading
open4416 25:3c6e83b449b2 500 //YR_ref = Steer_HMI*st2r*Vb_est/(Base+EG*Vb_est*Vb_est);
open4416 25:3c6e83b449b2 501
open4416 25:3c6e83b449b2 502 /*Still in test section, ignore*/
open4416 25:3c6e83b449b2 503 //sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
open4416 25:3c6e83b449b2 504 //FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
open4416 25:3c6e83b449b2 505 //FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
open4416 25:3c6e83b449b2 506 //RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
open4416 25:3c6e83b449b2 507 //RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);
open4416 25:3c6e83b449b2 508
open4416 25:3c6e83b449b2 509 /*A basic static distribution*/
open4416 25:3c6e83b449b2 510 FL_Tcmd = 0.11f*Trq_HMI;
open4416 25:3c6e83b449b2 511 FR_Tcmd = 0.11f*Trq_HMI;
open4416 25:3c6e83b449b2 512 RL_Tcmd = 0.25f*Trq_HMI;
open4416 25:3c6e83b449b2 513 RR_Tcmd = 0.25f*Trq_HMI;
open4416 25:3c6e83b449b2 514
open4416 25:3c6e83b449b2 515 /*A basic Yaw control*/
open4416 25:3c6e83b449b2 516 YR_ref = (Steer_HMI*st2r)*Vx_fil/Base; // Center calibration moved to can recieve
open4416 25:3c6e83b449b2 517 Tayc_tmp = (YR_ref - YR_imu*d2r)*10.0f; // map 1 rad/s YR difference to ~ 10Nm
open4416 25:3c6e83b449b2 518 Tayc_tmp = constrain(Tayc_tmp,-5.0f,5.0f);
open4416 25:3c6e83b449b2 519 FL_Tcmd += -Tayc_tmp;
open4416 25:3c6e83b449b2 520 FR_Tcmd += Tayc_tmp;
open4416 25:3c6e83b449b2 521 RL_Tcmd += -Tayc_tmp;
open4416 25:3c6e83b449b2 522 RR_Tcmd += Tayc_tmp;
open4416 25:3c6e83b449b2 523
open4416 25:3c6e83b449b2 524 /*A basic ideal differential controller*/
open4416 25:3c6e83b449b2 525 YdelW_ele = YR_ref*Track/0.235f*11.0f*4.0f; // dir, rate to speed, wheel radius, reducer, pole pair
open4416 25:3c6e83b449b2 526 //YdelW_ele > 0 when left turning
open4416 25:3c6e83b449b2 527
open4416 25:3c6e83b449b2 528 /*A basic static distribution + differential for test 2020/7/19*/
open4416 25:3c6e83b449b2 529 //Front differential
open4416 25:3c6e83b449b2 530 Tlsd_tmp = (FL_W_ele - FR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm
open4416 25:3c6e83b449b2 531 Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
open4416 25:3c6e83b449b2 532 FL_Tcmd += -Tlsd_tmp;
open4416 25:3c6e83b449b2 533 FR_Tcmd += Tlsd_tmp;
open4416 25:3c6e83b449b2 534
open4416 25:3c6e83b449b2 535
open4416 25:3c6e83b449b2 536 //Rear differential
open4416 25:3c6e83b449b2 537 Tlsd_tmp = (RL_W_ele - RR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm
open4416 25:3c6e83b449b2 538 Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
open4416 25:3c6e83b449b2 539 RL_Tcmd += -Tlsd_tmp;
open4416 25:3c6e83b449b2 540 RR_Tcmd += Tlsd_tmp;
open4416 25:3c6e83b449b2 541
open4416 25:3c6e83b449b2 542 } else {
open4416 25:3c6e83b449b2 543 //2020/8/19 for fast test pure rear
open4416 25:3c6e83b449b2 544 Tlsd_tmp = (FL_W_ele - FR_W_ele)*0.01f; // map 1000 rad/s difference to ~ 10Nm
open4416 25:3c6e83b449b2 545 Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
open4416 25:3c6e83b449b2 546 if(Tlsd_tmp>0.0f) { //L > R
open4416 25:3c6e83b449b2 547 FL_Tcmd = 0.15f*Trq_HMI - Tlsd_tmp;
open4416 25:3c6e83b449b2 548 FR_Tcmd = 0.15f*Trq_HMI;
open4416 25:3c6e83b449b2 549 } else { //L < R, Tlsd_tmp < 0
open4416 25:3c6e83b449b2 550 FL_Tcmd = 0.15f*Trq_HMI;
open4416 25:3c6e83b449b2 551 FR_Tcmd = 0.15f*Trq_HMI + Tlsd_tmp;
open4416 25:3c6e83b449b2 552 }
open4416 25:3c6e83b449b2 553 //Rear differential
open4416 25:3c6e83b449b2 554 Tlsd_tmp = (RL_W_ele - RR_W_ele)*0.005f; // map 1000 rad/s difference to ~ 5Nm
open4416 25:3c6e83b449b2 555 Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
open4416 25:3c6e83b449b2 556 if(Tlsd_tmp>0.0f) { //L > R
open4416 25:3c6e83b449b2 557 RL_Tcmd = 0.25f*Trq_HMI - Tlsd_tmp;
open4416 25:3c6e83b449b2 558 RR_Tcmd = 0.25f*Trq_HMI;
open4416 25:3c6e83b449b2 559 } else { //L < R, Tlsd_tmp < 0
open4416 25:3c6e83b449b2 560 RL_Tcmd = 0.25f*Trq_HMI;
open4416 25:3c6e83b449b2 561 RR_Tcmd = 0.25f*Trq_HMI + Tlsd_tmp;
open4416 25:3c6e83b449b2 562 }
open4416 25:3c6e83b449b2 563
open4416 25:3c6e83b449b2 564
open4416 25:3c6e83b449b2 565
open4416 25:3c6e83b449b2 566 // // A basic static distribution 2020/7/19
open4416 25:3c6e83b449b2 567 // FL_Tcmd = 0.15f*Trq_HMI;
open4416 25:3c6e83b449b2 568 // FR_Tcmd = 0.15f*Trq_HMI;
open4416 24:518ec8a4fb6d 569 // RL_Tcmd = 0.25f*Trq_HMI;
open4416 24:518ec8a4fb6d 570 // RR_Tcmd = 0.25f*Trq_HMI;
open4416 25:3c6e83b449b2 571 }//last line of: if(AWD_HMI){}
open4416 24:518ec8a4fb6d 572
open4416 24:518ec8a4fb6d 573 ////Add to force normal drive
open4416 24:518ec8a4fb6d 574 // FL_Tcmd = 0.25f*Trq_HMI;
open4416 24:518ec8a4fb6d 575 // FR_Tcmd = 0.25f*Trq_HMI;
open4416 24:518ec8a4fb6d 576 // RL_Tcmd = 0.25f*Trq_HMI;
open4416 24:518ec8a4fb6d 577 // RR_Tcmd = 0.25f*Trq_HMI;
open4416 24:518ec8a4fb6d 578
open4416 24:518ec8a4fb6d 579 //Add to force rear drive
open4416 25:3c6e83b449b2 580 // FL_Tcmd = 0.2f*Trq_HMI;
open4416 25:3c6e83b449b2 581 // FR_Tcmd = 0.2f*Trq_HMI;
open4416 25:3c6e83b449b2 582 // RL_Tcmd = 0.5f*Trq_HMI;
open4416 25:3c6e83b449b2 583 // RR_Tcmd = 0.5f*Trq_HMI;
open4416 24:518ec8a4fb6d 584
open4416 25:3c6e83b449b2 585 //Direction define, move to can sendout
open4416 25:3c6e83b449b2 586 // FL_Tcmd = -1.0f*FL_Tcmd;
open4416 25:3c6e83b449b2 587 // FR_Tcmd = 1.0f*FR_Tcmd;
open4416 25:3c6e83b449b2 588 // RL_Tcmd = -1.0f*RL_Tcmd;
open4416 25:3c6e83b449b2 589 // RR_Tcmd = 1.0f*RR_Tcmd;
open4416 26:ad4fbceeb4ae 590
open4416 26:ad4fbceeb4ae 591 //Add to force only one motor drive (DYNO)
open4416 26:ad4fbceeb4ae 592 FL_Tcmd = 0.0f*Trq_HMI;
open4416 26:ad4fbceeb4ae 593 FR_Tcmd = 0.0f*Trq_HMI;
open4416 26:ad4fbceeb4ae 594 RL_Tcmd = 0.1f*Trq_HMI;
open4416 26:ad4fbceeb4ae 595 RR_Tcmd = 0.0f*Trq_HMI;
open4416 26:ad4fbceeb4ae 596
open4416 8:f8b1402c8f3c 597 }
open4416 6:fbe401163489 598
open4416 8:f8b1402c8f3c 599 void ASL(void)
open4416 8:f8b1402c8f3c 600 {
open4416 8:f8b1402c8f3c 601 //Filter out each motor W_ele and get approximate accel, compare with IMU
open4416 8:f8b1402c8f3c 602 //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel)
open4416 8:f8b1402c8f3c 603 //Fill out if enough time
open4416 6:fbe401163489 604 }
open4416 6:fbe401163489 605
open4416 5:8116016abee0 606 void Rx_CAN1(void)
open4416 5:8116016abee0 607 {
open4416 18:780366f2534c 608 // LED = 1;
open4416 7:f674ef860c9c 609 int16_t tmp;
open4416 7:f674ef860c9c 610
open4416 5:8116016abee0 611 if(can1.read(can_msg_Rx)) {
open4416 7:f674ef860c9c 612 switch(can_msg_Rx.id) { //Filtered input message
open4416 7:f674ef860c9c 613 // Start of 100Hz msg
open4416 8:f8b1402c8f3c 614 case FL_HSB_ID://1
open4416 7:f674ef860c9c 615 //HSB from FL motor drive
open4416 20:e9daae390513 616 FL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
open4416 7:f674ef860c9c 617 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 25:3c6e83b449b2 618 FL_W_ele = tmp*-1.0f;
open4416 7:f674ef860c9c 619 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 25:3c6e83b449b2 620 FL_Trq_fil3 = tmp * -0.01f;
open4416 7:f674ef860c9c 621 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 25:3c6e83b449b2 622 FL_Trq_est = tmp * -0.01f;
open4416 20:e9daae390513 623 FL_online = 5;
open4416 18:780366f2534c 624 //If fault
open4416 18:780366f2534c 625 if(FL_DSM == 3U) {
open4416 25:3c6e83b449b2 626 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
open4416 18:780366f2534c 627 }
open4416 7:f674ef860c9c 628 break;
open4416 7:f674ef860c9c 629
open4416 8:f8b1402c8f3c 630 case FR_HSB_ID://2
open4416 7:f674ef860c9c 631 //HSB from FR motor drive
open4416 20:e9daae390513 632 FR_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
open4416 7:f674ef860c9c 633 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 634 FR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 635 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 636 FR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 637 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 638 FR_Trq_est = tmp * 0.01f;
open4416 20:e9daae390513 639 FR_online = 5;
open4416 18:780366f2534c 640 if(FR_DSM == 3U) {
open4416 25:3c6e83b449b2 641 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
open4416 18:780366f2534c 642 }
open4416 7:f674ef860c9c 643 break;
open4416 7:f674ef860c9c 644
open4416 8:f8b1402c8f3c 645 case RL_HSB_ID://3
open4416 7:f674ef860c9c 646 //HSB from RL motor drive
open4416 20:e9daae390513 647 RL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
open4416 7:f674ef860c9c 648 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 25:3c6e83b449b2 649 RL_W_ele = tmp*-1.0f;
open4416 7:f674ef860c9c 650 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 25:3c6e83b449b2 651 RL_Trq_fil3 = tmp * -0.01f;
open4416 7:f674ef860c9c 652 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 25:3c6e83b449b2 653 RL_Trq_est = tmp * -0.01f;
open4416 20:e9daae390513 654 RL_online = 5;
open4416 18:780366f2534c 655 if(RL_DSM == 3U) {
open4416 18:780366f2534c 656 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
open4416 18:780366f2534c 657 }
open4416 7:f674ef860c9c 658 break;
open4416 7:f674ef860c9c 659
open4416 8:f8b1402c8f3c 660 case RR_HSB_ID://4
open4416 7:f674ef860c9c 661 //HSB from RR motor drive
open4416 20:e9daae390513 662 RR_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
open4416 7:f674ef860c9c 663 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 664 RR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 665 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 666 RR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 667 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 668 RR_Trq_est = tmp * 0.01f;
open4416 20:e9daae390513 669 RR_online = 5;
open4416 18:780366f2534c 670 if(RR_DSM == 3U) {
open4416 25:3c6e83b449b2 671 VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
open4416 18:780366f2534c 672 }
open4416 7:f674ef860c9c 673 break;
open4416 7:f674ef860c9c 674
open4416 8:f8b1402c8f3c 675 case HMI_cmd_ID://5
open4416 7:f674ef860c9c 676 //HMI command from PSU
open4416 7:f674ef860c9c 677 AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch
open4416 7:f674ef860c9c 678 RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
open4416 7:f674ef860c9c 679 RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch
open4416 7:f674ef860c9c 680 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 25:3c6e83b449b2 681 Steer_HMI = tmp * 0.01f - 0.0f; //0.7f here is to calibrated center
open4416 7:f674ef860c9c 682 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 683 Trq_HMI = tmp * 0.01f;
open4416 20:e9daae390513 684 PSU_online = 5;
open4416 7:f674ef860c9c 685 break;
open4416 7:f674ef860c9c 686 // end of 100Hz msg
open4416 7:f674ef860c9c 687
open4416 7:f674ef860c9c 688 // Start of 10Hz msg
open4416 8:f8b1402c8f3c 689 case FL_LSB_ID://6
open4416 7:f674ef860c9c 690 //LSB from FL motor drive
open4416 7:f674ef860c9c 691 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 692 FL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 693 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 694 FL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 695 FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 696 FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 697 break;
open4416 7:f674ef860c9c 698
open4416 8:f8b1402c8f3c 699 case FR_LSB_ID://7
open4416 7:f674ef860c9c 700 //LSB from FR motor drive
open4416 7:f674ef860c9c 701 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 702 FR_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 703 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 704 FR_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 705 FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 706 FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 707 break;
open4416 7:f674ef860c9c 708
open4416 8:f8b1402c8f3c 709 case RL_LSB_ID://8
open4416 7:f674ef860c9c 710 //LSB from RL motor drive
open4416 7:f674ef860c9c 711 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 712 RL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 713 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 714 RL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 715 RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 716 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 717 break;
open4416 7:f674ef860c9c 718
open4416 8:f8b1402c8f3c 719 case RR_LSB_ID://9
open4416 7:f674ef860c9c 720 //LSB from RR motor drive
open4416 7:f674ef860c9c 721 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 8:f8b1402c8f3c 722 RR_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 723 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 8:f8b1402c8f3c 724 RR_Tmodule = tmp*0.01f;
open4416 8:f8b1402c8f3c 725 RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 8:f8b1402c8f3c 726 RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 727 break;
open4416 25:3c6e83b449b2 728
open4416 25:3c6e83b449b2 729 case PedalStat_ID://10
open4416 25:3c6e83b449b2 730 RTD_SW = 0x01&can_msg_Rx.data[1];
open4416 25:3c6e83b449b2 731 break;
open4416 7:f674ef860c9c 732 // end of 10Hz msg
open4416 7:f674ef860c9c 733 }
open4416 5:8116016abee0 734 }
open4416 18:780366f2534c 735 // LED = 0;
open4416 5:8116016abee0 736 }
open4416 5:8116016abee0 737
open4416 6:fbe401163489 738 void Tx_CLRerr_CAN1(void)
open4416 2:c7a3a8c1aeed 739 {
open4416 9:c99eeafa6fa3 740 Tx_Estop_CAN1(); //disable as default
open4416 20:e9daae390513 741 RST_cmd = 0; //clear out one shot
open4416 6:fbe401163489 742 }
open4416 6:fbe401163489 743
open4416 6:fbe401163489 744 void Tx_Estop_CAN1(void)
open4416 6:fbe401163489 745 {
open4416 9:c99eeafa6fa3 746 RTD_cmd = 0; //force disable
open4416 6:fbe401163489 747 Tx_Tcmd_CAN1();
open4416 2:c7a3a8c1aeed 748 }
open4416 2:c7a3a8c1aeed 749
open4416 9:c99eeafa6fa3 750 void Tx_Tcmd_CAN1(void) // 100 Hz
open4416 2:c7a3a8c1aeed 751 {
open4416 7:f674ef860c9c 752 int16_t tmp;
open4416 25:3c6e83b449b2 753 tmp = (int16_t) (FL_Tcmd * -100.0f);
open4416 7:f674ef860c9c 754 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 755 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 756 temp_msg[2] = RTD_cmd;
open4416 21:e01a019fae2f 757 temp_msg[3] = RST_cmd;
open4416 20:e9daae390513 758 // temp_msg[3] = 0U; // 2020/3/4 add to disable HMI reseting Driver
open4416 14:bcf5cb4d08a5 759 temp_msg[4] = 0U;
open4416 14:bcf5cb4d08a5 760 temp_msg[5] = 0U;
open4416 14:bcf5cb4d08a5 761 temp_msg[6] = 0U;
open4416 14:bcf5cb4d08a5 762 temp_msg[7] = 0U;
open4416 8:f8b1402c8f3c 763 can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 764 CANpendTX();
open4416 6:fbe401163489 765 can1.write(can_msg_Tx);
open4416 6:fbe401163489 766
open4416 7:f674ef860c9c 767 tmp = (int16_t) (FR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 768 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 769 temp_msg[1] = tmp >> 8U;
open4416 8:f8b1402c8f3c 770 can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 771 CANpendTX();
open4416 6:fbe401163489 772 can1.write(can_msg_Tx);
open4416 6:fbe401163489 773
open4416 25:3c6e83b449b2 774 tmp = (int16_t) (RL_Tcmd * -100.0f);
open4416 7:f674ef860c9c 775 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 776 temp_msg[1] = tmp >> 8U;
open4416 8:f8b1402c8f3c 777 can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 778 CANpendTX();
open4416 6:fbe401163489 779 can1.write(can_msg_Tx);
open4416 6:fbe401163489 780
open4416 7:f674ef860c9c 781 tmp = (int16_t) (RR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 782 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 783 temp_msg[1] = tmp >> 8U;
open4416 8:f8b1402c8f3c 784 can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 785 CANpendTX();
open4416 6:fbe401163489 786 can1.write(can_msg_Tx);
open4416 6:fbe401163489 787
open4416 25:3c6e83b449b2 788 // IMU attitude readings shitting out
open4416 25:3c6e83b449b2 789 tmp = (int16_t) (YR_imu * 100.0f);
open4416 25:3c6e83b449b2 790 temp_msg[0] = tmp;
open4416 25:3c6e83b449b2 791 temp_msg[1] = tmp >> 8U;
open4416 25:3c6e83b449b2 792 tmp = (int16_t) (Roll_imu * 100.0f);
open4416 25:3c6e83b449b2 793 temp_msg[2] = tmp;
open4416 25:3c6e83b449b2 794 temp_msg[3] = tmp >> 8U;
open4416 25:3c6e83b449b2 795 tmp = (int16_t) (Pitch_imu * 100.0f);
open4416 25:3c6e83b449b2 796 temp_msg[4] = tmp;
open4416 25:3c6e83b449b2 797 temp_msg[5] = tmp >> 8U;
open4416 25:3c6e83b449b2 798 temp_msg[6] = (int8_t)(Ax_imu * 50.0f);
open4416 25:3c6e83b449b2 799 temp_msg[7] = (int8_t)(Ay_imu * 50.0f);
open4416 25:3c6e83b449b2 800 can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 25:3c6e83b449b2 801 CANpendTX();
open4416 25:3c6e83b449b2 802 can1.write(can_msg_Tx);
open4416 25:3c6e83b449b2 803
open4416 25:3c6e83b449b2 804 // Some internal control variables shitting out
open4416 25:3c6e83b449b2 805 tmp = (int16_t) (Vx_fil * 100.0f);
open4416 25:3c6e83b449b2 806 temp_msg[0] = tmp;
open4416 25:3c6e83b449b2 807 temp_msg[1] = tmp >> 8U;
open4416 25:3c6e83b449b2 808 can_msg_Tx = CANMessage(RegularVar_ID,temp_msg,2,CANData,CANStandard);
open4416 25:3c6e83b449b2 809 CANpendTX();
open4416 25:3c6e83b449b2 810 can1.write(can_msg_Tx);
open4416 2:c7a3a8c1aeed 811 }
open4416 2:c7a3a8c1aeed 812
open4416 6:fbe401163489 813 void Tx_Qdrv_CAN1(void) // 10 Hz
open4416 0:c4747ebbe0b4 814 {
open4416 25:3c6e83b449b2 815 //int16_t tmp;
open4416 6:fbe401163489 816 // Auxilary sensor reading shitting out
open4416 7:f674ef860c9c 817 temp_msg[0] = AUX_1_u16;
open4416 7:f674ef860c9c 818 temp_msg[1] = AUX_1_u16 >> 8U;
open4416 7:f674ef860c9c 819 temp_msg[2] = AUX_2_u16;
open4416 7:f674ef860c9c 820 temp_msg[3] = AUX_2_u16 >> 8U;
open4416 7:f674ef860c9c 821 temp_msg[4] = AUX_3_u16;
open4416 7:f674ef860c9c 822 temp_msg[5] = AUX_3_u16 >> 8U;
open4416 7:f674ef860c9c 823 temp_msg[6] = AUX_4_u16;
open4416 7:f674ef860c9c 824 temp_msg[7] = AUX_4_u16 >> 8U;
open4416 6:fbe401163489 825 can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 826 CANpendTX();
open4416 6:fbe401163489 827 can1.write(can_msg_Tx);
open4416 6:fbe401163489 828
open4416 6:fbe401163489 829 // Qdrive internal state shitting out
open4416 8:f8b1402c8f3c 830 temp_msg[0] = VDU_FLT;
open4416 8:f8b1402c8f3c 831 temp_msg[1] = VDU_FLT >> 8U;
open4416 8:f8b1402c8f3c 832 temp_msg[2] = VDU_STAT;
open4416 14:bcf5cb4d08a5 833 temp_msg[3] = (int8_t)(SDn_voltage*10.0f);
open4416 25:3c6e83b449b2 834 temp_msg[4] = (int8_t)(Brk_voltage*50.0f);
open4416 14:bcf5cb4d08a5 835 temp_msg[5] = 0U;
open4416 14:bcf5cb4d08a5 836 temp_msg[6] = 0U;
open4416 14:bcf5cb4d08a5 837 temp_msg[7] = 0U;
open4416 6:fbe401163489 838 can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 839 CANpendTX();
open4416 6:fbe401163489 840 can1.write(can_msg_Tx);
open4416 6:fbe401163489 841 }
open4416 6:fbe401163489 842
open4416 6:fbe401163489 843 void CANpendTX(void)
open4416 6:fbe401163489 844 {
open4416 6:fbe401163489 845 //Pend till TX box has empty slot, timeout will generate error
open4416 6:fbe401163489 846 uint32_t timeout = 0;
open4416 6:fbe401163489 847 while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
open4416 6:fbe401163489 848 //Wait till non empty
open4416 6:fbe401163489 849 timeout += 1;
open4416 6:fbe401163489 850 if(timeout > 10000) {
open4416 6:fbe401163489 851 // Put some timeout error handler
open4416 6:fbe401163489 852 break;
open4416 6:fbe401163489 853 }
open4416 0:c4747ebbe0b4 854 }
open4416 0:c4747ebbe0b4 855 }
open4416 1:8a9ac822aab7 856
open4416 6:fbe401163489 857 void CAN_init(void)
open4416 6:fbe401163489 858 {
open4416 6:fbe401163489 859 //Set CAN system
open4416 6:fbe401163489 860 SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off
open4416 6:fbe401163489 861 can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode
open4416 6:fbe401163489 862 can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1);
open4416 6:fbe401163489 863 can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2);
open4416 6:fbe401163489 864 can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3);
open4416 6:fbe401163489 865 can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4);
open4416 6:fbe401163489 866 can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
open4416 6:fbe401163489 867 can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
open4416 6:fbe401163489 868 can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
open4416 8:f8b1402c8f3c 869 can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring
open4416 25:3c6e83b449b2 870 can1.filter(PedalStat_ID,0xFFFF,CANStandard,9); // PSU online monitoring
open4416 14:bcf5cb4d08a5 871 // can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
open4416 8:f8b1402c8f3c 872 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq
open4416 6:fbe401163489 873 }
open4416 2:c7a3a8c1aeed 874
open4416 6:fbe401163489 875 void Module_WD(void)
open4416 6:fbe401163489 876 {
open4416 9:c99eeafa6fa3 877 //Module online dissipitive indicator
open4416 6:fbe401163489 878 if (FL_online != 0) {
open4416 6:fbe401163489 879 FL_online -= 1;
open4416 6:fbe401163489 880 }
open4416 6:fbe401163489 881 if (FR_online != 0) {
open4416 6:fbe401163489 882 FR_online -= 1;
open4416 6:fbe401163489 883 }
open4416 6:fbe401163489 884 if (RL_online != 0) {
open4416 6:fbe401163489 885 RL_online -= 1;
open4416 6:fbe401163489 886 }
open4416 6:fbe401163489 887 if (RR_online != 0) {
open4416 6:fbe401163489 888 RR_online -= 1;
open4416 6:fbe401163489 889 }
open4416 6:fbe401163489 890 if (PSU_online != 0) {
open4416 6:fbe401163489 891 PSU_online -= 1;
open4416 6:fbe401163489 892 }
open4416 6:fbe401163489 893 }
open4416 18:780366f2534c 894 uint8_t Indication( // Blink indicator in pattern * repeat
open4416 18:780366f2534c 895 uint8_t pattern,
open4416 18:780366f2534c 896 uint16_t*repeat,
open4416 18:780366f2534c 897 uint8_t*blk_n)
open4416 18:780366f2534c 898 {
open4416 18:780366f2534c 899 uint8_t out = 0;
open4416 18:780366f2534c 900 if(*repeat==0) return out; // reject repeat = 0 case, out = 0
open4416 18:780366f2534c 901 out = (pattern>>(*blk_n)) & 1U; // blink from LSB to MSB
open4416 18:780366f2534c 902 if(*blk_n < 7U) {
open4416 18:780366f2534c 903 *blk_n += 1U;
open4416 18:780366f2534c 904 } else { // a full pattern was passed
open4416 18:780366f2534c 905 *blk_n = 0U;
open4416 18:780366f2534c 906 *repeat >>= 1U;
open4416 18:780366f2534c 907 }
open4416 18:780366f2534c 908 return out;
open4416 18:780366f2534c 909 }
open4416 18:780366f2534c 910
open4416 18:780366f2534c 911 uint8_t IndicationKernel( // Generate blink pattern, return 1 if all ind cleared
open4416 18:780366f2534c 912 uint8_t*pattern,
open4416 18:780366f2534c 913 uint16_t*repeat,
open4416 18:780366f2534c 914 uint8_t*phase,
open4416 18:780366f2534c 915 uint16_t*Post_ind,
open4416 18:780366f2534c 916 uint16_t*Run_ind,
open4416 18:780366f2534c 917 uint16_t*VDU_ind)
open4416 18:780366f2534c 918 {
open4416 18:780366f2534c 919 //Blink indicator in pattern
open4416 18:780366f2534c 920 //If FLT_Run = 0b0010-0110, pattern will be --......--...--..
open4416 18:780366f2534c 921 uint8_t refresh = 0;
open4416 18:780366f2534c 922 if(*repeat!=0) return refresh; // skip straight to Indication()
open4416 18:780366f2534c 923
open4416 18:780366f2534c 924 if(*Post_ind != 0) {
open4416 18:780366f2534c 925 switch(*phase) {
open4416 18:780366f2534c 926 case 0U:
open4416 18:780366f2534c 927 *repeat = Post_rep;
open4416 18:780366f2534c 928 *pattern = L_Pulse;
open4416 18:780366f2534c 929 *phase = 1U;
open4416 18:780366f2534c 930 break;
open4416 18:780366f2534c 931
open4416 18:780366f2534c 932 case 1U:
open4416 18:780366f2534c 933 *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit
open4416 18:780366f2534c 934 *Post_ind &= ~*repeat; // this bit is used out
open4416 18:780366f2534c 935 *pattern = S_Pulse;
open4416 18:780366f2534c 936 *phase = 2U;
open4416 18:780366f2534c 937 break;
open4416 18:780366f2534c 938
open4416 18:780366f2534c 939 case 2U:
open4416 18:780366f2534c 940 *repeat = 1U;
open4416 18:780366f2534c 941 *pattern = N_Pulse;
open4416 18:780366f2534c 942 *phase = 0U;
open4416 18:780366f2534c 943 break;
open4416 18:780366f2534c 944 }
open4416 18:780366f2534c 945 return refresh;
open4416 18:780366f2534c 946 }
open4416 18:780366f2534c 947
open4416 18:780366f2534c 948 if(*Run_ind != 0) {
open4416 18:780366f2534c 949 switch(*phase) {
open4416 18:780366f2534c 950 case 0U:
open4416 18:780366f2534c 951 *repeat = Run_rep;
open4416 18:780366f2534c 952 *pattern = L_Pulse;
open4416 18:780366f2534c 953 *phase = 1U;
open4416 18:780366f2534c 954 break;
open4416 18:780366f2534c 955
open4416 18:780366f2534c 956 case 1U:
open4416 18:780366f2534c 957 *repeat = (*Run_ind)&(-*Run_ind); // extract LSB bit
open4416 18:780366f2534c 958 *Run_ind &= ~*repeat; // this bit is used out
open4416 18:780366f2534c 959 *pattern = S_Pulse;
open4416 18:780366f2534c 960 *phase = 2U;
open4416 18:780366f2534c 961 break;
open4416 18:780366f2534c 962
open4416 18:780366f2534c 963 case 2U:
open4416 18:780366f2534c 964 *repeat = 1U;
open4416 18:780366f2534c 965 *pattern = N_Pulse;
open4416 18:780366f2534c 966 *phase = 0U;
open4416 18:780366f2534c 967 break;
open4416 18:780366f2534c 968 }
open4416 18:780366f2534c 969 return refresh;
open4416 18:780366f2534c 970 }
open4416 18:780366f2534c 971
open4416 18:780366f2534c 972 if(*VDU_ind != 0) {
open4416 18:780366f2534c 973 switch(*phase) {
open4416 18:780366f2534c 974 case 0U:
open4416 18:780366f2534c 975 *repeat = VDU_rep;
open4416 18:780366f2534c 976 *pattern = L_Pulse;
open4416 18:780366f2534c 977 *phase = 1U;
open4416 18:780366f2534c 978 break;
open4416 18:780366f2534c 979
open4416 18:780366f2534c 980 case 1U:
open4416 18:780366f2534c 981 *repeat = (*VDU_ind)&(-*VDU_ind); // extract LSB bit
open4416 18:780366f2534c 982 *VDU_ind &= ~*repeat; // this bit is used out
open4416 18:780366f2534c 983 *pattern = S_Pulse;
open4416 18:780366f2534c 984 *phase = 2U;
open4416 18:780366f2534c 985 break;
open4416 18:780366f2534c 986
open4416 18:780366f2534c 987 case 2U:
open4416 18:780366f2534c 988 *repeat = 1U;
open4416 18:780366f2534c 989 *pattern = N_Pulse;
open4416 18:780366f2534c 990 *phase = 0U;
open4416 18:780366f2534c 991 break;
open4416 18:780366f2534c 992 }
open4416 18:780366f2534c 993 return refresh;
open4416 18:780366f2534c 994 }
open4416 18:780366f2534c 995
open4416 18:780366f2534c 996 // else all XXX_ind is cleared out, set refresh
open4416 18:780366f2534c 997 refresh = 1U;
open4416 18:780366f2534c 998 return refresh;
open4416 18:780366f2534c 999 }
open4416 6:fbe401163489 1000
open4416 8:f8b1402c8f3c 1001 void Cooler(void)
open4416 8:f8b1402c8f3c 1002 {
open4416 8:f8b1402c8f3c 1003 //Cooling auto control, unfinish 2019/11/15
open4416 9:c99eeafa6fa3 1004 Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
open4416 9:c99eeafa6fa3 1005 Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
open4416 24:518ec8a4fb6d 1006
open4416 24:518ec8a4fb6d 1007 //2020/6/14 only for test use AWD_HMI
open4416 25:3c6e83b449b2 1008 if(RTD_SW) {
open4416 25:3c6e83b449b2 1009 Aux_Rly = 0;
open4416 25:3c6e83b449b2 1010 } else {
open4416 8:f8b1402c8f3c 1011 Aux_Rly = 1;
open4416 8:f8b1402c8f3c 1012 }
open4416 8:f8b1402c8f3c 1013 }
open4416 8:f8b1402c8f3c 1014
open4416 9:c99eeafa6fa3 1015 int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
open4416 2:c7a3a8c1aeed 1016 {
open4416 2:c7a3a8c1aeed 1017 int16_t max = i1;
open4416 2:c7a3a8c1aeed 1018 if(i2 > max) max = i2;
open4416 2:c7a3a8c1aeed 1019 if(i3 > max) max = i3;
open4416 6:fbe401163489 1020 if(i4 > max) max = i4;
open4416 2:c7a3a8c1aeed 1021 return max;
open4416 6:fbe401163489 1022 }
open4416 9:c99eeafa6fa3 1023
open4416 9:c99eeafa6fa3 1024 float max_fval(float i1, float i2, float i3, float i4)
open4416 9:c99eeafa6fa3 1025 {
open4416 9:c99eeafa6fa3 1026 float max = i1;
open4416 9:c99eeafa6fa3 1027 if(i2 > max) max = i2;
open4416 9:c99eeafa6fa3 1028 if(i3 > max) max = i3;
open4416 9:c99eeafa6fa3 1029 if(i4 > max) max = i4;
open4416 9:c99eeafa6fa3 1030 return max;
open4416 9:c99eeafa6fa3 1031 }