SHENG-HEN HSIEH / Mbed 2 deprecated VDU_2021

Just a regular publish

Dependencies:   mbed imu_driver

main.h@14:bcf5cb4d08a5, 2019-12-20 (annotated)

Committer:
open4416
Date:
Fri Dec 20 04:51:03 2019 +0000
Revision:
14:bcf5cb4d08a5
Parent:
13:ac024885d0bf
Child:
18:780366f2534c
Add AHRS/IMU in, done by Jacky

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 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 14:bcf5cb4d08a5 36 float YR_imu = 0; // deg/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);