SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@20:e9daae390513, 2020-03-04 (annotated)

Committer:
open4416
Date:
Wed Mar 04 08:17:12 2020 +0000
Revision:
20:e9daae390513
Parent:
19:d68f21173c23
Child:
21:e01a019fae2f
Fix issue with error handling; Tested with PSU+QDrive

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