SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
main.h
- Committer:
- open4416
- Date:
- 2021-03-06
- Revision:
- 27:52260996ce32
- Parent:
- 25:3c6e83b449b2
File content as of revision 27:52260996ce32:
#define Track 1.240f //Average wheel track #define Base 1.560f //wheel base #define Rwhl 0.230f //Wheel radius #define HCG 0.204f //Height of CG #define Mtot 320.0f //Total weight with driver #define g0 9.81f //gravity #define EG 0.000f //EG<0: over steer, EG>0:under steer #define K_yaw 1.000f //Gain for yaw regulator #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) // CAN ID define #define FL_HSB_ID 0xA0 // Rx, 100Hz #define FR_HSB_ID 0xA1 // Rx, 100Hz #define RL_HSB_ID 0xA2 // Rx, 100Hz #define RR_HSB_ID 0xA3 // Rx, 100Hz #define FL_LSB_ID 0xB0 // Rx, 10Hz #define FR_LSB_ID 0xB1 // Rx, 10Hz #define RL_LSB_ID 0xB2 // Rx, 10Hz #define RR_LSB_ID 0xB3 // Rx, 10Hz #define HMI_cmd_ID 0xC4 // Rx, 100Hz #define FL_CMD_ID 0xC0 // Tx, 100Hz #define FR_CMD_ID 0xC1 // Tx, 100Hz #define RL_CMD_ID 0xC2 // Tx, 100Hz #define RR_CMD_ID 0xC3 // Tx, 100Hz #define AUX_sense_ID 0xE0 // Tx, 10Hz #define Qdrv_stat_ID 0xE1 // Tx, 10Hz #define IMU_sense_ID 0xE2 // Tx, 100Hz #define RegularVar_ID 0xE3 // Tx, 100Hz #define PedalStat_ID 0xE4 // Tx, 10Hz #define MODOFL_VDUFLTCode 0x0001U //Drive module timeout #define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout #define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal #define ShDVol_VDUFLTCode 0x0008U //Shutdown voltage abnormal #define DSM_VDUFLTCode 0x0010U //One or more slave report DSM_Fault //CAN msg bank char temp_msg[8] = {0,0,0,0,0,0,0,0}; //IMU readings float YR_imu = 0; // deg/s yawrate estimated threw IMU sensor fusion float Roll_imu = 0; // deg float Pitch_imu = 0; // deg float Yaw_imu = 0; // deg float Ax_imu = 0; // g float Ay_imu = 0; // g //AWD variable float YR_ref = 0; // rad/s yawrate reference generate from ideal single track model float Vx_wss = 0; // m/s estimated body Velocity X (wheelspeed only) float Vx_fil = 0; // m/s estimated body Velocity X float Mz_reg = 0; // Nm addon yaw torque regulating yawrate, gain given by "K_yaw" float sig = 0; // Front rear distribution factor //CAN to motor drive module, 100Hz //msg ID: 0xC0~0xC3 float FL_Tcmd = 0; // *100 before sent in int16_t float FR_Tcmd = 0; float RL_Tcmd = 0; float RR_Tcmd = 0; uint8_t RTD_cmd = 0; // start inverter switching cmd uint8_t RST_cmd = 0; // send out once to reset module fault //Module online flag uint8_t FL_online = 0; // 0 indicate detection timeout uint8_t FR_online = 0; // reset value is 3 to hold for 0.03sec uint8_t RL_online = 0; // -1 per 100Hz task uint8_t RR_online = 0; uint8_t PSU_online = 0; //Feedback data decoded out storage float FL_Tmotor = 0; // motor temperature degC, 10Hz recieving float FR_Tmotor = 0; float RL_Tmotor = 0; float RR_Tmotor = 0; float Max_Tmotor = 0; // motor temperature degC, 10Hz recieving float FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving float FR_Tmodule = 0; float RL_Tmodule = 0; float RR_Tmodule = 0; float Max_Tmodule = 0; // motor temperature degC, 10Hz recieving uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz recieving uint16_t FR_FLT_Run = 0; uint16_t RL_FLT_Run = 0; uint16_t RR_FLT_Run = 0; uint16_t FLT_Run = 0; uint16_t FLT_Run_ind = 0; // for indication uint16_t FL_FLT_Post = 0; // POST fault code, 10Hz recieving uint16_t FR_FLT_Post = 0; uint16_t RL_FLT_Post = 0; uint16_t RR_FLT_Post = 0; uint16_t FLT_Post = 0; uint16_t FLT_Post_ind = 0; // for indication float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving float FR_Trq_fil3 = 0; float RL_Trq_fil3 = 0; float RR_Trq_fil3 = 0; float FL_Trq_est = 0; // Estimated Torque, 100Hz recieving float FR_Trq_est = 0; float RL_Trq_est = 0; float RR_Trq_est = 0; float FL_W_ele = 0; // Estimated W_ele, 100Hz recieving float FR_W_ele = 0; float RL_W_ele = 0; float RR_W_ele = 0; uint8_t FL_DSM = 0; // DSM state, 100Hz recieving uint8_t FR_DSM = 0; uint8_t RL_DSM = 0; uint8_t RR_DSM = 0; //From Pedal Box msg uint8_t RTD_HMI = 0; // 1 = HMI requesting uint8_t RST_HMI = 0; // 1 = HMI request once uint8_t AWD_HMI = 0; // 1 = HMI requesting float Trq_HMI = 0.0f; // Nm user request total torque float Steer_HMI = 0.0f; // Steering wheel signal, *st2r to get rad uint8_t RTD_SW = 0; // 1 = RTD switch on //10/100Hz tasking uint8_t HSTick = 5; // High speed tick uint8_t LSTick = 0; uint8_t HST_EXFL = 0; // High speed execution flag uint8_t LST_EXFL = 0; uint8_t FLT_print = 0; // Send repeative error message uint8_t FLT_print_cnt = 0; uint16_t AUX_1_u16 = 0x0; // acquired data uint16_t AUX_2_u16 = 0x0; uint16_t AUX_3_u16 = 0x0; uint16_t AUX_4_u16 = 0x0; float SDn_voltage = 0.0f; float Brk_voltage = 0.0f; //VDU states typedef enum { VDU_PowerOn = 0U, VDU_Idle = 1U, VDU_Run = 2U, VDU_Fault = 3U, VDU_Reset = 4U } VDU_STATE_TYPE; VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; // VDU current state uint16_t VDU_FLT = 0; // VDU internal fault code uint16_t VDU_FLT_ind = 0; // A copy for indication uint8_t Reset_to = 0; // Timer for reset pending //Indicator pattern generation uint8_t Ind_refresh = 0; // Flag to copy error bits for indicator uint8_t Pattern = 0U; uint16_t Repeat = 0U; // Bitwise repeative variable uint8_t Phase = 0U; // 0:Type ind, 1:Num ind, 2:Blank uint8_t Blk_n = 0U; //Category repetive code #define Post_rep 0b00000010 #define Run_rep 0b00000001 #define VDU_rep 0b00000100 //Blink Pattern #define F_Blink 0b01010101 // blink from LSB to MSB #define L_Pulse 0b01111110 #define S_Pulse 0b01000000 #define N_Pulse 0b00000000 //Prototype void CAN_init(void); //Initial CAN frequency filter... void Module_WD(void); //Software watchdog indicate module state void IMU_isr(void); //Use if test mode void IMU_read(void); //Update IMU readings by once void Aux_read(void); //Update AUX reafings by once void Idle2Run(void); //Initializing before running void Run2Idle(void); //Initializing before idling void POST(void); //Check IMU error void RUNT(void); //Check POST, timeout, ShutDrv voltage error void AWD(void); //AWD main program, all wheel drive void ASL(void); //ASL main program, anti slip //void REG_mask(void); //For low speed non regen rule, implement in PSU void Cooler(void); //Function for active cooler working void Rx_CAN1(void); //CAN RX handler void Tx_CLRerr_CAN1(void); //Send reset cmd to modules void Tx_Estop_CAN1(void); //Send out heart beat but force RTD off void Tx_Tcmd_CAN1(void); //Send out heart beat command void Tx_Qdrv_CAN1(void); //Send out low speed heart beat for logging void CANpendTX(void); //Helper function for CAN Tx int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4); float max_fval(float i1, float i2, float i3, float i4); uint8_t Indication( //Blink indicator in pattern * repeat uint8_t pattern, uint16_t*repeat, uint8_t*blk_n); uint8_t IndicationKernel( //Generate blink pattern, return 1 if all done uint8_t*pattern, uint16_t*repeat, uint8_t*phase, uint16_t*Post_ind, uint16_t*Run_ind, uint16_t*VDU_ind);