main.h

00001 #define Track   1.240f      //Average wheel track
00002 #define Base    1.560f      //wheel base
00003 #define Rwhl    0.230f      //Wheel radius
00004 #define HCG     0.204f      //Height of CG
00005 #define Mtot    320.0f      //Total weight with driver
00006 #define g0      9.81f       //gravity
00007 #define EG      0.000f      //EG<0: over steer, EG>0:under steer
00008 #define K_yaw   1.000f      //Gain for yaw regulator
00009 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
00010 
00011 // CAN ID define
00012 #define FL_HSB_ID 0xA0      // Rx, 100Hz
00013 #define FR_HSB_ID 0xA1      // Rx, 100Hz
00014 #define RL_HSB_ID 0xA2      // Rx, 100Hz
00015 #define RR_HSB_ID 0xA3      // Rx, 100Hz
00016 #define FL_LSB_ID 0xB0      // Rx, 10Hz
00017 #define FR_LSB_ID 0xB1      // Rx, 10Hz
00018 #define RL_LSB_ID 0xB2      // Rx, 10Hz
00019 #define RR_LSB_ID 0xB3      // Rx, 10Hz
00020 #define HMI_cmd_ID 0xC4     // Rx, 100Hz
00021 #define FL_CMD_ID 0xC0      // Tx, 100Hz
00022 #define FR_CMD_ID 0xC1      // Tx, 100Hz
00023 #define RL_CMD_ID 0xC2      // Tx, 100Hz
00024 #define RR_CMD_ID 0xC3      // Tx, 100Hz
00025 #define AUX_sense_ID 0xE0   // Tx, 10Hz
00026 #define Qdrv_stat_ID 0xE1   // Tx, 10Hz
00027 #define IMU_sense_ID 0xE2   // Tx, 100Hz
00028 #define RegularVar_ID 0xE3  // Tx, 100Hz
00029 #define PedalStat_ID 0xE4   // Tx, 10Hz
00030 
00031 
00032 #define MODOFL_VDUFLTCode       0x0001U     //Drive module timeout
00033 #define PSUOFL_VDUFLTCode       0x0002U     //Pedal unit timeout
00034 #define IMUSTA_VDUFLTCode       0x0004U     //IMU module abnormal
00035 #define ShDVol_VDUFLTCode       0x0008U     //Shutdown voltage abnormal
00036 #define DSM_VDUFLTCode          0x0010U     //One or more slave report DSM_Fault
00037 
00038 //CAN msg bank
00039 char    temp_msg[8] = {0,0,0,0,0,0,0,0};
00040 
00041 //IMU readings
00042 float YR_imu = 0;                   // deg/s yawrate estimated threw IMU sensor fusion
00043 float Roll_imu = 0;                 // deg
00044 float Pitch_imu = 0;                // deg
00045 float Yaw_imu = 0;                  // deg
00046 float Ax_imu = 0;                   // g
00047 float Ay_imu = 0;                   // g
00048 
00049 //AWD variable
00050 float YR_ref = 0;                   // rad/s yawrate reference generate from ideal single track model
00051 float Vx_wss = 0;                   // m/s estimated body Velocity X (wheelspeed only)
00052 float Vx_fil = 0;                   // m/s estimated body Velocity X
00053 float Mz_reg = 0;                   // Nm addon yaw torque regulating yawrate, gain given by "K_yaw"
00054 float sig = 0;                      // Front rear distribution factor
00055 
00056 //CAN to motor drive module, 100Hz
00057 //msg ID: 0xC0~0xC3
00058 float FL_Tcmd = 0;                  // *100 before sent in int16_t
00059 float FR_Tcmd = 0;
00060 float RL_Tcmd = 0;
00061 float RR_Tcmd = 0;
00062 uint8_t RTD_cmd = 0;                // start inverter switching cmd
00063 uint8_t RST_cmd = 0;                // send out once to reset module fault
00064 
00065 //Module online flag
00066 uint8_t FL_online = 0;              // 0 indicate detection timeout
00067 uint8_t FR_online = 0;              // reset value is 3 to hold for 0.03sec
00068 uint8_t RL_online = 0;              // -1 per 100Hz task
00069 uint8_t RR_online = 0;
00070 uint8_t PSU_online = 0;
00071 
00072 //Feedback data decoded out storage
00073 float FL_Tmotor = 0;                // motor temperature degC, 10Hz recieving
00074 float FR_Tmotor = 0;
00075 float RL_Tmotor = 0;
00076 float RR_Tmotor = 0;
00077 float Max_Tmotor = 0;               // motor temperature degC, 10Hz recieving
00078 float FL_Tmodule = 0;               // inverter temperature degC, 10Hz recieving
00079 float FR_Tmodule = 0;
00080 float RL_Tmodule = 0;
00081 float RR_Tmodule = 0;
00082 float Max_Tmodule = 0;              // motor temperature degC, 10Hz recieving
00083 uint16_t FL_FLT_Run = 0;            // RUN fault code, 10Hz recieving
00084 uint16_t FR_FLT_Run = 0;
00085 uint16_t RL_FLT_Run = 0;
00086 uint16_t RR_FLT_Run = 0;
00087 uint16_t FLT_Run = 0;
00088 uint16_t FLT_Run_ind = 0;           // for indication
00089 uint16_t FL_FLT_Post = 0;           // POST fault code, 10Hz recieving
00090 uint16_t FR_FLT_Post = 0;
00091 uint16_t RL_FLT_Post = 0;
00092 uint16_t RR_FLT_Post = 0;
00093 uint16_t FLT_Post = 0;
00094 uint16_t FLT_Post_ind = 0;           // for indication
00095 float FL_Trq_fil3 = 0;              // Internal Tcmd, 100Hz recieving
00096 float FR_Trq_fil3 = 0;
00097 float RL_Trq_fil3 = 0;
00098 float RR_Trq_fil3 = 0;
00099 float FL_Trq_est = 0;               // Estimated Torque, 100Hz recieving
00100 float FR_Trq_est = 0;
00101 float RL_Trq_est = 0;
00102 float RR_Trq_est = 0;
00103 float FL_W_ele = 0;                 // Estimated W_ele, 100Hz recieving
00104 float FR_W_ele = 0;
00105 float RL_W_ele = 0;
00106 float RR_W_ele = 0;
00107 uint8_t FL_DSM = 0;                 // DSM state, 100Hz recieving
00108 uint8_t FR_DSM = 0;
00109 uint8_t RL_DSM = 0;
00110 uint8_t RR_DSM = 0;
00111 
00112 //From Pedal Box msg
00113 uint8_t RTD_HMI = 0;                // 1 = HMI requesting
00114 uint8_t RST_HMI = 0;                // 1 = HMI request once
00115 uint8_t AWD_HMI = 0;                // 1 = HMI requesting
00116 float Trq_HMI = 0.0f;               // Nm user request total torque
00117 float Steer_HMI = 0.0f;             // Steering wheel signal, *st2r to get rad
00118 uint8_t RTD_SW = 0;                 // 1 = RTD switch on
00119 
00120 //10/100Hz tasking
00121 uint8_t HSTick = 5;                 // High speed tick
00122 uint8_t LSTick = 0;
00123 uint8_t HST_EXFL = 0;               // High speed execution flag
00124 uint8_t LST_EXFL = 0;
00125 uint8_t FLT_print = 0;              // Send repeative error message
00126 uint8_t FLT_print_cnt = 0;
00127 uint16_t AUX_1_u16 = 0x0;           // acquired data
00128 uint16_t AUX_2_u16 = 0x0;
00129 uint16_t AUX_3_u16 = 0x0;
00130 uint16_t AUX_4_u16 = 0x0;
00131 float SDn_voltage = 0.0f;
00132 float Brk_voltage = 0.0f;
00133 
00134 //VDU states
00135 typedef enum {
00136     VDU_PowerOn               = 0U,
00137     VDU_Idle                  = 1U,
00138     VDU_Run                   = 2U,
00139     VDU_Fault                 = 3U,
00140     VDU_Reset                 = 4U
00141 } VDU_STATE_TYPE;
00142 VDU_STATE_TYPE VDU_STAT = VDU_PowerOn;  // VDU current state
00143 uint16_t VDU_FLT = 0;                   // VDU internal fault code
00144 uint16_t VDU_FLT_ind = 0;               // A copy for indication
00145 uint8_t Reset_to = 0;                   // Timer for reset pending
00146 
00147 //Indicator pattern generation
00148 uint8_t Ind_refresh = 0;                // Flag to copy error bits for indicator
00149 uint8_t Pattern = 0U;
00150 uint16_t Repeat = 0U;                   // Bitwise repeative variable
00151 uint8_t Phase = 0U;                     // 0:Type ind, 1:Num ind, 2:Blank
00152 uint8_t Blk_n = 0U;
00153 //Category repetive code
00154 #define Post_rep 0b00000010
00155 #define Run_rep  0b00000001
00156 #define VDU_rep  0b00000100
00157 //Blink Pattern
00158 #define F_Blink 0b01010101              // blink from LSB to MSB
00159 #define L_Pulse 0b01111110
00160 #define S_Pulse 0b01000000
00161 #define N_Pulse 0b00000000
00162 
00163 //Prototype
00164 void CAN_init(void);            //Initial CAN frequency filter...
00165 void Module_WD(void);           //Software watchdog indicate module state
00166 void IMU_isr(void);             //Use if test mode
00167 void IMU_read(void);            //Update IMU readings by once
00168 void Aux_read(void);            //Update AUX reafings by once
00169 void Idle2Run(void);            //Initializing before running
00170 void Run2Idle(void);            //Initializing before idling
00171 void POST(void);                //Check IMU error
00172 void RUNT(void);                //Check POST, timeout, ShutDrv voltage error
00173 void AWD(void);                 //AWD main program, all wheel drive
00174 void ASL(void);                 //ASL main program, anti slip
00175 //void REG_mask(void);          //For low speed non regen rule, implement in PSU
00176 void Cooler(void);              //Function for active cooler working
00177 void Rx_CAN1(void);             //CAN RX handler
00178 void Tx_CLRerr_CAN1(void);      //Send reset cmd to modules
00179 void Tx_Estop_CAN1(void);       //Send out heart beat but force RTD off
00180 void Tx_Tcmd_CAN1(void);        //Send out heart beat command
00181 void Tx_Qdrv_CAN1(void);        //Send out low speed heart beat for logging
00182 void CANpendTX(void);           //Helper function for CAN Tx
00183 int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
00184 float max_fval(float i1, float i2, float i3, float i4);
00185 uint8_t Indication(             //Blink indicator in pattern * repeat
00186     uint8_t pattern,
00187     uint16_t*repeat,
00188     uint8_t*blk_n);
00189 uint8_t IndicationKernel(       //Generate blink pattern, return 1 if all done
00190     uint8_t*pattern,
00191     uint16_t*repeat,
00192     uint8_t*phase,
00193     uint16_t*Post_ind,
00194     uint16_t*Run_ind,
00195     uint16_t*VDU_ind);