Just a regular publish

Dependencies:   mbed imu_driver

Committer:
open4416
Date:
Sat Dec 28 14:16:21 2019 +0000
Revision:
18:780366f2534c
Parent:
14:bcf5cb4d08a5
Child:
19:d68f21173c23
Add idiot blinky ability

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