SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
main.h@13:ac024885d0bf, 2019-11-19 (annotated)
- Committer:
- open4416
- Date:
- Tue Nov 19 11:54:12 2019 +0000
- Revision:
- 13:ac024885d0bf
- Parent:
- 9:c99eeafa6fa3
- Child:
- 14:bcf5cb4d08a5
Fix error after test & add brk sensor
Who changed what in which revision?
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 | 8:f8b1402c8f3c | 7 | #define EG 0.000f // <0: over steer, >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 | 8:f8b1402c8f3c | 31 | |
open4416 | 8:f8b1402c8f3c | 32 | //CAN msg bank |
open4416 | 8:f8b1402c8f3c | 33 | char temp_msg[8] = {0,0,0,0,0,0,0,0}; |
open4416 | 8:f8b1402c8f3c | 34 | |
open4416 | 8:f8b1402c8f3c | 35 | //IMU readings |
open4416 | 8:f8b1402c8f3c | 36 | float YR_imu = 0; // rad/s yawrate estimated threw IMU sensor fusion |
open4416 | 8:f8b1402c8f3c | 37 | float Roll_imu = 0; // deg |
open4416 | 8:f8b1402c8f3c | 38 | float Pitch_imu = 0; // deg |
open4416 | 8:f8b1402c8f3c | 39 | float Ax_imu = 0; // m/s/s |
open4416 | 8:f8b1402c8f3c | 40 | float Ay_imu = 0; // m/s/s |
open4416 | 8:f8b1402c8f3c | 41 | |
open4416 | 8:f8b1402c8f3c | 42 | //AWD variable |
open4416 | 8:f8b1402c8f3c | 43 | float YR_ref = 0; // rad/s yawrate reference generate from ideal single track model |
open4416 | 8:f8b1402c8f3c | 44 | float Vb_est = 0; // m/s estimated body Velocity |
open4416 | 8:f8b1402c8f3c | 45 | float Mz_reg = 0; // Nm addon yaw torque regulating yawrate, gain given by "K_yaw" |
open4416 | 8:f8b1402c8f3c | 46 | float sig = 0; // Front rear distribution factor |
open4416 | 8:f8b1402c8f3c | 47 | |
open4416 | 8:f8b1402c8f3c | 48 | //CAN to motor drive module, 100Hz |
open4416 | 8:f8b1402c8f3c | 49 | //msg ID: 0xC0~0xC3 |
open4416 | 8:f8b1402c8f3c | 50 | float FL_Tcmd = 0; // *100 before sent in int16_t |
open4416 | 8:f8b1402c8f3c | 51 | float FR_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 52 | float RL_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 53 | float RR_Tcmd = 0; |
open4416 | 8:f8b1402c8f3c | 54 | uint8_t RTD_cmd = 0; // start inverter switching cmd |
open4416 | 8:f8b1402c8f3c | 55 | uint8_t RST_cmd = 0; // send out once to reset module fault |
open4416 | 8:f8b1402c8f3c | 56 | |
open4416 | 8:f8b1402c8f3c | 57 | //Module online flag |
open4416 | 8:f8b1402c8f3c | 58 | uint8_t FL_online = 0; // 0 indicate detection timeout |
open4416 | 8:f8b1402c8f3c | 59 | uint8_t FR_online = 0; // reset value is 3 to hold for 0.03sec |
open4416 | 8:f8b1402c8f3c | 60 | uint8_t RL_online = 0; // -1 per 100Hz task |
open4416 | 8:f8b1402c8f3c | 61 | uint8_t RR_online = 0; |
open4416 | 8:f8b1402c8f3c | 62 | uint8_t PSU_online = 0; |
open4416 | 8:f8b1402c8f3c | 63 | |
open4416 | 8:f8b1402c8f3c | 64 | //Feedback data decoded out storage |
open4416 | 8:f8b1402c8f3c | 65 | float FL_Tmotor = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 66 | float FR_Tmotor = 0; |
open4416 | 8:f8b1402c8f3c | 67 | float RL_Tmotor = 0; |
open4416 | 8:f8b1402c8f3c | 68 | float RR_Tmotor = 0; |
open4416 | 9:c99eeafa6fa3 | 69 | float Max_Tmotor = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 70 | float FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 71 | float FR_Tmodule = 0; |
open4416 | 8:f8b1402c8f3c | 72 | float RL_Tmodule = 0; |
open4416 | 8:f8b1402c8f3c | 73 | float RR_Tmodule = 0; |
open4416 | 9:c99eeafa6fa3 | 74 | float Max_Tmodule = 0; // motor temperature degC, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 75 | uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 76 | uint16_t FR_FLT_Run = 0; |
open4416 | 8:f8b1402c8f3c | 77 | uint16_t RL_FLT_Run = 0; |
open4416 | 8:f8b1402c8f3c | 78 | uint16_t RR_FLT_Run = 0; |
open4416 | 8:f8b1402c8f3c | 79 | uint16_t FL_FLT_Post = 0; // POST fault code, 10Hz recieving |
open4416 | 8:f8b1402c8f3c | 80 | uint16_t FR_FLT_Post = 0; |
open4416 | 8:f8b1402c8f3c | 81 | uint16_t RL_FLT_Post = 0; |
open4416 | 8:f8b1402c8f3c | 82 | uint16_t RR_FLT_Post = 0; |
open4416 | 8:f8b1402c8f3c | 83 | float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 84 | float FR_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 85 | float RL_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 86 | float RR_Trq_fil3 = 0; |
open4416 | 8:f8b1402c8f3c | 87 | float FL_Trq_est = 0; // Estimated Torque, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 88 | float FR_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 89 | float RL_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 90 | float RR_Trq_est = 0; |
open4416 | 8:f8b1402c8f3c | 91 | float FL_W_ele = 0; // Estimated W_ele, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 92 | float FR_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 93 | float RL_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 94 | float RR_W_ele = 0; |
open4416 | 8:f8b1402c8f3c | 95 | uint8_t FL_DSM = 0; // DSM state, 100Hz recieving |
open4416 | 8:f8b1402c8f3c | 96 | uint8_t FR_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 97 | uint8_t RL_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 98 | uint8_t RR_DSM = 0; |
open4416 | 8:f8b1402c8f3c | 99 | |
open4416 | 8:f8b1402c8f3c | 100 | //From Pedal Box msg |
open4416 | 8:f8b1402c8f3c | 101 | uint8_t RTD_HMI = 0; // 1 = HMI requesting |
open4416 | 8:f8b1402c8f3c | 102 | uint8_t RST_HMI = 0; // 1 = HMI request once |
open4416 | 8:f8b1402c8f3c | 103 | uint8_t AWD_HMI = 0; // 1 = HMI requesting |
open4416 | 8:f8b1402c8f3c | 104 | float Trq_HMI = 0.0f; // Nm user request total torque |
open4416 | 8:f8b1402c8f3c | 105 | float Steer_HMI = 0.0f; // deg steering wheel angel |
open4416 | 8:f8b1402c8f3c | 106 | |
open4416 | 8:f8b1402c8f3c | 107 | //10/100Hz tasking |
open4416 | 8:f8b1402c8f3c | 108 | uint8_t HSTick = 5; // High speed tick |
open4416 | 8:f8b1402c8f3c | 109 | uint8_t LSTick = 0; |
open4416 | 8:f8b1402c8f3c | 110 | uint8_t HST_EXFL = 0; // High speed execution flag |
open4416 | 8:f8b1402c8f3c | 111 | uint8_t LST_EXFL = 0; |
open4416 | 8:f8b1402c8f3c | 112 | uint8_t FLT_print = 0; // Send repeative error message |
open4416 | 8:f8b1402c8f3c | 113 | uint8_t FLT_print_cnt = 0; |
open4416 | 8:f8b1402c8f3c | 114 | uint16_t AUX_1_u16 = 0x0; // acquired data |
open4416 | 8:f8b1402c8f3c | 115 | uint16_t AUX_2_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 116 | uint16_t AUX_3_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 117 | uint16_t AUX_4_u16 = 0x0; |
open4416 | 8:f8b1402c8f3c | 118 | float SDn_voltage = 0.0f; |
open4416 | 13:ac024885d0bf | 119 | float Brk_voltage = 0.0f; |
open4416 | 8:f8b1402c8f3c | 120 | //VDU states |
open4416 | 8:f8b1402c8f3c | 121 | typedef enum { |
open4416 | 8:f8b1402c8f3c | 122 | VDU_PowerOn = 0U, |
open4416 | 8:f8b1402c8f3c | 123 | VDU_Idle = 1U, |
open4416 | 8:f8b1402c8f3c | 124 | VDU_Run = 2U, |
open4416 | 8:f8b1402c8f3c | 125 | VDU_Fault = 3U |
open4416 | 8:f8b1402c8f3c | 126 | } VDU_STATE_TYPE; |
open4416 | 8:f8b1402c8f3c | 127 | VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; //VDU current state |
open4416 | 8:f8b1402c8f3c | 128 | uint16_t VDU_FLT = 0; //VDU internal fault code |
open4416 | 8:f8b1402c8f3c | 129 | |
open4416 | 8:f8b1402c8f3c | 130 | //Prototype |
open4416 | 8:f8b1402c8f3c | 131 | void CAN_init(void); //Initial CAN frequency filter... |
open4416 | 8:f8b1402c8f3c | 132 | void Module_WD(void); //Software watchdog indicate module state |
open4416 | 8:f8b1402c8f3c | 133 | void IMU_read(void); //Update IMU readings by once |
open4416 | 8:f8b1402c8f3c | 134 | void Aux_read(void); //Update AUX reafings by once |
open4416 | 8:f8b1402c8f3c | 135 | void Idle2Run(void); //Initializing before running |
open4416 | 8:f8b1402c8f3c | 136 | void Run2Idle(void); //Initializing before idling |
open4416 | 8:f8b1402c8f3c | 137 | void POST(void); //Check IMU error |
open4416 | 8:f8b1402c8f3c | 138 | void RUNT(void); //Check POST, timeout, ShutDrv voltage error |
open4416 | 8:f8b1402c8f3c | 139 | void AWD(void); //AWD main program, all wheel drive |
open4416 | 8:f8b1402c8f3c | 140 | void ASL(void); //ASL main program, anti slip |
open4416 | 8:f8b1402c8f3c | 141 | //void REG_mask(void); //For low speed non regen rule, implement in PSU |
open4416 | 8:f8b1402c8f3c | 142 | void Cooler(void); //Function for active cooler working |
open4416 | 8:f8b1402c8f3c | 143 | void Rx_CAN1(void); //CAN RX handler |
open4416 | 8:f8b1402c8f3c | 144 | void Tx_CLRerr_CAN1(void); //Send reset cmd to modules |
open4416 | 8:f8b1402c8f3c | 145 | void Tx_Estop_CAN1(void); //Send out heart beat but force RTD off |
open4416 | 8:f8b1402c8f3c | 146 | void Tx_Tcmd_CAN1(void); //Send out heart beat command |
open4416 | 8:f8b1402c8f3c | 147 | void Tx_Qdrv_CAN1(void); //Send out low speed heart beat for logging |
open4416 | 8:f8b1402c8f3c | 148 | void CANpendTX(void); //Helper function for CAN Tx |
open4416 | 9:c99eeafa6fa3 | 149 | int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4); |
open4416 | 9:c99eeafa6fa3 | 150 | float max_fval(float i1, float i2, float i3, float i4); |