SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.cpp@18:780366f2534c, 2019-12-28 (annotated)

Committer:
open4416
Date:
Sat Dec 28 14:16:21 2019 +0000
Revision:
18:780366f2534c
Parent:
14:bcf5cb4d08a5
Child:
19:d68f21173c23
Add idiot blinky ability

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