SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.h@27:52260996ce32, 2021-03-06 (annotated)

Committer:
open4416
Date:
Sat Mar 06 00:11:27 2021 +0000
Revision:
27:52260996ce32
Parent:
25:3c6e83b449b2 
Yet test

Who changed what in which revision?

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