SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@7:f674ef860c9c, 2019-11-14 (annotated)

Committer:
open4416
Date:
Thu Nov 14 17:06:09 2019 +0000
Revision:
7:f674ef860c9c
Parent:
6:fbe401163489
Child:
8:f8b1402c8f3c
Fill in CAN receiving part

Who changed what in which revision?

UserRevisionLine numberNew contents of line
open4416 0:c4747ebbe0b4 1 #include "mbed.h"
open4416 2:c7a3a8c1aeed 2 #define dt 0.01f
open4416 2:c7a3a8c1aeed 3 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
open4416 5:8116016abee0 4 #define FL_HSB_ID 0xA0 // Rx, 100Hz
open4416 7:f674ef860c9c 5 #define FR_HSB_ID 0xA1 // Rx, 100Hz
open4416 7:f674ef860c9c 6 #define RL_HSB_ID 0xA2 // Rx, 100Hz
open4416 7:f674ef860c9c 7 #define RR_HSB_ID 0xA3 // Rx, 100Hz
open4416 5:8116016abee0 8 #define FL_LSB_ID 0xB0 // Rx, 10Hz
open4416 7:f674ef860c9c 9 #define FR_LSB_ID 0xB1 // Rx, 10Hz
open4416 7:f674ef860c9c 10 #define RL_LSB_ID 0xB2 // Rx, 10Hz
open4416 7:f674ef860c9c 11 #define RR_LSB_ID 0xB3 // Rx, 10Hz
open4416 7:f674ef860c9c 12 #define HMI_cmd_ID 0xC4 // Rx, 100Hz
open4416 2:c7a3a8c1aeed 13
open4416 5:8116016abee0 14 #define FL_CMD_ID 0xC0 // Tx, 100Hz
open4416 7:f674ef860c9c 15 #define FR_CMD_ID 0xC1 // Tx, 100Hz
open4416 7:f674ef860c9c 16 #define RL_CMD_ID 0xC2 // Tx, 100Hz
open4416 7:f674ef860c9c 17 #define RR_CMD_ID 0xC3 // Tx, 100Hz
open4416 5:8116016abee0 18 #define AUX_sense_ID 0xE0 // Tx, 10Hz
open4416 7:f674ef860c9c 19 #define Qdrv_stat_ID 0xE1 // Tx, 10Hz
open4416 7:f674ef860c9c 20 #define IMU_sense_ID 0xE2 // Tx, 10Hz
open4416 5:8116016abee0 21
open4416 6:fbe401163489 22 #define MODOFL_VDUFLTCode 0x0001U //Drive module timeout after once online
open4416 6:fbe401163489 23 #define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online
open4416 6:fbe401163489 24 #define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal
open4416 6:fbe401163489 25
open4416 6:fbe401163489 26
open4416 5:8116016abee0 27 DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
open4416 5:8116016abee0 28 DigitalOut Fault_Ind(PC_10,0); //Indicate fault bt flashing, 1 active
open4416 2:c7a3a8c1aeed 29 DigitalOut LED(D13, 0); //Internal LED output, general purpose
open4416 6:fbe401163489 30 AnalogIn AUX_1(PC_0); //Auxilaru analog sensor
open4416 6:fbe401163489 31 AnalogIn AUX_2(PC_4);
open4416 6:fbe401163489 32 AnalogIn AUX_3(PC_2);
open4416 6:fbe401163489 33 AnalogIn AUX_4(PC_1);
open4416 7:f674ef860c9c 34 AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection
open4416 5:8116016abee0 35 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
open4416 2:c7a3a8c1aeed 36 Serial pc(USBTX, USBRX, 115200);
open4416 5:8116016abee0 37 Ticker ticker1; //100Hz task
open4416 5:8116016abee0 38
open4416 5:8116016abee0 39 CANMessage can_msg_Rx;
open4416 5:8116016abee0 40 CANMessage can_msg_Tx;
open4416 2:c7a3a8c1aeed 41
open4416 7:f674ef860c9c 42 //CAN msg bank
open4416 5:8116016abee0 43 char temp_msg[8] = {0,0,0,0,0,0,0,0};
open4416 0:c4747ebbe0b4 44
open4416 5:8116016abee0 45 //CAN to motor drive module, 100Hz
open4416 5:8116016abee0 46 //msg ID: 0xC0~0xC3
open4416 5:8116016abee0 47 float FL_Tcmd = 0; // *100 before sent in int16_t
open4416 5:8116016abee0 48 float FR_Tcmd = 0;
open4416 5:8116016abee0 49 float RL_Tcmd = 0;
open4416 5:8116016abee0 50 float RR_Tcmd = 0;
open4416 5:8116016abee0 51 uint8_t RTD_cmd = 0; // start inverter switching cmd
open4416 5:8116016abee0 52 uint8_t RST_cmd = 0; // send out once to reset module fault
open4416 5:8116016abee0 53
open4416 5:8116016abee0 54 //Module online flag
open4416 5:8116016abee0 55 uint8_t FL_online = 0; // 0 indicate detection timeout
open4416 7:f674ef860c9c 56 uint8_t FR_online = 0; // reset value is 3 to hold for 0.03sec
open4416 5:8116016abee0 57 uint8_t RL_online = 0; // -1 per 100Hz task
open4416 5:8116016abee0 58 uint8_t RR_online = 0;
open4416 6:fbe401163489 59 uint8_t PSU_online = 0;
open4416 0:c4747ebbe0b4 60
open4416 6:fbe401163489 61 //Feedback data decoded out storage
open4416 7:f674ef860c9c 62 float FL_Tmotor = 0; // motor temperature degC, 10Hz recieving
open4416 7:f674ef860c9c 63 float FR_Tmotor = 0;
open4416 7:f674ef860c9c 64 float RL_Tmotor = 0;
open4416 7:f674ef860c9c 65 float RR_Tmotor = 0;
open4416 7:f674ef860c9c 66 float FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving
open4416 7:f674ef860c9c 67 float FR_Tmodule = 0;
open4416 7:f674ef860c9c 68 float RL_Tmodule = 0;
open4416 7:f674ef860c9c 69 float RR_Tmodule = 0;
open4416 6:fbe401163489 70 uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving
open4416 5:8116016abee0 71 uint16_t FR_FLT_Run = 0;
open4416 5:8116016abee0 72 uint16_t RL_FLT_Run = 0;
open4416 5:8116016abee0 73 uint16_t RR_FLT_Run = 0;
open4416 6:fbe401163489 74 uint16_t FL_FLT_Post = 0; // POST fault code, 10Hz recieving
open4416 5:8116016abee0 75 uint16_t FR_FLT_Post = 0;
open4416 5:8116016abee0 76 uint16_t RL_FLT_Post = 0;
open4416 5:8116016abee0 77 uint16_t RR_FLT_Post = 0;
open4416 6:fbe401163489 78 float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving
open4416 5:8116016abee0 79 float FR_Trq_fil3 = 0;
open4416 5:8116016abee0 80 float RL_Trq_fil3 = 0;
open4416 5:8116016abee0 81 float RR_Trq_fil3 = 0;
open4416 6:fbe401163489 82 float FL_Trq_est = 0; // Estimated Torque, 100Hz recieving
open4416 5:8116016abee0 83 float FR_Trq_est = 0;
open4416 5:8116016abee0 84 float RL_Trq_est = 0;
open4416 5:8116016abee0 85 float RR_Trq_est = 0;
open4416 6:fbe401163489 86 float FL_W_ele = 0; // Estimated W_ele, 100Hz recieving
open4416 5:8116016abee0 87 float FR_W_ele = 0;
open4416 5:8116016abee0 88 float RL_W_ele = 0;
open4416 5:8116016abee0 89 float RR_W_ele = 0;
open4416 6:fbe401163489 90 uint8_t FL_DSM = 0; // DSM state, 100Hz recieving
open4416 5:8116016abee0 91 uint8_t FR_DSM = 0;
open4416 5:8116016abee0 92 uint8_t RL_DSM = 0;
open4416 5:8116016abee0 93 uint8_t RR_DSM = 0;
open4416 2:c7a3a8c1aeed 94
open4416 5:8116016abee0 95 //From Pedal Box msg
open4416 6:fbe401163489 96 uint8_t RTD_HMI = 0; // 1 = HMI requesting
open4416 6:fbe401163489 97 uint8_t RST_HMI = 0; // 1 = HMI request once
open4416 6:fbe401163489 98 uint8_t AWD_HMI = 0; // 1 = HMI requesting
open4416 7:f674ef860c9c 99 float Trq_HMI = 0.0f; // Nm user request total torque
open4416 7:f674ef860c9c 100 float Steer_HMI = 0.0f; // deg steering wheel angel
open4416 0:c4747ebbe0b4 101
open4416 6:fbe401163489 102 //10/100Hz tasking
open4416 6:fbe401163489 103 uint8_t HSTick = 5; // High speed tick
open4416 6:fbe401163489 104 uint8_t LSTick = 0;
open4416 6:fbe401163489 105 uint8_t HST_EXFL = 0; // High speed execution flag
open4416 6:fbe401163489 106 uint8_t LST_EXFL = 0;
open4416 6:fbe401163489 107 uint8_t FLT_print = 0; // Send repeative error message
open4416 6:fbe401163489 108 uint8_t FLT_print_cnt = 0;
open4416 7:f674ef860c9c 109 uint16_t AUX_1_u16 = 0x0; // acquired data
open4416 7:f674ef860c9c 110 uint16_t AUX_2_u16 = 0x0;
open4416 7:f674ef860c9c 111 uint16_t AUX_3_u16 = 0x0;
open4416 7:f674ef860c9c 112 uint16_t AUX_4_u16 = 0x0;
open4416 7:f674ef860c9c 113 float SDn_voltage = 0.0f;
open4416 2:c7a3a8c1aeed 114
open4416 6:fbe401163489 115 //VDU states
open4416 6:fbe401163489 116 typedef enum {
open4416 6:fbe401163489 117 VDU_PowerOn = 0U,
open4416 6:fbe401163489 118 VDU_Idle = 1U,
open4416 6:fbe401163489 119 VDU_Run = 2U,
open4416 6:fbe401163489 120 VDU_Fault = 3U
open4416 6:fbe401163489 121 } VDU_STATE_TYPE;
open4416 6:fbe401163489 122 VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; //VDU current state
open4416 6:fbe401163489 123 uint16_t VDU_FLT = 0; //VDU internal fault code
open4416 2:c7a3a8c1aeed 124
open4416 6:fbe401163489 125 //Prototype
open4416 6:fbe401163489 126 void CAN_init(void); //Initial CAN frequency filter...
open4416 6:fbe401163489 127 void Module_WD(void); //Software watchdog indicate module state
open4416 6:fbe401163489 128 void IMU_read(void); //Update IMU readings by once
open4416 6:fbe401163489 129 void Aux_read(void); //Update AUX reafings by once
open4416 6:fbe401163489 130 void Idle2Run(void); //Initializing before running
open4416 6:fbe401163489 131 void Run2Idle(void); //Initializing before idling
open4416 6:fbe401163489 132 void POST(void); //Check IMU error
open4416 6:fbe401163489 133 void RUNT(void); //Check POST, timeout, ShutDrv voltage error
open4416 6:fbe401163489 134 void AWD(void); //AWD main program
open4416 6:fbe401163489 135 void Rx_CAN1(void); //CAN RX handler
open4416 6:fbe401163489 136 void Tx_CLRerr_CAN1(void); //Send reset cmd to modules
open4416 6:fbe401163489 137 void Tx_Estop_CAN1(void); //Send out heart beat but force RTD off
open4416 6:fbe401163489 138 void Tx_Tcmd_CAN1(void); //Send out heart beat command
open4416 6:fbe401163489 139 void Tx_Qdrv_CAN1(void); //Send out low speed heart beat for logging
open4416 6:fbe401163489 140 void CANpendTX(void); //Helper function for CAN Tx
open4416 6:fbe401163489 141 int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
open4416 2:c7a3a8c1aeed 142
open4416 2:c7a3a8c1aeed 143 void timer1_interrupt(void)
open4416 2:c7a3a8c1aeed 144 {
open4416 6:fbe401163489 145 HSTick += 1;
open4416 6:fbe401163489 146 LSTick += 1;
open4416 6:fbe401163489 147 if (HSTick > 9) { // 100Hz
open4416 6:fbe401163489 148 HST_EXFL = 1;
open4416 6:fbe401163489 149 HSTick = 0;
open4416 6:fbe401163489 150 }
open4416 6:fbe401163489 151 if (LSTick > 99) { // 10Hz
open4416 6:fbe401163489 152 LST_EXFL = 1;
open4416 6:fbe401163489 153 LSTick = 0;
open4416 6:fbe401163489 154 }
open4416 2:c7a3a8c1aeed 155 }
open4416 0:c4747ebbe0b4 156
open4416 0:c4747ebbe0b4 157 int main()
open4416 0:c4747ebbe0b4 158 {
open4416 6:fbe401163489 159 //Init CAN network
open4416 6:fbe401163489 160 CAN_init();
open4416 6:fbe401163489 161
open4416 6:fbe401163489 162 //Start House keeping task
open4416 6:fbe401163489 163 printf("VDU start up, pend for module online\n");
open4416 6:fbe401163489 164 ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
open4416 6:fbe401163489 165 while(1) {
open4416 6:fbe401163489 166
open4416 6:fbe401163489 167 // Do high speed loop
open4416 6:fbe401163489 168 if (HST_EXFL == 1) {
open4416 6:fbe401163489 169 HST_EXFL = 0;
open4416 6:fbe401163489 170
open4416 6:fbe401163489 171 // Get IMU, Auxs
open4416 6:fbe401163489 172 IMU_read();
open4416 6:fbe401163489 173 Aux_read();
open4416 6:fbe401163489 174 Module_WD(); // Module online watch dog
open4416 6:fbe401163489 175
open4416 6:fbe401163489 176 // Run state machine
open4416 6:fbe401163489 177 switch (VDU_STAT) {
open4416 6:fbe401163489 178
open4416 6:fbe401163489 179 case VDU_PowerOn:
open4416 6:fbe401163489 180 /* Power on state
open4416 6:fbe401163489 181 * Description:
open4416 6:fbe401163489 182 * Simple start up sequence will be done here
open4416 6:fbe401163489 183 * Do:
open4416 6:fbe401163489 184 * VDU internal POST
open4416 6:fbe401163489 185 * Wait till modules + PSU online
open4416 6:fbe401163489 186 * To VDU_Idle (RTD off):
open4416 6:fbe401163489 187 * Prepare for 4WD main program
open4416 6:fbe401163489 188 * To VDU_Fault:
open4416 6:fbe401163489 189 * Run the error handling service
open4416 6:fbe401163489 190 */
open4416 6:fbe401163489 191
open4416 6:fbe401163489 192 //Basic IMU test
open4416 6:fbe401163489 193 POST();
open4416 6:fbe401163489 194
open4416 6:fbe401163489 195 //Check if state transition only when all module online
open4416 7:f674ef860c9c 196 // if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
open4416 7:f674ef860c9c 197 if(1) { //Force online check pass only for debug 2019/11/14
open4416 6:fbe401163489 198
open4416 6:fbe401163489 199 if ( //Check if any error
open4416 6:fbe401163489 200 (FL_DSM == 3U)||
open4416 6:fbe401163489 201 (FR_DSM == 3U)||
open4416 6:fbe401163489 202 (RL_DSM == 3U)||
open4416 6:fbe401163489 203 (RR_DSM == 3U)||
open4416 6:fbe401163489 204 (VDU_FLT != 0)) {
open4416 6:fbe401163489 205 VDU_STAT = VDU_Fault;
open4416 6:fbe401163489 206 FLT_print = 1;
open4416 6:fbe401163489 207 } else {
open4416 6:fbe401163489 208 //All module online & POST pass
open4416 6:fbe401163489 209 VDU_STAT = VDU_Idle;
open4416 6:fbe401163489 210 printf("All module online, VDU now Idle\n");
open4416 6:fbe401163489 211 }
open4416 6:fbe401163489 212 } //Else keep in state VDU_PowerOn
open4416 6:fbe401163489 213 break;
open4416 6:fbe401163489 214
open4416 6:fbe401163489 215 case VDU_Idle:
open4416 6:fbe401163489 216 /* Controller Idle state
open4416 6:fbe401163489 217 * Description:
open4416 6:fbe401163489 218 * Normal latched state, wait for RTD_HMI set from PSU
open4416 6:fbe401163489 219 * 4WD in running but output mask to 0
open4416 6:fbe401163489 220 * Do:
open4416 6:fbe401163489 221 * 4WD controller
open4416 6:fbe401163489 222 * Check:
open4416 6:fbe401163489 223 * RUN faults if any
open4416 6:fbe401163489 224 * To VDU_Run:
open4416 6:fbe401163489 225 * Initialize parameters for start up, set RTD_cmd
open4416 6:fbe401163489 226 * To VDU_Fault:
open4416 6:fbe401163489 227 * Run the error handling service
open4416 6:fbe401163489 228 */
open4416 6:fbe401163489 229
open4416 6:fbe401163489 230 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 7:f674ef860c9c 231 RTD_HMI = 1; //Should be set if can bus received data
open4416 6:fbe401163489 232 //Forced RTD_HMI for debug purpose 2019/11/14
open4416 6:fbe401163489 233
open4416 6:fbe401163489 234 RUNT(); //Run test
open4416 6:fbe401163489 235 AWD(); //AWD main program
open4416 1:8a9ac822aab7 236
open4416 6:fbe401163489 237 if ( //Check if any error
open4416 6:fbe401163489 238 (FL_DSM == 3U)||
open4416 6:fbe401163489 239 (FR_DSM == 3U)||
open4416 6:fbe401163489 240 (RL_DSM == 3U)||
open4416 6:fbe401163489 241 (RR_DSM == 3U)||
open4416 6:fbe401163489 242 (VDU_FLT != 0)) {
open4416 6:fbe401163489 243 VDU_STAT = VDU_Fault;
open4416 7:f674ef860c9c 244 printf("Idle 2 Fault\n");
open4416 6:fbe401163489 245 FLT_print = 1;
open4416 6:fbe401163489 246 } else if (RTD_HMI != 0) { //Or command to run threw PSU
open4416 6:fbe401163489 247 //Prepare to send out RTD and start motor
open4416 6:fbe401163489 248 Idle2Run();
open4416 6:fbe401163489 249 VDU_STAT = VDU_Run;
open4416 6:fbe401163489 250 printf("Idle 2 Run\n");
open4416 6:fbe401163489 251 } //Else keep in state
open4416 6:fbe401163489 252 break;
open4416 6:fbe401163489 253
open4416 6:fbe401163489 254 case VDU_Run:
open4416 6:fbe401163489 255 /* Controller Run state
open4416 6:fbe401163489 256 * Description:
open4416 6:fbe401163489 257 * Normal latched state, after RTD_HMI is set from PSU
open4416 6:fbe401163489 258 * Same to Idle state except RTD_cmd is set
open4416 6:fbe401163489 259 * Do:
open4416 6:fbe401163489 260 * 4WD controller
open4416 6:fbe401163489 261 * Check:
open4416 6:fbe401163489 262 * RUN faults if any
open4416 6:fbe401163489 263 * To VDU_Idle:
open4416 6:fbe401163489 264 * Initialize parameters for idling, reset RTD_cmd
open4416 6:fbe401163489 265 * To VDU_Fault:
open4416 6:fbe401163489 266 * Run the error handling service
open4416 6:fbe401163489 267 */
open4416 6:fbe401163489 268
open4416 6:fbe401163489 269 RUNT(); //Run test
open4416 6:fbe401163489 270 AWD(); //AWD main program
open4416 6:fbe401163489 271
open4416 6:fbe401163489 272 //Temporary debug posting area 2019/11/14
open4416 6:fbe401163489 273 //printf("%d,%d\n", Encoder_cnt, Encoder_del);
open4416 6:fbe401163489 274 //printf("%d\n\r", (int16_t)Tmodule);//
open4416 6:fbe401163489 275 //printf("%d\n\r", (int16_t)Vbus);
open4416 0:c4747ebbe0b4 276
open4416 6:fbe401163489 277 if ( //Check if any error
open4416 6:fbe401163489 278 (FL_DSM == 3U)||
open4416 6:fbe401163489 279 (FR_DSM == 3U)||
open4416 6:fbe401163489 280 (RL_DSM == 3U)||
open4416 6:fbe401163489 281 (RR_DSM == 3U)||
open4416 6:fbe401163489 282 (VDU_FLT != 0)) {
open4416 6:fbe401163489 283 VDU_STAT = VDU_Fault;
open4416 7:f674ef860c9c 284 printf("Run 2 Fault\n");
open4416 6:fbe401163489 285 FLT_print = 1;
open4416 6:fbe401163489 286 } else if (RTD_HMI != 1) { //Or command to stop threw can bus
open4416 6:fbe401163489 287 Run2Idle();
open4416 6:fbe401163489 288 VDU_STAT = VDU_Idle;
open4416 6:fbe401163489 289 printf("Run 2 Idle\n");
open4416 6:fbe401163489 290 } //Else keep in state
open4416 6:fbe401163489 291 break;
open4416 6:fbe401163489 292
open4416 6:fbe401163489 293 case VDU_Fault:
open4416 6:fbe401163489 294 /* Controller Fault state
open4416 6:fbe401163489 295 * Description:
open4416 6:fbe401163489 296 * Fault latched state if any faults is detected
open4416 6:fbe401163489 297 * Same to Idle state except keep till RTD_HMI reset
open4416 6:fbe401163489 298 * Do:
open4416 6:fbe401163489 299 * Nothing, like a piece of shit
open4416 6:fbe401163489 300 * Check:
open4416 6:fbe401163489 301 * RUN faults if any
open4416 6:fbe401163489 302 * To VDU_PowerOn:
open4416 6:fbe401163489 303 * Restart VDU
open4416 6:fbe401163489 304 */
open4416 6:fbe401163489 305
open4416 6:fbe401163489 306 RUNT(); //Run test
open4416 6:fbe401163489 307
open4416 6:fbe401163489 308 if (RST_HMI == 1) { //PSU reset to clear error
open4416 6:fbe401163489 309 RST_cmd = 1;
open4416 6:fbe401163489 310 FLT_print = 0;
open4416 6:fbe401163489 311 VDU_STAT = VDU_PowerOn;
open4416 6:fbe401163489 312 printf("Module reset\nVDU restarting...\n");
open4416 6:fbe401163489 313 } //Else keep in state
open4416 6:fbe401163489 314 break;
open4416 6:fbe401163489 315 }
open4416 6:fbe401163489 316
open4416 7:f674ef860c9c 317 // Shit out torque distribution and special command
open4416 6:fbe401163489 318 if(VDU_STAT == VDU_Run) {
open4416 6:fbe401163489 319 //Allow output torque
open4416 6:fbe401163489 320 Tx_Tcmd_CAN1();
open4416 6:fbe401163489 321 } else if(RST_cmd != 0) {
open4416 7:f674ef860c9c 322 //Send out reset cmd once
open4416 6:fbe401163489 323 Tx_CLRerr_CAN1();
open4416 6:fbe401163489 324 } else {
open4416 6:fbe401163489 325 //Force RTD off when not in VDU_Run
open4416 6:fbe401163489 326 Tx_Estop_CAN1();
open4416 6:fbe401163489 327 }
open4416 6:fbe401163489 328
open4416 6:fbe401163489 329 }
open4416 7:f674ef860c9c 330 // End of high speed loop
open4416 6:fbe401163489 331
open4416 6:fbe401163489 332 // Do low speed state reporting
open4416 6:fbe401163489 333 if (LST_EXFL == 1) {
open4416 6:fbe401163489 334 LST_EXFL = 0;
open4416 5:8116016abee0 335 Tx_Qdrv_CAN1();
open4416 6:fbe401163489 336
open4416 6:fbe401163489 337 // Print out error mesagge if exist, 1Hz repeative
open4416 6:fbe401163489 338 if(FLT_print != 0) {
open4416 7:f674ef860c9c 339 FLT_print_cnt += FLT_print;
open4416 6:fbe401163489 340 if(FLT_print_cnt > 19) {
open4416 6:fbe401163489 341 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 342 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 343 printf("VDU\n0x%04X\n\n", VDU_FLT);
open4416 6:fbe401163489 344 FLT_print_cnt = 0;
open4416 6:fbe401163489 345 }
open4416 6:fbe401163489 346 }
open4416 7:f674ef860c9c 347 }
open4416 7:f674ef860c9c 348 // End of low speed state reporting
open4416 6:fbe401163489 349
open4416 7:f674ef860c9c 350 } // end of while
open4416 0:c4747ebbe0b4 351 }
open4416 0:c4747ebbe0b4 352
open4416 6:fbe401163489 353 void Idle2Run(void)
open4416 6:fbe401163489 354 {
open4416 6:fbe401163489 355 RTD_cmd = 1;
open4416 6:fbe401163489 356 }
open4416 6:fbe401163489 357
open4416 6:fbe401163489 358 void Run2Idle(void)
open4416 6:fbe401163489 359 {
open4416 6:fbe401163489 360 RTD_cmd = 0;
open4416 6:fbe401163489 361 }
open4416 6:fbe401163489 362
open4416 6:fbe401163489 363 void POST(void)
open4416 6:fbe401163489 364 {
open4416 6:fbe401163489 365 //Check IMU status
open4416 6:fbe401163489 366 if(0) {
open4416 6:fbe401163489 367 VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
open4416 6:fbe401163489 368 }
open4416 6:fbe401163489 369 }
open4416 6:fbe401163489 370
open4416 6:fbe401163489 371 void RUNT(void)
open4416 6:fbe401163489 372 {
open4416 6:fbe401163489 373 POST();
open4416 6:fbe401163489 374
open4416 6:fbe401163489 375 //Check timeout
open4416 6:fbe401163489 376 if((FL_online*FR_online*RL_online*RR_online) == 0) {
open4416 6:fbe401163489 377 VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout
open4416 6:fbe401163489 378 }
open4416 6:fbe401163489 379 if(PSU_online == 0) {
open4416 6:fbe401163489 380 VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout
open4416 6:fbe401163489 381 }
open4416 6:fbe401163489 382
open4416 6:fbe401163489 383 //Check IMU status
open4416 6:fbe401163489 384 //XXX
open4416 6:fbe401163489 385
open4416 6:fbe401163489 386 //Check ShutDrv voltage
open4416 6:fbe401163489 387 //XXX
open4416 6:fbe401163489 388 }
open4416 6:fbe401163489 389
open4416 6:fbe401163489 390 void Aux_read(void)
open4416 6:fbe401163489 391 {
open4416 7:f674ef860c9c 392 //Capture analog in at once
open4416 7:f674ef860c9c 393 AUX_1_u16 = AUX_1.read_u16();
open4416 7:f674ef860c9c 394 AUX_2_u16 = AUX_2.read_u16();
open4416 7:f674ef860c9c 395 AUX_3_u16 = AUX_3.read_u16();
open4416 7:f674ef860c9c 396 AUX_4_u16 = AUX_4.read_u16();
open4416 7:f674ef860c9c 397 SDn_voltage = 18.81f*SDn_sense.read();
open4416 6:fbe401163489 398 }
open4416 6:fbe401163489 399
open4416 6:fbe401163489 400 void IMU_read(void)
open4416 6:fbe401163489 401 {
open4416 7:f674ef860c9c 402
open4416 6:fbe401163489 403 }
open4416 6:fbe401163489 404
open4416 6:fbe401163489 405 void AWD(void)
open4416 6:fbe401163489 406 {
open4416 6:fbe401163489 407
open4416 6:fbe401163489 408 }
open4416 6:fbe401163489 409
open4416 5:8116016abee0 410 void Rx_CAN1(void)
open4416 5:8116016abee0 411 {
open4416 5:8116016abee0 412 LED = 1;
open4416 7:f674ef860c9c 413 int16_t tmp;
open4416 7:f674ef860c9c 414
open4416 5:8116016abee0 415 if(can1.read(can_msg_Rx)) {
open4416 7:f674ef860c9c 416 switch(can_msg_Rx.id) { //Filtered input message
open4416 7:f674ef860c9c 417 // Start of 100Hz msg
open4416 7:f674ef860c9c 418 case FL_HSB_ID:
open4416 7:f674ef860c9c 419 //HSB from FL motor drive
open4416 7:f674ef860c9c 420 FL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 421 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 422 FL_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 423 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 424 FL_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 425 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 426 FL_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 427 FL_online = 3;
open4416 7:f674ef860c9c 428 break;
open4416 7:f674ef860c9c 429
open4416 7:f674ef860c9c 430 case FR_HSB_ID:
open4416 7:f674ef860c9c 431 //HSB from FR motor drive
open4416 7:f674ef860c9c 432 FR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 433 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 434 FR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 435 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 436 FR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 437 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 438 FR_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 439 FR_online = 3;
open4416 7:f674ef860c9c 440 break;
open4416 7:f674ef860c9c 441
open4416 7:f674ef860c9c 442 case RL_HSB_ID:
open4416 7:f674ef860c9c 443 //HSB from RL motor drive
open4416 7:f674ef860c9c 444 RL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 445 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 446 RL_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 447 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 448 RL_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 449 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 450 RL_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 451 RL_online = 3;
open4416 7:f674ef860c9c 452 break;
open4416 7:f674ef860c9c 453
open4416 7:f674ef860c9c 454 case RR_HSB_ID:
open4416 7:f674ef860c9c 455 //HSB from RR motor drive
open4416 7:f674ef860c9c 456 RR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
open4416 7:f674ef860c9c 457 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 458 RR_W_ele = tmp*1.0f;
open4416 7:f674ef860c9c 459 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 460 RR_Trq_fil3 = tmp * 0.01f;
open4416 7:f674ef860c9c 461 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 462 RR_Trq_est = tmp * 0.01f;
open4416 7:f674ef860c9c 463 RR_online = 3;
open4416 7:f674ef860c9c 464 break;
open4416 7:f674ef860c9c 465
open4416 7:f674ef860c9c 466 case HMI_cmd_ID:
open4416 7:f674ef860c9c 467 //HMI command from PSU
open4416 7:f674ef860c9c 468 AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch
open4416 7:f674ef860c9c 469 RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
open4416 7:f674ef860c9c 470 RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch
open4416 7:f674ef860c9c 471 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 472 Steer_HMI = tmp * 0.01f;
open4416 7:f674ef860c9c 473 tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 474 Trq_HMI = tmp * 0.01f;
open4416 7:f674ef860c9c 475 PSU_online = 3;
open4416 7:f674ef860c9c 476 break;
open4416 7:f674ef860c9c 477 // end of 100Hz msg
open4416 7:f674ef860c9c 478
open4416 7:f674ef860c9c 479 // Start of 10Hz msg
open4416 7:f674ef860c9c 480 case FL_LSB_ID:
open4416 7:f674ef860c9c 481 //LSB from FL motor drive
open4416 7:f674ef860c9c 482 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 483 FL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 484 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 485 FL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 486 FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 487 FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 488 break;
open4416 7:f674ef860c9c 489
open4416 7:f674ef860c9c 490 case FR_LSB_ID:
open4416 7:f674ef860c9c 491 //LSB from FR motor drive
open4416 7:f674ef860c9c 492 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 493 FR_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 494 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 495 FR_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 496 FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 497 FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 498 break;
open4416 7:f674ef860c9c 499
open4416 7:f674ef860c9c 500 case RL_LSB_ID:
open4416 7:f674ef860c9c 501 //LSB from RL motor drive
open4416 7:f674ef860c9c 502 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 503 RL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 504 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 505 RL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 506 RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 507 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 508 break;
open4416 7:f674ef860c9c 509
open4416 7:f674ef860c9c 510 case RR_LSB_ID:
open4416 7:f674ef860c9c 511 //LSB from RR motor drive
open4416 7:f674ef860c9c 512 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
open4416 7:f674ef860c9c 513 RL_Tmotor = tmp*0.01f;
open4416 7:f674ef860c9c 514 tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
open4416 7:f674ef860c9c 515 RL_Tmodule = tmp*0.01f;
open4416 7:f674ef860c9c 516 RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
open4416 7:f674ef860c9c 517 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
open4416 7:f674ef860c9c 518 break;
open4416 7:f674ef860c9c 519 // end of 10Hz msg
open4416 7:f674ef860c9c 520 }
open4416 5:8116016abee0 521 }
open4416 5:8116016abee0 522 LED = 0;
open4416 5:8116016abee0 523 }
open4416 5:8116016abee0 524
open4416 6:fbe401163489 525 void Tx_CLRerr_CAN1(void)
open4416 2:c7a3a8c1aeed 526 {
open4416 6:fbe401163489 527 RTD_cmd = 0; //disable as default
open4416 6:fbe401163489 528 Tx_Tcmd_CAN1();
open4416 6:fbe401163489 529 RST_cmd = 0; //on shot
open4416 6:fbe401163489 530 }
open4416 6:fbe401163489 531
open4416 6:fbe401163489 532 void Tx_Estop_CAN1(void)
open4416 6:fbe401163489 533 {
open4416 6:fbe401163489 534 RTD_cmd = 0; //force disable
open4416 6:fbe401163489 535 Tx_Tcmd_CAN1();
open4416 2:c7a3a8c1aeed 536 }
open4416 2:c7a3a8c1aeed 537
open4416 6:fbe401163489 538 void Tx_Tcmd_CAN1(void) // 100 Hz
open4416 2:c7a3a8c1aeed 539 {
open4416 7:f674ef860c9c 540 int16_t tmp;
open4416 7:f674ef860c9c 541 // Need to change ID for real case 2019/11/14
open4416 7:f674ef860c9c 542 tmp = (int16_t) (FL_Tcmd * 100.0f);
open4416 7:f674ef860c9c 543 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 544 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 545 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 546 temp_msg[3] = RST_cmd;
open4416 6:fbe401163489 547 can_msg_Tx = CANMessage(FL_HSB_ID,temp_msg,8,CANData,CANStandard); // FL_CMD_ID, now only for testing
open4416 7:f674ef860c9c 548 // can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 549 CANpendTX();
open4416 6:fbe401163489 550 can1.write(can_msg_Tx);
open4416 6:fbe401163489 551
open4416 7:f674ef860c9c 552 tmp = (int16_t) (FR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 553 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 554 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 555 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 556 temp_msg[3] = RST_cmd;
open4416 6:fbe401163489 557 can_msg_Tx = CANMessage(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
open4416 7:f674ef860c9c 558 // can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 559 CANpendTX();
open4416 6:fbe401163489 560 can1.write(can_msg_Tx);
open4416 6:fbe401163489 561
open4416 7:f674ef860c9c 562 tmp = (int16_t) (RL_Tcmd * 100.0f);
open4416 7:f674ef860c9c 563 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 564 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 565 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 566 temp_msg[3] = RST_cmd;
open4416 6:fbe401163489 567 can_msg_Tx = CANMessage(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
open4416 7:f674ef860c9c 568 // can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 569 CANpendTX();
open4416 6:fbe401163489 570 can1.write(can_msg_Tx);
open4416 6:fbe401163489 571
open4416 7:f674ef860c9c 572 tmp = (int16_t) (RR_Tcmd * 100.0f);
open4416 7:f674ef860c9c 573 temp_msg[0] = tmp;
open4416 7:f674ef860c9c 574 temp_msg[1] = tmp >> 8U;
open4416 7:f674ef860c9c 575 temp_msg[2] = RTD_cmd;
open4416 7:f674ef860c9c 576 temp_msg[3] = RST_cmd;
open4416 6:fbe401163489 577 can_msg_Tx = CANMessage(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
open4416 7:f674ef860c9c 578 // can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 579 CANpendTX();
open4416 6:fbe401163489 580 can1.write(can_msg_Tx);
open4416 6:fbe401163489 581
open4416 2:c7a3a8c1aeed 582 }
open4416 2:c7a3a8c1aeed 583
open4416 6:fbe401163489 584 void Tx_Qdrv_CAN1(void) // 10 Hz
open4416 0:c4747ebbe0b4 585 {
open4416 6:fbe401163489 586 // Auxilary sensor reading shitting out
open4416 7:f674ef860c9c 587 temp_msg[0] = AUX_1_u16;
open4416 7:f674ef860c9c 588 temp_msg[1] = AUX_1_u16 >> 8U;
open4416 7:f674ef860c9c 589 temp_msg[2] = AUX_2_u16;
open4416 7:f674ef860c9c 590 temp_msg[3] = AUX_2_u16 >> 8U;
open4416 7:f674ef860c9c 591 temp_msg[4] = AUX_3_u16;
open4416 7:f674ef860c9c 592 temp_msg[5] = AUX_3_u16 >> 8U;
open4416 7:f674ef860c9c 593 temp_msg[6] = AUX_4_u16;
open4416 7:f674ef860c9c 594 temp_msg[7] = AUX_4_u16 >> 8U;
open4416 6:fbe401163489 595 can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 596 CANpendTX();
open4416 6:fbe401163489 597 can1.write(can_msg_Tx);
open4416 6:fbe401163489 598
open4416 6:fbe401163489 599 // Qdrive internal state shitting out
open4416 6:fbe401163489 600 temp_msg[0] = 1;
open4416 6:fbe401163489 601 temp_msg[1] = 2;
open4416 6:fbe401163489 602 temp_msg[2] = 3;
open4416 6:fbe401163489 603 temp_msg[3] = 4;
open4416 6:fbe401163489 604 temp_msg[4] = 5;
open4416 6:fbe401163489 605 temp_msg[5] = 6;
open4416 6:fbe401163489 606 temp_msg[6] = 7;
open4416 6:fbe401163489 607 temp_msg[7] = 8;
open4416 6:fbe401163489 608 can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 609 CANpendTX();
open4416 6:fbe401163489 610 can1.write(can_msg_Tx);
open4416 2:c7a3a8c1aeed 611
open4416 6:fbe401163489 612 // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
open4416 6:fbe401163489 613 temp_msg[0] = 1;
open4416 6:fbe401163489 614 temp_msg[1] = 2;
open4416 6:fbe401163489 615 temp_msg[2] = 3;
open4416 6:fbe401163489 616 temp_msg[3] = 4;
open4416 6:fbe401163489 617 temp_msg[4] = 5;
open4416 6:fbe401163489 618 temp_msg[5] = 6;
open4416 6:fbe401163489 619 temp_msg[6] = 7;
open4416 6:fbe401163489 620 temp_msg[7] = 8;
open4416 6:fbe401163489 621 can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
open4416 6:fbe401163489 622 CANpendTX();
open4416 6:fbe401163489 623 can1.write(can_msg_Tx);
open4416 6:fbe401163489 624 }
open4416 6:fbe401163489 625
open4416 6:fbe401163489 626 void CANpendTX(void)
open4416 6:fbe401163489 627 {
open4416 6:fbe401163489 628 //Pend till TX box has empty slot, timeout will generate error
open4416 6:fbe401163489 629 uint32_t timeout = 0;
open4416 6:fbe401163489 630 while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
open4416 6:fbe401163489 631 //Wait till non empty
open4416 6:fbe401163489 632 timeout += 1;
open4416 6:fbe401163489 633 if(timeout > 10000) {
open4416 6:fbe401163489 634 // Put some timeout error handler
open4416 6:fbe401163489 635 break;
open4416 6:fbe401163489 636 }
open4416 0:c4747ebbe0b4 637 }
open4416 0:c4747ebbe0b4 638 }
open4416 1:8a9ac822aab7 639
open4416 6:fbe401163489 640 void CAN_init(void)
open4416 6:fbe401163489 641 {
open4416 6:fbe401163489 642 //Set CAN system
open4416 6:fbe401163489 643 SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off
open4416 6:fbe401163489 644 can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode
open4416 6:fbe401163489 645 can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1);
open4416 6:fbe401163489 646 can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2);
open4416 6:fbe401163489 647 can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3);
open4416 6:fbe401163489 648 can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4);
open4416 6:fbe401163489 649 can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
open4416 6:fbe401163489 650 can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
open4416 6:fbe401163489 651 can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
open4416 7:f674ef860c9c 652 can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8);
open4416 6:fbe401163489 653 can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
open4416 6:fbe401163489 654 can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
open4416 6:fbe401163489 655 }
open4416 2:c7a3a8c1aeed 656
open4416 6:fbe401163489 657 void Module_WD(void)
open4416 6:fbe401163489 658 {
open4416 6:fbe401163489 659 if (FL_online != 0) {
open4416 6:fbe401163489 660 FL_online -= 1;
open4416 6:fbe401163489 661 }
open4416 6:fbe401163489 662 if (FR_online != 0) {
open4416 6:fbe401163489 663 FR_online -= 1;
open4416 6:fbe401163489 664 }
open4416 6:fbe401163489 665 if (RL_online != 0) {
open4416 6:fbe401163489 666 RL_online -= 1;
open4416 6:fbe401163489 667 }
open4416 6:fbe401163489 668 if (RR_online != 0) {
open4416 6:fbe401163489 669 RR_online -= 1;
open4416 6:fbe401163489 670 }
open4416 6:fbe401163489 671 if (PSU_online != 0) {
open4416 6:fbe401163489 672 PSU_online -= 1;
open4416 6:fbe401163489 673 }
open4416 6:fbe401163489 674 }
open4416 6:fbe401163489 675
open4416 6:fbe401163489 676 int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
open4416 2:c7a3a8c1aeed 677 {
open4416 2:c7a3a8c1aeed 678 int16_t max = i1;
open4416 2:c7a3a8c1aeed 679 if(i2 > max) max = i2;
open4416 2:c7a3a8c1aeed 680 if(i3 > max) max = i3;
open4416 6:fbe401163489 681 if(i4 > max) max = i4;
open4416 2:c7a3a8c1aeed 682 return max;
open4416 6:fbe401163489 683 }
open4416 6:fbe401163489 684 // pc.printf("SOC: %.2f\n", Module_Total*0.01f);