SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
main.h@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?
User | Revision | Line number | New contents of line |
---|---|---|---|
open4416 | 8:f8b1402c8f3c | 1 | #define Track 1.240f //Average wheel track |
open4416 | 8:f8b1402c8f3c | 2 | #define Base 1.530f //wheel base |
open4416 | 8:f8b1402c8f3c | 3 | #define Rwhl 0.230f //Wheel radius |
open4416 | 8:f8b1402c8f3c | 4 | #define HCG 0.204f //Height of CG |
open4416 | 8:f8b1402c8f3c | 5 | #define Mtot 320.0f //Total weight with driver |
open4416 | 8:f8b1402c8f3c | 6 | #define g0 9.81f //gravity |
open4416 | 18:780366f2534c | 7 | #define EG 0.000f //EG<0: over steer, EG>0:under steer |
open4416 | 8:f8b1402c8f3c | 8 | #define K_yaw 0.000f //Gain for yaw regulator |
open4416 | 8:f8b1402c8f3c | 9 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) |
open4416 | 8:f8b1402c8f3c | 10 | #define FL_HSB_ID 0xA0 // Rx, 100Hz |
open4416 | 8:f8b1402c8f3c | 11 | #define FR_HSB_ID 0xA1 // Rx, 100Hz |
open4416 | 8:f8b1402c8f3c | 12 | #define RL_HSB_ID 0xA2 // Rx, 100Hz |
open4416 | 8:f8b1402c8f3c | 13 | #define RR_HSB_ID 0xA3 // Rx, 100Hz |
open4416 | 8:f8b1402c8f3c | 14 | #define FL_LSB_ID 0xB0 // Rx, 10Hz |
open4416 | 8:f8b1402c8f3c | 15 | #define FR_LSB_ID 0xB1 // Rx, 10Hz |
open4416 | 8:f8b1402c8f3c | 16 | #define RL_LSB_ID 0xB2 // Rx, 10Hz |
open4416 | 8:f8b1402c8f3c | 17 | #define RR_LSB_ID 0xB3 // Rx, 10Hz |
open4416 | 8:f8b1402c8f3c | 18 | #define HMI_cmd_ID 0xC4 // Rx, 100Hz |
open4416 | 8:f8b1402c8f3c | 19 | #define FL_CMD_ID 0xC0 // Tx, 100Hz |
open4416 | 8:f8b1402c8f3c | 20 | #define FR_CMD_ID 0xC1 // Tx, 100Hz |
open4416 | 8:f8b1402c8f3c | 21 | #define RL_CMD_ID 0xC2 // Tx, 100Hz |
open4416 | 8:f8b1402c8f3c | 22 | #define RR_CMD_ID 0xC3 // Tx, 100Hz |
open4416 | 8:f8b1402c8f3c | 23 | #define AUX_sense_ID 0xE0 // Tx, 10Hz |
open4416 | 8:f8b1402c8f3c | 24 | #define Qdrv_stat_ID 0xE1 // Tx, 10Hz |
open4416 | 8:f8b1402c8f3c | 25 | #define IMU_sense_ID 0xE2 // Tx, 10Hz |
open4416 | 8:f8b1402c8f3c | 26 | |
open4416 | 8:f8b1402c8f3c | 27 | #define MODOFL_VDUFLTCode 0x0001U //Drive module timeout after once online |
open4416 | 8:f8b1402c8f3c | 28 | #define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online |
open4416 | 8:f8b1402c8f3c | 29 | #define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal |
open4416 | 8:f8b1402c8f3c | 30 | #define ShDVol_VDUFLTCode 0x0008U //Shutdown voltage abnormal |
open4416 | 18:780366f2534c | 31 | #define DSM_VDUFLTCode 0x0010U //One or more slave report DSM_Fault |
open4416 | 8:f8b1402c8f3c | 32 | |
open4416 | 8:f8b1402c8f3c | 33 | //CAN msg bank |
open4416 | 8:f8b1402c8f3c | 34 | char temp_msg[8] = {0,0,0,0,0,0,0,0}; |
open4416 | 8:f8b1402c8f3c | 35 | |
open4416 | 8:f8b1402c8f3c | 36 | //IMU readings |
open4416 | 14:bcf5cb4d08a5 | 37 | float YR_imu = 0; // deg/s yawrate estimated threw IMU sensor fusion |
open4416 | 8:f8b1402c8f3c | 38 | float Roll_imu = 0; // deg |
open4416 | 8:f8b1402c8f3c | 39 | float Pitch_imu = 0; // deg |
open4416 | 8:f8b1402c8f3c | 40 | float Ax_imu = 0; // m/s/s |
open4416 | 8:f8b1402c8f3c | 41 | float Ay_imu = 0; // m/s/s |
open4416 | 8:f8b1402c8f3c | 42 | |
open4416 | 8:f8b1402c8f3c | 43 | //AWD variable |
open4416 | 8:f8b1402c8f3c | 44 | float YR_ref = 0; // rad/s yawrate reference generate from ideal single track model |
open4416 | 8:f8b1402c8f3c | 45 | float Vb_est = 0; // m/s estimated body Velocity |
open4416 | 8:f8b1402c8f3c | 46 | float Mz_reg = 0; // Nm addon yaw torque regulating yawrate, gain given by "K_yaw" |
open4416 | 8:f8b1402c8f3c | 47 | float sig = 0; // Front rear distribution factor |
open4416 | 8:f8b1402c8f3c | 48 | |
open4416 | 8:f8b1402c8f3c | 49 | //CAN to motor drive module, 100Hz |
open4416 | 8:f8b1402c8f3c | 50 | //msg ID: 0xC0~0xC3 |
open4416 | 8:f8b1402c8f3c | 51 | float FL_Tcmd = 0; // *100 before sent in int16_t |
open4416 | 8:f8b1402c8f3c | 52 | float FR_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 53 | float RL_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 54 | float RR_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 55 | uint8_t RTD_cmd = 0; // start inverter switching cmd |
open4416 | 8:f8b1402c8f3c | 56 | uint8_t RST_cmd = 0; // send out once to reset module fault |
open4416 | 8:f8b1402c8f3c | 57 | |
open4416 | 8:f8b1402c8f3c | 58 | //Module online flag |
open4416 | 8:f8b1402c8f3c | 59 | uint8_t FL_online = 0; // 0 indicate detection timeout |
open4416 | 8:f8b1402c8f3c | 60 | uint8_t FR_online = 0; // reset value is 3 to hold for 0.03sec |
open4416 | 8:f8b1402c8f3c | 61 | uint8_t RL_online = 0; // -1 per 100Hz task |
open4416 | 8:f8b1402c8f3c | 62 | uint8_t RR_online = 0; |
open4416 | 8:f8b1402c8f3c | 63 | uint8_t PSU_online = 0; |
open4416 | 8:f8b1402c8f3c | 64 | |
open4416 | 8:f8b1402c8f3c | 65 | //Feedback data decoded out storage |
open4416 | 8:f8b1402c8f3c | 66 | float FL_Tmotor = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 67 | float FR_Tmotor = 0; |
open4416 | 8:f8b1402c8f3c | 68 | float RL_Tmotor = 0; |
open4416 | 8:f8b1402c8f3c | 69 | float RR_Tmotor = 0; |
open4416 | 9:c99eeafa6fa3 | 70 | float Max_Tmotor = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 71 | float FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 72 | float FR_Tmodule = 0; |
open4416 | 8:f8b1402c8f3c | 73 | float RL_Tmodule = 0; |
open4416 | 8:f8b1402c8f3c | 74 | float RR_Tmodule = 0; |
open4416 | 9:c99eeafa6fa3 | 75 | float Max_Tmodule = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 76 | uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 77 | uint16_t FR_FLT_Run = 0; |
open4416 | 8:f8b1402c8f3c | 78 | uint16_t RL_FLT_Run = 0; |
open4416 | 8:f8b1402c8f3c | 79 | uint16_t RR_FLT_Run = 0; |
open4416 | 18:780366f2534c | 80 | uint16_t FLT_Run = 0; |
open4416 | 18:780366f2534c | 81 | uint16_t FLT_Run_ind = 0; // for indication |
open4416 | 8:f8b1402c8f3c | 82 | uint16_t FL_FLT_Post = 0; // POST fault code, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 83 | uint16_t FR_FLT_Post = 0; |
open4416 | 8:f8b1402c8f3c | 84 | uint16_t RL_FLT_Post = 0; |
open4416 | 8:f8b1402c8f3c | 85 | uint16_t RR_FLT_Post = 0; |
open4416 | 18:780366f2534c | 86 | uint16_t FLT_Post = 0; |
open4416 | 18:780366f2534c | 87 | uint16_t FLT_Post_ind = 0; // for indication |
open4416 | 8:f8b1402c8f3c | 88 | float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 89 | float FR_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 90 | float RL_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 91 | float RR_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 92 | float FL_Trq_est = 0; // Estimated Torque, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 93 | float FR_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 94 | float RL_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 95 | float RR_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 96 | float FL_W_ele = 0; // Estimated W_ele, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 97 | float FR_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 98 | float RL_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 99 | float RR_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 100 | uint8_t FL_DSM = 0; // DSM state, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 101 | uint8_t FR_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 102 | uint8_t RL_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 103 | uint8_t RR_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 104 | |
open4416 | 8:f8b1402c8f3c | 105 | //From Pedal Box msg |
open4416 | 8:f8b1402c8f3c | 106 | uint8_t RTD_HMI = 0; // 1 = HMI requesting |
open4416 | 8:f8b1402c8f3c | 107 | uint8_t RST_HMI = 0; // 1 = HMI request once |
open4416 | 8:f8b1402c8f3c | 108 | uint8_t AWD_HMI = 0; // 1 = HMI requesting |
open4416 | 8:f8b1402c8f3c | 109 | float Trq_HMI = 0.0f; // Nm user request total torque |
open4416 | 8:f8b1402c8f3c | 110 | float Steer_HMI = 0.0f; // deg steering wheel angel |
open4416 | 8:f8b1402c8f3c | 111 | |
open4416 | 8:f8b1402c8f3c | 112 | //10/100Hz tasking |
open4416 | 8:f8b1402c8f3c | 113 | uint8_t HSTick = 5; // High speed tick |
open4416 | 8:f8b1402c8f3c | 114 | uint8_t LSTick = 0; |
open4416 | 8:f8b1402c8f3c | 115 | uint8_t HST_EXFL = 0; // High speed execution flag |
open4416 | 8:f8b1402c8f3c | 116 | uint8_t LST_EXFL = 0; |
open4416 | 8:f8b1402c8f3c | 117 | uint8_t FLT_print = 0; // Send repeative error message |
open4416 | 8:f8b1402c8f3c | 118 | uint8_t FLT_print_cnt = 0; |
open4416 | 8:f8b1402c8f3c | 119 | uint16_t AUX_1_u16 = 0x0; // acquired data |
open4416 | 8:f8b1402c8f3c | 120 | uint16_t AUX_2_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 121 | uint16_t AUX_3_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 122 | uint16_t AUX_4_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 123 | float SDn_voltage = 0.0f; |
open4416 | 13:ac024885d0bf | 124 | float Brk_voltage = 0.0f; |
open4416 | 18:780366f2534c | 125 | |
open4416 | 8:f8b1402c8f3c | 126 | //VDU states |
open4416 | 8:f8b1402c8f3c | 127 | typedef enum { |
open4416 | 8:f8b1402c8f3c | 128 | VDU_PowerOn = 0U, |
open4416 | 8:f8b1402c8f3c | 129 | VDU_Idle = 1U, |
open4416 | 8:f8b1402c8f3c | 130 | VDU_Run = 2U, |
open4416 | 8:f8b1402c8f3c | 131 | VDU_Fault = 3U |
open4416 | 8:f8b1402c8f3c | 132 | } VDU_STATE_TYPE; |
open4416 | 18:780366f2534c | 133 | VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; // VDU current state |
open4416 | 18:780366f2534c | 134 | uint16_t VDU_FLT = 0; // VDU internal fault code |
open4416 | 18:780366f2534c | 135 | uint16_t VDU_FLT_ind = 0; // A copy for indication |
open4416 | 18:780366f2534c | 136 | |
open4416 | 18:780366f2534c | 137 | //Indicator pattern generation |
open4416 | 18:780366f2534c | 138 | uint8_t Ind_refresh = 0; // Flag to copy error bits for indicator |
open4416 | 18:780366f2534c | 139 | uint8_t Pattern = 0U; |
open4416 | 18:780366f2534c | 140 | uint16_t Repeat = 0U; // Bitwise repeative variable |
open4416 | 18:780366f2534c | 141 | uint8_t Phase = 0U; // 0:Type ind, 1:Num ind, 2:Blank |
open4416 | 18:780366f2534c | 142 | uint8_t Blk_n = 0U; |
open4416 | 18:780366f2534c | 143 | //Category repetive code |
open4416 | 18:780366f2534c | 144 | #define Post_rep 0b00000001; |
open4416 | 18:780366f2534c | 145 | #define Run_rep 0b00000010; |
open4416 | 18:780366f2534c | 146 | #define VDU_rep 0b00000100; |
open4416 | 18:780366f2534c | 147 | //Blink Pattern |
open4416 | 18:780366f2534c | 148 | #define F_Blink 0b01010101; // blink from LSB to MSB |
open4416 | 18:780366f2534c | 149 | #define L_Pulse 0b01111110; |
open4416 | 18:780366f2534c | 150 | #define S_Pulse 0b01000000; |
open4416 | 18:780366f2534c | 151 | #define N_Pulse 0b00000000; |
open4416 | 8:f8b1402c8f3c | 152 | |
open4416 | 8:f8b1402c8f3c | 153 | //Prototype |
open4416 | 8:f8b1402c8f3c | 154 | void CAN_init(void); //Initial CAN frequency filter... |
open4416 | 8:f8b1402c8f3c | 155 | void Module_WD(void); //Software watchdog indicate module state |
open4416 | 8:f8b1402c8f3c | 156 | void IMU_read(void); //Update IMU readings by once |
open4416 | 8:f8b1402c8f3c | 157 | void Aux_read(void); //Update AUX reafings by once |
open4416 | 8:f8b1402c8f3c | 158 | void Idle2Run(void); //Initializing before running |
open4416 | 8:f8b1402c8f3c | 159 | void Run2Idle(void); //Initializing before idling |
open4416 | 8:f8b1402c8f3c | 160 | void POST(void); //Check IMU error |
open4416 | 8:f8b1402c8f3c | 161 | void RUNT(void); //Check POST, timeout, ShutDrv voltage error |
open4416 | 8:f8b1402c8f3c | 162 | void AWD(void); //AWD main program, all wheel drive |
open4416 | 8:f8b1402c8f3c | 163 | void ASL(void); //ASL main program, anti slip |
open4416 | 8:f8b1402c8f3c | 164 | //void REG_mask(void); //For low speed non regen rule, implement in PSU |
open4416 | 8:f8b1402c8f3c | 165 | void Cooler(void); //Function for active cooler working |
open4416 | 8:f8b1402c8f3c | 166 | void Rx_CAN1(void); //CAN RX handler |
open4416 | 8:f8b1402c8f3c | 167 | void Tx_CLRerr_CAN1(void); //Send reset cmd to modules |
open4416 | 8:f8b1402c8f3c | 168 | void Tx_Estop_CAN1(void); //Send out heart beat but force RTD off |
open4416 | 8:f8b1402c8f3c | 169 | void Tx_Tcmd_CAN1(void); //Send out heart beat command |
open4416 | 8:f8b1402c8f3c | 170 | void Tx_Qdrv_CAN1(void); //Send out low speed heart beat for logging |
open4416 | 8:f8b1402c8f3c | 171 | void CANpendTX(void); //Helper function for CAN Tx |
open4416 | 9:c99eeafa6fa3 | 172 | int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4); |
open4416 | 9:c99eeafa6fa3 | 173 | float max_fval(float i1, float i2, float i3, float i4); |
open4416 | 18:780366f2534c | 174 | uint8_t Indication( //Blink indicator in pattern * repeat |
open4416 | 18:780366f2534c | 175 | uint8_t pattern, |
open4416 | 18:780366f2534c | 176 | uint16_t*repeat, |
open4416 | 18:780366f2534c | 177 | uint8_t*blk_n); |
open4416 | 18:780366f2534c | 178 | uint8_t IndicationKernel( //Generate blink pattern, return 1 if all done |
open4416 | 18:780366f2534c | 179 | uint8_t*pattern, |
open4416 | 18:780366f2534c | 180 | uint16_t*repeat, |
open4416 | 18:780366f2534c | 181 | uint8_t*phase, |
open4416 | 18:780366f2534c | 182 | uint16_t*Post_ind, |
open4416 | 18:780366f2534c | 183 | uint16_t*Run_ind, |
open4416 | 18:780366f2534c | 184 | uint16_t*VDU_ind); |