SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.h@19:d68f21173c23, 2020-01-08 (annotated)

Committer:
open4416
Date:
Wed Jan 08 13:10:51 2020 +0000
Revision:
19:d68f21173c23
Parent:
18:780366f2534c
Child:
20:e9daae390513
Add test code for IMU, in case future update

Who changed what in which revision?

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