SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@24:518ec8a4fb6d, 2020-07-19 (annotated)

Committer:
open4416
Date:
Sun Jul 19 03:27:01 2020 +0000
Revision:
24:518ec8a4fb6d
Parent:
23:100661e2ad70
Child:
25:3c6e83b449b2
Before testing virtual differential back up

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