SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 8:f8b1402c8f3c
- Parent:
- 7:f674ef860c9c
- Child:
- 9:c99eeafa6fa3
--- a/main.cpp Thu Nov 14 17:06:09 2019 +0000
+++ b/main.cpp Fri Nov 15 08:19:32 2019 +0000
@@ -1,28 +1,8 @@
#include "mbed.h"
+#include "main.h"
+#define pi 3.14159265359f
+#define d2r 0.01745329252f
#define dt 0.01f
-#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
-#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, 10Hz
-
-#define MODOFL_VDUFLTCode 0x0001U //Drive module timeout after once online
-#define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online
-#define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal
-
DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
DigitalOut Fault_Ind(PC_10,0); //Indicate fault bt flashing, 1 active
@@ -33,113 +13,12 @@
AnalogIn AUX_4(PC_1);
AnalogIn SDn_sense(PB_0); //Shutdown circuit driving end detection
CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
+SPI spi2(PB_15, PB_14, PB_13); //1Mbps, MOSI MISO SCLK, forIMU
Serial pc(USBTX, USBRX, 115200);
Ticker ticker1; //100Hz task
-
CANMessage can_msg_Rx;
CANMessage can_msg_Tx;
-//CAN msg bank
-char temp_msg[8] = {0,0,0,0,0,0,0,0};
-
-//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 FL_Tmodule = 0; // inverter temperature degC, 10Hz recieving
-float FR_Tmodule = 0;
-float RL_Tmodule = 0;
-float RR_Tmodule = 0;
-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 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;
-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; // deg steering wheel angel
-
-//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;
-
-//VDU states
-typedef enum {
- VDU_PowerOn = 0U,
- VDU_Idle = 1U,
- VDU_Run = 2U,
- VDU_Fault = 3U
-} VDU_STATE_TYPE;
-VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; //VDU current state
-uint16_t VDU_FLT = 0; //VDU internal fault code
-
-//Prototype
-void CAN_init(void); //Initial CAN frequency filter...
-void Module_WD(void); //Software watchdog indicate module state
-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
-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_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
-
void timer1_interrupt(void)
{
HSTick += 1;
@@ -332,6 +211,7 @@
// Do low speed state reporting
if (LST_EXFL == 1) {
LST_EXFL = 0;
+ Cooler();
Tx_Qdrv_CAN1();
// Print out error mesagge if exist, 1Hz repeative
@@ -362,17 +242,18 @@
void POST(void)
{
- //Check IMU status
- if(0) {
+ //Check IMU status abnormal
+ if(fabsf(YR_imu) > pi) { //half turn per sec, strange
VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
}
}
void RUNT(void)
{
+ //POST
POST();
- //Check timeout
+ //Check module response timeout
if((FL_online*FR_online*RL_online*RR_online) == 0) {
VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout
}
@@ -380,11 +261,12 @@
VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout
}
- //Check IMU status
- //XXX
-
//Check ShutDrv voltage
- //XXX
+ if(VDU_STAT == VDU_Run) {
+ if(SDn_voltage < 8.0f) {
+ VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv
+ }
+ }
}
void Aux_read(void)
@@ -394,17 +276,44 @@
AUX_2_u16 = AUX_2.read_u16();
AUX_3_u16 = AUX_3.read_u16();
AUX_4_u16 = AUX_4.read_u16();
- SDn_voltage = 18.81f*SDn_sense.read();
+ SDn_voltage = 18.81f*SDn_sense.read(); //Read in Shutdown circuit voltage in output end
}
void IMU_read(void)
{
-
+ //Get readings threw SPI, unfinished 2019/11/15
+ YR_imu = 0.0f;
+ Ax_imu = 0.0f;
+ Ay_imu = 0.0f;
+ Roll_imu = 0.0f;
+ Pitch_imu = 0.0f;
}
void AWD(void)
{
+ if(AWD_HMI) {
+ // A simple version is put here for reading
+ Vb_est = 0.25f * (FL_W_ele + FR_W_ele + RL_W_ele + RR_W_ele);
+ YR_ref = Steer_HMI*d2r*Vb_est/(Base+EG*Vb_est*Vb_est);
+ Mz_reg = (YR_ref - YR_imu) * K_yaw; //K_yaw unfinished 2019/11/15
+ sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
+ FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
+ FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
+ RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
+ RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);
+ } else {
+ FL_Tcmd = 0.25f*Trq_HMI;
+ FR_Tcmd = 0.25f*Trq_HMI;
+ RL_Tcmd = 0.25f*Trq_HMI;
+ RR_Tcmd = 0.25f*Trq_HMI;
+ }
+}
+void ASL(void)
+{
+ //Filter out each motor W_ele and get approximate accel, compare with IMU
+ //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel)
+ //Fill out if enough time
}
void Rx_CAN1(void)
@@ -415,7 +324,7 @@
if(can1.read(can_msg_Rx)) {
switch(can_msg_Rx.id) { //Filtered input message
// Start of 100Hz msg
- case FL_HSB_ID:
+ case FL_HSB_ID://1
//HSB from FL motor drive
FL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
@@ -427,7 +336,7 @@
FL_online = 3;
break;
- case FR_HSB_ID:
+ case FR_HSB_ID://2
//HSB from FR motor drive
FR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
@@ -439,7 +348,7 @@
FR_online = 3;
break;
- case RL_HSB_ID:
+ case RL_HSB_ID://3
//HSB from RL motor drive
RL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
@@ -451,7 +360,7 @@
RL_online = 3;
break;
- case RR_HSB_ID:
+ case RR_HSB_ID://4
//HSB from RR motor drive
RR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
@@ -463,7 +372,7 @@
RR_online = 3;
break;
- case HMI_cmd_ID:
+ case HMI_cmd_ID://5
//HMI command from PSU
AWD_HMI = can_msg_Rx.data[6] & 0x01; //Get AWD switch
RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
@@ -477,7 +386,7 @@
// end of 100Hz msg
// Start of 10Hz msg
- case FL_LSB_ID:
+ case FL_LSB_ID://6
//LSB from FL motor drive
tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
FL_Tmotor = tmp*0.01f;
@@ -487,7 +396,7 @@
FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
break;
- case FR_LSB_ID:
+ case FR_LSB_ID://7
//LSB from FR motor drive
tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
FR_Tmotor = tmp*0.01f;
@@ -497,7 +406,7 @@
FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
break;
- case RL_LSB_ID:
+ case RL_LSB_ID://8
//LSB from RL motor drive
tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
RL_Tmotor = tmp*0.01f;
@@ -507,14 +416,14 @@
RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
break;
- case RR_LSB_ID:
+ case RR_LSB_ID://9
//LSB from RR motor drive
tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
- RL_Tmotor = tmp*0.01f;
+ RR_Tmotor = tmp*0.01f;
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
- RL_Tmodule = tmp*0.01f;
- RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
- RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+ RR_Tmodule = tmp*0.01f;
+ RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+ RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
break;
// end of 10Hz msg
}
@@ -524,9 +433,8 @@
void Tx_CLRerr_CAN1(void)
{
- RTD_cmd = 0; //disable as default
- Tx_Tcmd_CAN1();
- RST_cmd = 0; //on shot
+ Tx_Estop_CAN1(); //disable as default
+ RST_cmd = 0; //clear out on shot
}
void Tx_Estop_CAN1(void)
@@ -538,14 +446,12 @@
void Tx_Tcmd_CAN1(void) // 100 Hz
{
int16_t tmp;
- // Need to change ID for real case 2019/11/14
tmp = (int16_t) (FL_Tcmd * 100.0f);
temp_msg[0] = tmp;
temp_msg[1] = tmp >> 8U;
temp_msg[2] = RTD_cmd;
temp_msg[3] = RST_cmd;
- can_msg_Tx = CANMessage(FL_HSB_ID,temp_msg,8,CANData,CANStandard); // FL_CMD_ID, now only for testing
-// can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
+ can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -554,8 +460,7 @@
temp_msg[1] = tmp >> 8U;
temp_msg[2] = RTD_cmd;
temp_msg[3] = RST_cmd;
- can_msg_Tx = CANMessage(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
-// can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
+ can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -564,8 +469,7 @@
temp_msg[1] = tmp >> 8U;
temp_msg[2] = RTD_cmd;
temp_msg[3] = RST_cmd;
- can_msg_Tx = CANMessage(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
-// can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
+ can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -574,8 +478,7 @@
temp_msg[1] = tmp >> 8U;
temp_msg[2] = RTD_cmd;
temp_msg[3] = RST_cmd;
- can_msg_Tx = CANMessage(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
-// can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
+ can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -583,6 +486,7 @@
void Tx_Qdrv_CAN1(void) // 10 Hz
{
+ int16_t tmp;
// Auxilary sensor reading shitting out
temp_msg[0] = AUX_1_u16;
temp_msg[1] = AUX_1_u16 >> 8U;
@@ -597,27 +501,25 @@
can1.write(can_msg_Tx);
// Qdrive internal state shitting out
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ temp_msg[0] = VDU_FLT;
+ temp_msg[1] = VDU_FLT >> 8U;
+ temp_msg[2] = VDU_STAT;
can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
// IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
- temp_msg[0] = 1;
- temp_msg[1] = 2;
- temp_msg[2] = 3;
- temp_msg[3] = 4;
- temp_msg[4] = 5;
- temp_msg[5] = 6;
- temp_msg[6] = 7;
- temp_msg[7] = 8;
+ tmp = (int16_t) (YR_imu * 10000.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ tmp = (int16_t) (Roll_imu * 100.0f);
+ temp_msg[2] = tmp;
+ temp_msg[3] = tmp >> 8U;
+ tmp = (int16_t) (Pitch_imu * 100.0f);
+ temp_msg[4] = tmp;
+ temp_msg[5] = tmp >> 8U;
+ temp_msg[6] = (int8_t)Ax_imu;
+ temp_msg[7] = (int8_t)Ay_imu;
can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
@@ -649,9 +551,9 @@
can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
- can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8);
+ can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring
can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
- can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
+ can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq
}
void Module_WD(void)
@@ -673,6 +575,16 @@
}
}
+void Cooler(void)
+{
+ //Cooling auto control, unfinish 2019/11/15
+ if(0) {
+ Aux_Rly = 1;
+ } else {
+ Aux_Rly = 0;
+ }
+}
+
int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
int16_t max = i1;
@@ -681,4 +593,4 @@
if(i4 > max) max = i4;
return max;
}
-// pc.printf("SOC: %.2f\n", Module_Total*0.01f);
+// pc.printf("SOC: %.2f\n", Module_Total*0.01f);
\ No newline at end of file