SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@13:ac024885d0bf, 2019-11-19 (annotated)

Committer:
open4416
Date:
Tue Nov 19 11:54:12 2019 +0000
Revision:
13:ac024885d0bf
Parent:
9:c99eeafa6fa3
Child:
14:bcf5cb4d08a5
Fix error after test & add brk sensor

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 9:c99eeafa6fa3 3 #include "imu_driver.hpp"
open4416 8:f8b1402c8f3c 4 #define pi 3.14159265359f
open4416 8:f8b1402c8f3c 5 #define d2r 0.01745329252f
open4416 2:c7a3a8c1aeed 6 #define dt 0.01f
open4416 6:fbe401163489 7
open4416 5:8116016abee0 8 DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
open4416 9:c99eeafa6fa3 9 DigitalOut Fault_Ind(PC_12,0); //Indicate fault bt flashing, 1 active
open4416 2:c7a3a8c1aeed 10 DigitalOut LED(D13, 0); //Internal LED output, general purpose
open4416 6:fbe401163489 11 AnalogIn AUX_1(PC_0); //Auxilaru analog sensor
open4416 13:ac024885d0bf 12 AnalogIn AUX_2(PC_3);
open4416 6:fbe401163489 13 AnalogIn AUX_3(PC_2);
open4416 6:fbe401163489 14 AnalogIn AUX_4(PC_1);
open4416 7:f674ef860c9c 15 AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection
open4416 13:ac024885d0bf 16 AnalogIn Brk_sense(PA_4); //Brake sensor readings
open4416 9:c99eeafa6fa3 17 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
open4416 9:c99eeafa6fa3 18 SPI spi2(PB_15, PB_14, PB_13); //1Mbps default, MOSI MISO SCLK, forIMU
open4416 9:c99eeafa6fa3 19 ImuDriver <spi2, PA_13, PA_14, PA_15> imu; //SPI instance, reset, data ready, slave select
open4416 9:c99eeafa6fa3 20 Serial pc(USBTX, USBRX, 115200);
open4416 9:c99eeafa6fa3 21 Ticker ticker1; //100Hz task
open4416 5:8116016abee0 22 CANMessage can_msg_Rx;
open4416 5:8116016abee0 23 CANMessage can_msg_Tx;
open4416 2:c7a3a8c1aeed 24
open4416 2:c7a3a8c1aeed 25 void timer1_interrupt(void)
open4416 2:c7a3a8c1aeed 26 {
open4416 6:fbe401163489 27 HSTick += 1;
open4416 6:fbe401163489 28 LSTick += 1;
open4416 6:fbe401163489 29 if (HSTick > 9) { // 100Hz
open4416 6:fbe401163489 30 HST_EXFL = 1;
open4416 6:fbe401163489 31 HSTick = 0;
open4416 6:fbe401163489 32 }
open4416 6:fbe401163489 33 if (LSTick > 99) { // 10Hz
open4416 6:fbe401163489 34 LST_EXFL = 1;
open4416 6:fbe401163489 35 LSTick = 0;
open4416 6:fbe401163489 36 }
open4416 2:c7a3a8c1aeed 37 }
open4416 0:c4747ebbe0b4 38
open4416 0:c4747ebbe0b4 39 int main()
open4416 0:c4747ebbe0b4 40 {
open4416 6:fbe401163489 41 //Init CAN network
open4416 9:c99eeafa6fa3 42 CAN_init(); // Note now in Gloable test mode only for testing 2019/11/17
open4416 6:fbe401163489 43
open4416 6:fbe401163489 44 //Start House keeping task
open4416 6:fbe401163489 45 printf("VDU start up, pend for module online\n");
open4416 6:fbe401163489 46 ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
open4416 6:fbe401163489 47 while(1) {
open4416 6:fbe401163489 48
open4416 6:fbe401163489 49 // Do high speed loop
open4416 6:fbe401163489 50 if (HST_EXFL == 1) {
open4416 6:fbe401163489 51 HST_EXFL = 0;
open4416 6:fbe401163489 52
open4416 9:c99eeafa6fa3 53 // Get IMU, Auxs, Max Temperature
open4416 9:c99eeafa6fa3 54 Cooler();
open4416 6:fbe401163489 55 IMU_read();
open4416 6:fbe401163489 56 Aux_read();
open4416 9:c99eeafa6fa3 57 Module_WD();
open4416 6:fbe401163489 58
open4416 6:fbe401163489 59 // Run state machine
open4416 6:fbe401163489 60 switch (VDU_STAT) {
open4416 6:fbe401163489 61
open4416 6:fbe401163489 62 case VDU_PowerOn:
open4416 6:fbe401163489 63 /* Power on state
open4416 6:fbe401163489 64 * Description:
open4416 6:fbe401163489 65 * Simple start up sequence will be done here
open4416 6:fbe401163489 66 * Do:
open4416 6:fbe401163489 67 * VDU internal POST
open4416 6:fbe401163489 68 * Wait till modules + PSU online
open4416 6:fbe401163489 69 * To VDU_Idle (RTD off):
open4416 6:fbe401163489 70 * Prepare for 4WD main program
open4416 6:fbe401163489 71 * To VDU_Fault:
open4416 6:fbe401163489 72 * Run the error handling service
open4416 6:fbe401163489 73 */
open4416 6:fbe401163489 74
open4416 6:fbe401163489 75 //Basic IMU test
open4416 6:fbe401163489 76 POST();
open4416 6:fbe401163489 77
open4416 6:fbe401163489 78 //Check if state transition only when all module online
open4416 7:f674ef860c9c 79 // if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
open4416 7:f674ef860c9c 80 if(1) { //Force online check pass only for debug 2019/11/14
open4416 6:fbe401163489 81
open4416 6:fbe401163489 82 if ( //Check if any error
open4416 6:fbe401163489 83 (FL_DSM == 3U)||
open4416 6:fbe401163489 84 (FR_DSM == 3U)||
open4416 6:fbe401163489 85 (RL_DSM == 3U)||
open4416 6:fbe401163489 86 (RR_DSM == 3U)||
open4416 6:fbe401163489 87 (VDU_FLT != 0)) {
open4416 6:fbe401163489 88 VDU_STAT = VDU_Fault;
open4416 6:fbe401163489 89 FLT_print = 1;
open4416 13:ac024885d0bf 90 } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) {
open4416 6:fbe401163489 91 //All module online & POST pass
open4416 6:fbe401163489 92 VDU_STAT = VDU_Idle;
open4416 6:fbe401163489 93 printf("All module online, VDU now Idle\n");
open4416 6:fbe401163489 94 }
open4416 6:fbe401163489 95 } //Else keep in state VDU_PowerOn
open4416 6:fbe401163489 96 break;
open4416 6:fbe401163489 97
open4416 6:fbe401163489 98 case VDU_Idle:
open4416 6:fbe401163489 99 /* Controller Idle state
open4416 6:fbe401163489 100 * Description:
open4416 6:fbe401163489 101 * Normal latched state, wait for RTD_HMI set from PSU
open4416 6:fbe401163489 102 * 4WD in running but output mask to 0
open4416 6:fbe401163489 103 * Do:
open4416 6:fbe401163489 104 * 4WD controller
open4416 6:fbe401163489 105 * Check:
open4416 6:fbe401163489 106 * RUN faults if any
open4416 6:fbe401163489 107 * To VDU_Run:
open4416 6:fbe401163489 108 * Initialize parameters for start up, set RTD_cmd
open4416 6:fbe401163489 109 * To VDU_Fault:
open4416 6:fbe401163489 110 * Run the error handling service
open4416 6:fbe401163489 111 */
open4416 6:fbe401163489 112
open4416 6:fbe401163489 113 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 13:ac024885d0bf 114 // RTD_HMI = 1; //Should be set if can bus received data
open4416 6:fbe401163489 115 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 6:fbe401163489 116
open4416 6:fbe401163489 117 RUNT(); //Run test
open4416 6:fbe401163489 118 AWD(); //AWD main program
open4416 1:8a9ac822aab7 119
open4416 6:fbe401163489 120 if ( //Check if any error
open4416 6:fbe401163489 121 (FL_DSM == 3U)||
open4416 6:fbe401163489 122 (FR_DSM == 3U)||
open4416 6:fbe401163489 123 (RL_DSM == 3U)||
open4416 6:fbe401163489 124 (RR_DSM == 3U)||
open4416 6:fbe401163489 125 (VDU_FLT != 0)) {
open4416 6:fbe401163489 126 VDU_STAT = VDU_Fault;
open4416 7:f674ef860c9c 127 printf("Idle 2 Fault\n");
open4416 6:fbe401163489 128 FLT_print = 1;
open4416 6:fbe401163489 129 } else if (RTD_HMI != 0) { //Or command to run threw PSU
open4416 6:fbe401163489 130 //Prepare to send out RTD and start motor
open4416 6:fbe401163489 131 Idle2Run();
open4416 6:fbe401163489 132 VDU_STAT = VDU_Run;
open4416 6:fbe401163489 133 printf("Idle 2 Run\n");
open4416 6:fbe401163489 134 } //Else keep in state
open4416 6:fbe401163489 135 break;
open4416 6:fbe401163489 136
open4416 6:fbe401163489 137 case VDU_Run:
open4416 6:fbe401163489 138 /* Controller Run state
open4416 6:fbe401163489 139 * Description:
open4416 6:fbe401163489 140 * Normal latched state, after RTD_HMI is set from PSU
open4416 6:fbe401163489 141 * Same to Idle state except RTD_cmd is set
open4416 6:fbe401163489 142 * Do:
open4416 6:fbe401163489 143 * 4WD controller
open4416 6:fbe401163489 144 * Check:
open4416 6:fbe401163489 145 * RUN faults if any
open4416 6:fbe401163489 146 * To VDU_Idle:
open4416 6:fbe401163489 147 * Initialize parameters for idling, reset RTD_cmd
open4416 6:fbe401163489 148 * To VDU_Fault:
open4416 6:fbe401163489 149 * Run the error handling service
open4416 6:fbe401163489 150 */
open4416 6:fbe401163489 151
open4416 6:fbe401163489 152 RUNT(); //Run test
open4416 6:fbe401163489 153 AWD(); //AWD main program
open4416 6:fbe401163489 154
open4416 6:fbe401163489 155 //Temporary debug posting area 2019/11/14
open4416 6:fbe401163489 156 //printf("%d,%d\n", Encoder_cnt, Encoder_del);
open4416 6:fbe401163489 157 //printf("%d\n\r", (int16_t)Tmodule);//
open4416 6:fbe401163489 158 //printf("%d\n\r", (int16_t)Vbus);
open4416 0:c4747ebbe0b4 159
open4416 6:fbe401163489 160 if ( //Check if any error
open4416 6:fbe401163489 161 (FL_DSM == 3U)||
open4416 6:fbe401163489 162 (FR_DSM == 3U)||
open4416 6:fbe401163489 163 (RL_DSM == 3U)||
open4416 6:fbe401163489 164 (RR_DSM == 3U)||
open4416 6:fbe401163489 165 (VDU_FLT != 0)) {
open4416 6:fbe401163489 166 VDU_STAT = VDU_Fault;
open4416 7:f674ef860c9c 167 printf("Run 2 Fault\n");
open4416 6:fbe401163489 168 FLT_print = 1;
open4416 6:fbe401163489 169 } else if (RTD_HMI != 1) { //Or command to stop threw can bus
open4416 6:fbe401163489 170 Run2Idle();
open4416 6:fbe401163489 171 VDU_STAT = VDU_Idle;
open4416 6:fbe401163489 172 printf("Run 2 Idle\n");
open4416 6:fbe401163489 173 } //Else keep in state
open4416 6:fbe401163489 174 break;
open4416 6:fbe401163489 175
open4416 6:fbe401163489 176 case VDU_Fault:
open4416 6:fbe401163489 177 /* Controller Fault state
open4416 6:fbe401163489 178 * Description:
open4416 6:fbe401163489 179 * Fault latched state if any faults is detected
open4416 6:fbe401163489 180 * Same to Idle state except keep till RTD_HMI reset
open4416 6:fbe401163489 181 * Do:
open4416 6:fbe401163489 182 * Nothing, like a piece of shit
open4416 6:fbe401163489 183 * Check:
open4416 6:fbe401163489 184 * RUN faults if any
open4416 6:fbe401163489 185 * To VDU_PowerOn:
open4416 6:fbe401163489 186 * Restart VDU
open4416 6:fbe401163489 187 */
open4416 6:fbe401163489 188
open4416 6:fbe401163489 189 RUNT(); //Run test
open4416 6:fbe401163489 190
open4416 6:fbe401163489 191 if (RST_HMI == 1) { //PSU reset to clear error
open4416 6:fbe401163489 192 RST_cmd = 1;
open4416 6:fbe401163489 193 FLT_print = 0;
open4416 6:fbe401163489 194 VDU_STAT = VDU_PowerOn;
open4416 6:fbe401163489 195 printf("Module reset\nVDU restarting...\n");
open4416 6:fbe401163489 196 } //Else keep in state
open4416 6:fbe401163489 197 break;
open4416 6:fbe401163489 198 }
open4416 6:fbe401163489 199
open4416 7:f674ef860c9c 200 // Shit out torque distribution and special command
open4416 6:fbe401163489 201 if(VDU_STAT == VDU_Run) {
open4416 6:fbe401163489 202 //Allow output torque
open4416 6:fbe401163489 203 Tx_Tcmd_CAN1();
open4416 6:fbe401163489 204 } else if(RST_cmd != 0) {
open4416 7:f674ef860c9c 205 //Send out reset cmd once
open4416 6:fbe401163489 206 Tx_CLRerr_CAN1();
open4416 6:fbe401163489 207 } else {
open4416 6:fbe401163489 208 //Force RTD off when not in VDU_Run
open4416 6:fbe401163489 209 Tx_Estop_CAN1();
open4416 6:fbe401163489 210 }
open4416 6:fbe401163489 211
open4416 6:fbe401163489 212 }
open4416 7:f674ef860c9c 213 // End of high speed loop
open4416 6:fbe401163489 214
open4416 9:c99eeafa6fa3 215 // Do low speed state reporting 10 Hz
open4416 6:fbe401163489 216 if (LST_EXFL == 1) {
open4416 6:fbe401163489 217 LST_EXFL = 0;
open4416 8:f8b1402c8f3c 218 Cooler();
open4416 5:8116016abee0 219 Tx_Qdrv_CAN1();
open4416 6:fbe401163489 220
open4416 6:fbe401163489 221 // Print out error mesagge if exist, 1Hz repeative
open4416 6:fbe401163489 222 if(FLT_print != 0) {
open4416 7:f674ef860c9c 223 FLT_print_cnt += FLT_print;
open4416 6:fbe401163489 224 if(FLT_print_cnt > 19) {
open4416 6:fbe401163489 225 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 7:f674ef860c9c 226 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 7:f674ef860c9c 227 printf("VDU\n0x%04X\n\n", VDU_FLT);
open4416 6:fbe401163489 228 FLT_print_cnt = 0;
open4416 6:fbe401163489 229 }
open4416 6:fbe401163489 230 }
open4416 7:f674ef860c9c 231 }
open4416 7:f674ef860c9c 232 // End of low speed state reporting
open4416 6:fbe401163489 233
open4416 7:f674ef860c9c 234 } // end of while
open4416 0:c4747ebbe0b4 235 }
open4416 0:c4747ebbe0b4 236
open4416 6:fbe401163489 237 void Idle2Run(void)
open4416 6:fbe401163489 238 {
open4416 6:fbe401163489 239 RTD_cmd = 1;
open4416 6:fbe401163489 240 }
open4416 6:fbe401163489 241
open4416 6:fbe401163489 242 void Run2Idle(void)
open4416 6:fbe401163489 243 {
open4416 6:fbe401163489 244 RTD_cmd = 0;
open4416 6:fbe401163489 245 }
open4416 6:fbe401163489 246
open4416 6:fbe401163489 247 void POST(void)
open4416 6:fbe401163489 248 {
open4416 8:f8b1402c8f3c 249 //Check IMU status abnormal
open4416 8:f8b1402c8f3c 250 if(fabsf(YR_imu) > pi) { //half turn per sec, strange
open4416 6:fbe401163489 251 VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
open4416 6:fbe401163489 252 }
open4416 6:fbe401163489 253 }
open4416 6:fbe401163489 254
open4416 6:fbe401163489 255 void RUNT(void)
open4416 6:fbe401163489 256 {
open4416 8:f8b1402c8f3c 257 //POST
open4416 6:fbe401163489 258 POST();
open4416 6:fbe401163489 259
open4416 8:f8b1402c8f3c 260 //Check module response timeout
open4416 6:fbe401163489 261 if((FL_online*FR_online*RL_online*RR_online) == 0) {
open4416 6:fbe401163489 262 VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout
open4416 6:fbe401163489 263 }
open4416 6:fbe401163489 264 if(PSU_online == 0) {
open4416 6:fbe401163489 265 VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout
open4416 6:fbe401163489 266 }
open4416 6:fbe401163489 267
open4416 6:fbe401163489 268 //Check ShutDrv voltage
open4416 8:f8b1402c8f3c 269 if(VDU_STAT == VDU_Run) {
open4416 8:f8b1402c8f3c 270 if(SDn_voltage < 8.0f) {
open4416 8:f8b1402c8f3c 271 VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv
open4416 8:f8b1402c8f3c 272 }
open4416 8:f8b1402c8f3c 273 }
open4416 6:fbe401163489 274 }
open4416 6:fbe401163489 275
open4416 6:fbe401163489 276 void Aux_read(void)
open4416 6:fbe401163489 277 {
open4416 7:f674ef860c9c 278 //Capture analog in at once
open4416 13:ac024885d0bf 279 AUX_1_u16 = AUX_1.read_u16() >> 4U;
open4416 13:ac024885d0bf 280 AUX_2_u16 = AUX_2.read_u16() >> 4U;
open4416 13:ac024885d0bf 281 AUX_3_u16 = AUX_3.read_u16() >> 4U;
open4416 13:ac024885d0bf 282 AUX_4_u16 = AUX_4.read_u16() >> 4U;
open4416 8:f8b1402c8f3c 283 SDn_voltage = 18.81f*SDn_sense.read(); //Read in Shutdown circuit voltage in output end
open4416 13:ac024885d0bf 284 Brk_voltage = 5.5f*Brk_sense.read();
open4416 6:fbe401163489 285 }
open4416 6:fbe401163489 286
open4416 6:fbe401163489 287 void IMU_read(void)
open4416 6:fbe401163489 288 {
open4416 8:f8b1402c8f3c 289 //Get readings threw SPI, unfinished 2019/11/15
open4416 8:f8b1402c8f3c 290 YR_imu = 0.0f;
open4416 8:f8b1402c8f3c 291 Ax_imu = 0.0f;
open4416 8:f8b1402c8f3c 292 Ay_imu = 0.0f;
open4416 8:f8b1402c8f3c 293 Roll_imu = 0.0f;
open4416 8:f8b1402c8f3c 294 Pitch_imu = 0.0f;
open4416 6:fbe401163489 295 }
open4416 6:fbe401163489 296
open4416 6:fbe401163489 297 void AWD(void)
open4416 6:fbe401163489 298 {
open4416 8:f8b1402c8f3c 299 if(AWD_HMI) {
open4416 8:f8b1402c8f3c 300 // A simple version is put here for reading
open4416 8:f8b1402c8f3c 301 Vb_est = 0.25f * (FL_W_ele + FR_W_ele + RL_W_ele + RR_W_ele);
open4416 8:f8b1402c8f3c 302 YR_ref = Steer_HMI*d2r*Vb_est/(Base+EG*Vb_est*Vb_est);
open4416 8:f8b1402c8f3c 303 Mz_reg = (YR_ref - YR_imu) * K_yaw; //K_yaw unfinished 2019/11/15
open4416 8:f8b1402c8f3c 304 sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
open4416 8:f8b1402c8f3c 305 FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
open4416 8:f8b1402c8f3c 306 FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
open4416 8:f8b1402c8f3c 307 RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
open4416 8:f8b1402c8f3c 308 RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);
open4416 8:f8b1402c8f3c 309 } else {
open4416 8:f8b1402c8f3c 310 FL_Tcmd = 0.25f*Trq_HMI;
open4416 8:f8b1402c8f3c 311 FR_Tcmd = 0.25f*Trq_HMI;
open4416 8:f8b1402c8f3c 312 RL_Tcmd = 0.25f*Trq_HMI;
open4416 8:f8b1402c8f3c 313 RR_Tcmd = 0.25f*Trq_HMI;
open4416 8:f8b1402c8f3c 314 }
open4416 8:f8b1402c8f3c 315 }
open4416 6:fbe401163489 316
open4416 8:f8b1402c8f3c 317 void ASL(void)
open4416 8:f8b1402c8f3c 318 {
open4416 8:f8b1402c8f3c 319 //Filter out each motor W_ele and get approximate accel, compare with IMU
open4416 8:f8b1402c8f3c 320 //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel)
open4416 8:f8b1402c8f3c 321 //Fill out if enough time
open4416 6:fbe401163489 322 }
open4416 6:fbe401163489 323
open4416 5:8116016abee0 324 void Rx_CAN1(void)
open4416 5:8116016abee0 325 {
open4416 5:8116016abee0 326 LED = 1;
open4416 7:f674ef860c9c 327 int16_t tmp;
open4416 7:f674ef860c9c 328
open4416 5:8116016abee0 329 if(can1.read(can_msg_Rx)) {
open4416 7:f674ef860c9c 330 switch(can_msg_Rx.id) { //Filtered input message
open4416 7:f674ef860c9c 331 // Start of 100Hz msg
open4416 8:f8b1402c8f3c 332 case FL_HSB_ID://1
open4416 7:f674ef860c9c 333 //HSB from FL motor drive
open4416 7:f674ef860c9c 334 FL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 335 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 336 FL_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 337 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 338 FL_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 339 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 340 FL_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 341 FL_online = 3;
open4416 7:f674ef860c9c 342 break;
open4416 7:f674ef860c9c 343
open4416 8:f8b1402c8f3c 344 case FR_HSB_ID://2
open4416 7:f674ef860c9c 345 //HSB from FR motor drive
open4416 7:f674ef860c9c 346 FR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 347 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 348 FR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 349 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 350 FR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 351 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 352 FR_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 353 FR_online = 3;
open4416 7:f674ef860c9c 354 break;
open4416 7:f674ef860c9c 355
open4416 8:f8b1402c8f3c 356 case RL_HSB_ID://3
open4416 7:f674ef860c9c 357 //HSB from RL motor drive
open4416 7:f674ef860c9c 358 RL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 359 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 360 RL_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 361 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 362 RL_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 363 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 364 RL_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 365 RL_online = 3;
open4416 7:f674ef860c9c 366 break;
open4416 7:f674ef860c9c 367
open4416 8:f8b1402c8f3c 368 case RR_HSB_ID://4
open4416 7:f674ef860c9c 369 //HSB from RR motor drive
open4416 7:f674ef860c9c 370 RR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 371 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 372 RR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 373 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 374 RR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 375 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 376 RR_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 377 RR_online = 3;
open4416 7:f674ef860c9c 378 break;
open4416 7:f674ef860c9c 379
open4416 8:f8b1402c8f3c 380 case HMI_cmd_ID://5
open4416 7:f674ef860c9c 381 //HMI command from PSU
open4416 7:f674ef860c9c 382 AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch
open4416 7:f674ef860c9c 383 RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
open4416 7:f674ef860c9c 384 RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch
open4416 7:f674ef860c9c 385 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 386 Steer_HMI = tmp * 0.01f;
open4416 7:f674ef860c9c 387 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 388 Trq_HMI = tmp * 0.01f;
open4416 7:f674ef860c9c 389 PSU_online = 3;
open4416 7:f674ef860c9c 390 break;
open4416 7:f674ef860c9c 391 // end of 100Hz msg
open4416 7:f674ef860c9c 392
open4416 7:f674ef860c9c 393 // Start of 10Hz msg
open4416 8:f8b1402c8f3c 394 case FL_LSB_ID://6
open4416 7:f674ef860c9c 395 //LSB from FL motor drive
open4416 7:f674ef860c9c 396 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 397 FL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 398 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 399 FL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 400 FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 401 FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 402 break;
open4416 7:f674ef860c9c 403
open4416 8:f8b1402c8f3c 404 case FR_LSB_ID://7
open4416 7:f674ef860c9c 405 //LSB from FR motor drive
open4416 7:f674ef860c9c 406 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 407 FR_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 408 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 409 FR_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 410 FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 411 FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 412 break;
open4416 7:f674ef860c9c 413
open4416 8:f8b1402c8f3c 414 case RL_LSB_ID://8
open4416 7:f674ef860c9c 415 //LSB from RL motor drive
open4416 7:f674ef860c9c 416 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 417 RL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 418 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 419 RL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 420 RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 421 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 422 break;
open4416 7:f674ef860c9c 423
open4416 8:f8b1402c8f3c 424 case RR_LSB_ID://9
open4416 7:f674ef860c9c 425 //LSB from RR motor drive
open4416 7:f674ef860c9c 426 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 8:f8b1402c8f3c 427 RR_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 428 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 8:f8b1402c8f3c 429 RR_Tmodule = tmp*0.01f;
open4416 8:f8b1402c8f3c 430 RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 8:f8b1402c8f3c 431 RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 432 break;
open4416 7:f674ef860c9c 433 // end of 10Hz msg
open4416 7:f674ef860c9c 434 }
open4416 5:8116016abee0 435 }
open4416 5:8116016abee0 436 LED = 0;
open4416 5:8116016abee0 437 }
open4416 5:8116016abee0 438
open4416 6:fbe401163489 439 void Tx_CLRerr_CAN1(void)
open4416 2:c7a3a8c1aeed 440 {
open4416 9:c99eeafa6fa3 441 Tx_Estop_CAN1(); //disable as default
open4416 9:c99eeafa6fa3 442 RST_cmd = 0; //clear out on shot
open4416 6:fbe401163489 443 }
open4416 6:fbe401163489 444
open4416 6:fbe401163489 445 void Tx_Estop_CAN1(void)
open4416 6:fbe401163489 446 {
open4416 9:c99eeafa6fa3 447 RTD_cmd = 0; //force disable
open4416 6:fbe401163489 448 Tx_Tcmd_CAN1();
open4416 2:c7a3a8c1aeed 449 }
open4416 2:c7a3a8c1aeed 450
open4416 9:c99eeafa6fa3 451 void Tx_Tcmd_CAN1(void) // 100 Hz
open4416 2:c7a3a8c1aeed 452 {
open4416 7:f674ef860c9c 453 int16_t tmp;
open4416 7:f674ef860c9c 454 tmp = (int16_t) (FL_Tcmd * 100.0f);
open4416 7:f674ef860c9c 455 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 456 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 457 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 458 temp_msg[3] = RST_cmd;
open4416 8:f8b1402c8f3c 459 can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 460 CANpendTX();
open4416 6:fbe401163489 461 can1.write(can_msg_Tx);
open4416 6:fbe401163489 462
open4416 7:f674ef860c9c 463 tmp = (int16_t) (FR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 464 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 465 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 466 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 467 temp_msg[3] = RST_cmd;
open4416 8:f8b1402c8f3c 468 can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 469 CANpendTX();
open4416 6:fbe401163489 470 can1.write(can_msg_Tx);
open4416 6:fbe401163489 471
open4416 7:f674ef860c9c 472 tmp = (int16_t) (RL_Tcmd * 100.0f);
open4416 7:f674ef860c9c 473 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 474 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 475 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 476 temp_msg[3] = RST_cmd;
open4416 8:f8b1402c8f3c 477 can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 478 CANpendTX();
open4416 6:fbe401163489 479 can1.write(can_msg_Tx);
open4416 6:fbe401163489 480
open4416 7:f674ef860c9c 481 tmp = (int16_t) (RR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 482 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 483 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 484 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 485 temp_msg[3] = RST_cmd;
open4416 8:f8b1402c8f3c 486 can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 487 CANpendTX();
open4416 6:fbe401163489 488 can1.write(can_msg_Tx);
open4416 6:fbe401163489 489
open4416 2:c7a3a8c1aeed 490 }
open4416 2:c7a3a8c1aeed 491
open4416 6:fbe401163489 492 void Tx_Qdrv_CAN1(void) // 10 Hz
open4416 0:c4747ebbe0b4 493 {
open4416 8:f8b1402c8f3c 494 int16_t tmp;
open4416 6:fbe401163489 495 // Auxilary sensor reading shitting out
open4416 7:f674ef860c9c 496 temp_msg[0] = AUX_1_u16;
open4416 7:f674ef860c9c 497 temp_msg[1] = AUX_1_u16 >> 8U;
open4416 7:f674ef860c9c 498 temp_msg[2] = AUX_2_u16;
open4416 7:f674ef860c9c 499 temp_msg[3] = AUX_2_u16 >> 8U;
open4416 7:f674ef860c9c 500 temp_msg[4] = AUX_3_u16;
open4416 7:f674ef860c9c 501 temp_msg[5] = AUX_3_u16 >> 8U;
open4416 7:f674ef860c9c 502 temp_msg[6] = AUX_4_u16;
open4416 7:f674ef860c9c 503 temp_msg[7] = AUX_4_u16 >> 8U;
open4416 6:fbe401163489 504 can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 505 CANpendTX();
open4416 6:fbe401163489 506 can1.write(can_msg_Tx);
open4416 6:fbe401163489 507
open4416 6:fbe401163489 508 // Qdrive internal state shitting out
open4416 8:f8b1402c8f3c 509 temp_msg[0] = VDU_FLT;
open4416 8:f8b1402c8f3c 510 temp_msg[1] = VDU_FLT >> 8U;
open4416 8:f8b1402c8f3c 511 temp_msg[2] = VDU_STAT;
open4416 6:fbe401163489 512 can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 513 CANpendTX();
open4416 6:fbe401163489 514 can1.write(can_msg_Tx);
open4416 2:c7a3a8c1aeed 515
open4416 6:fbe401163489 516 // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
open4416 8:f8b1402c8f3c 517 tmp = (int16_t) (YR_imu * 10000.0f);
open4416 8:f8b1402c8f3c 518 temp_msg[0] = tmp;
open4416 8:f8b1402c8f3c 519 temp_msg[1] = tmp >> 8U;
open4416 8:f8b1402c8f3c 520 tmp = (int16_t) (Roll_imu * 100.0f);
open4416 8:f8b1402c8f3c 521 temp_msg[2] = tmp;
open4416 8:f8b1402c8f3c 522 temp_msg[3] = tmp >> 8U;
open4416 8:f8b1402c8f3c 523 tmp = (int16_t) (Pitch_imu * 100.0f);
open4416 8:f8b1402c8f3c 524 temp_msg[4] = tmp;
open4416 8:f8b1402c8f3c 525 temp_msg[5] = tmp >> 8U;
open4416 8:f8b1402c8f3c 526 temp_msg[6] = (int8_t)Ax_imu;
open4416 8:f8b1402c8f3c 527 temp_msg[7] = (int8_t)Ay_imu;
open4416 6:fbe401163489 528 can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 529 CANpendTX();
open4416 6:fbe401163489 530 can1.write(can_msg_Tx);
open4416 6:fbe401163489 531 }
open4416 6:fbe401163489 532
open4416 6:fbe401163489 533 void CANpendTX(void)
open4416 6:fbe401163489 534 {
open4416 6:fbe401163489 535 //Pend till TX box has empty slot, timeout will generate error
open4416 6:fbe401163489 536 uint32_t timeout = 0;
open4416 6:fbe401163489 537 while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
open4416 6:fbe401163489 538 //Wait till non empty
open4416 6:fbe401163489 539 timeout += 1;
open4416 6:fbe401163489 540 if(timeout > 10000) {
open4416 6:fbe401163489 541 // Put some timeout error handler
open4416 6:fbe401163489 542 break;
open4416 6:fbe401163489 543 }
open4416 0:c4747ebbe0b4 544 }
open4416 0:c4747ebbe0b4 545 }
open4416 1:8a9ac822aab7 546
open4416 6:fbe401163489 547 void CAN_init(void)
open4416 6:fbe401163489 548 {
open4416 6:fbe401163489 549 //Set CAN system
open4416 6:fbe401163489 550 SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off
open4416 6:fbe401163489 551 can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode
open4416 6:fbe401163489 552 can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1);
open4416 6:fbe401163489 553 can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2);
open4416 6:fbe401163489 554 can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3);
open4416 6:fbe401163489 555 can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4);
open4416 6:fbe401163489 556 can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
open4416 6:fbe401163489 557 can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
open4416 6:fbe401163489 558 can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
open4416 8:f8b1402c8f3c 559 can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring
open4416 6:fbe401163489 560 can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
open4416 8:f8b1402c8f3c 561 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq
open4416 6:fbe401163489 562 }
open4416 2:c7a3a8c1aeed 563
open4416 6:fbe401163489 564 void Module_WD(void)
open4416 6:fbe401163489 565 {
open4416 9:c99eeafa6fa3 566 //Module online dissipitive indicator
open4416 6:fbe401163489 567 if (FL_online != 0) {
open4416 6:fbe401163489 568 FL_online -= 1;
open4416 6:fbe401163489 569 }
open4416 6:fbe401163489 570 if (FR_online != 0) {
open4416 6:fbe401163489 571 FR_online -= 1;
open4416 6:fbe401163489 572 }
open4416 6:fbe401163489 573 if (RL_online != 0) {
open4416 6:fbe401163489 574 RL_online -= 1;
open4416 6:fbe401163489 575 }
open4416 6:fbe401163489 576 if (RR_online != 0) {
open4416 6:fbe401163489 577 RR_online -= 1;
open4416 6:fbe401163489 578 }
open4416 6:fbe401163489 579 if (PSU_online != 0) {
open4416 6:fbe401163489 580 PSU_online -= 1;
open4416 6:fbe401163489 581 }
open4416 6:fbe401163489 582 }
open4416 6:fbe401163489 583
open4416 8:f8b1402c8f3c 584 void Cooler(void)
open4416 8:f8b1402c8f3c 585 {
open4416 8:f8b1402c8f3c 586 //Cooling auto control, unfinish 2019/11/15
open4416 9:c99eeafa6fa3 587 Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
open4416 9:c99eeafa6fa3 588 Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
open4416 8:f8b1402c8f3c 589 if(0) {
open4416 8:f8b1402c8f3c 590 Aux_Rly = 1;
open4416 8:f8b1402c8f3c 591 } else {
open4416 8:f8b1402c8f3c 592 Aux_Rly = 0;
open4416 8:f8b1402c8f3c 593 }
open4416 8:f8b1402c8f3c 594 }
open4416 8:f8b1402c8f3c 595
open4416 9:c99eeafa6fa3 596 int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
open4416 2:c7a3a8c1aeed 597 {
open4416 2:c7a3a8c1aeed 598 int16_t max = i1;
open4416 2:c7a3a8c1aeed 599 if(i2 > max) max = i2;
open4416 2:c7a3a8c1aeed 600 if(i3 > max) max = i3;
open4416 6:fbe401163489 601 if(i4 > max) max = i4;
open4416 2:c7a3a8c1aeed 602 return max;
open4416 6:fbe401163489 603 }
open4416 9:c99eeafa6fa3 604
open4416 9:c99eeafa6fa3 605 float max_fval(float i1, float i2, float i3, float i4)
open4416 9:c99eeafa6fa3 606 {
open4416 9:c99eeafa6fa3 607 float max = i1;
open4416 9:c99eeafa6fa3 608 if(i2 > max) max = i2;
open4416 9:c99eeafa6fa3 609 if(i3 > max) max = i3;
open4416 9:c99eeafa6fa3 610 if(i4 > max) max = i4;
open4416 9:c99eeafa6fa3 611 return max;
open4416 9:c99eeafa6fa3 612 }
open4416 8:f8b1402c8f3c 613 // pc.printf("SOC: %.2f\n", Module_Total*0.01f);