SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 5:8116016abee0
- Parent:
- 4:f05bef255292
- Child:
- 6:fbe401163489
--- a/main.cpp Tue Nov 12 14:12:58 2019 +0000
+++ b/main.cpp Wed Nov 13 13:56:55 2019 +0000
@@ -1,130 +1,136 @@
#include "mbed.h"
#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
+#define RL_HSB_ID 0xA2 // Rx
+#define RR_HSB_ID 0xA3 // Rx
+#define FL_LSB_ID 0xB0 // Rx, 10Hz
+#define FR_LSB_ID 0xB1 // Rx
+#define RL_LSB_ID 0xB2 // Rx
+#define RR_LSB_ID 0xB3 // Rx
+#define PSU_cmd_ID 0xC4 // Rx, 100Hz
-DigitalOut Cool(PA_15,0);
+#define FL_CMD_ID 0xC0 // Tx, 100Hz
+#define FR_CMD_ID 0xC1 // Tx
+#define RL_CMD_ID 0xC2 // Tx
+#define RR_CMD_ID 0xC3 // Tx
+#define AUX_sense_ID 0xE0 // Tx, 10Hz
+#define Qdrv_stat_ID 0xE1 // Tx
+#define IMU_sense_ID 0xE2 // Tx
+
+DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
+DigitalOut Fault_Ind(PC_10,0); //Indicate fault bt flashing, 1 active
DigitalOut LED(D13, 0); //Internal LED output, general purpose
-CAN can1(PA_11, PA_12, 1000000); //1Mbps, contain critical torque command message
+CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
Serial pc(USBTX, USBRX, 115200);
-Ticker ticker1; //General
+Ticker ticker1; //100Hz task
+
+CANMessage can_msg_Rx;
+CANMessage can_msg_Tx;
-Ticker ticker2; //Logger
+//Recive bank for HSB & LFB
+char temp_msg[8] = {0,0,0,0,0,0,0,0};
+char FL_HFB[8]; //data space to speed up can_Rx speed
+char FR_HFB[8];
+char RL_HFB[8];
+char RR_HFB[8];
+char FL_LFB[8];
+char FR_LFB[8];
+char RL_LFB[8];
+char RR_LFB[8];
-CANMessage can_msg_1;
-CANMessage can_msg_2;
-CANMessage can_msg_send;
-char data_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 10 to hold for 0.1sec
+uint8_t RL_online = 0; // -1 per 100Hz task
+uint8_t RR_online = 0;
-int16_t IGBT_Temp_A = 0; // *0.1 to get float value
-int16_t IGBT_Temp_B = 0; // *0.1 to get float value
-int16_t IGBT_Temp_C = 0; // *0.1 to get float value
-int16_t IGBT_Temp = 0; // *0.1 to get float value
-int16_t Motor_Temp = 0; // *0.1 to get float value
-int16_t DC_Current = 0; // *0.1 to get float value
-int16_t DC_Voltage = 0; // *0.1 to get float value
-int16_t Motor_Speed = 0;
+//Decoded out data storage
+float FL_Temp_mtr = 0; // motor temperature degC, 10Hz refresh
+float FR_Temp_mtr = 0;
+float RL_Temp_mtr = 0;
+float RR_Temp_mtr = 0;
+float FL_Temp_ivt = 0; // inverter temperature degC, 10Hz refresh
+float FR_Temp_ivt = 0;
+float RL_Temp_ivt = 0;
+float RR_Temp_ivt = 0;
+uint16_t FL_FLT_Run = 0; // RUN fault code, 10Hz refresh
+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 refresh
+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 refresh
+float FR_Trq_fil3 = 0;
+float RL_Trq_fil3 = 0;
+float RR_Trq_fil3 = 0;
+float FL_Trq_est = 0; // Estimated Torque, 100Hz refresh
+float FR_Trq_est = 0;
+float RL_Trq_est = 0;
+float RR_Trq_est = 0;
+float FL_W_ele = 0; // Estimated W_ele, 100Hz refresh
+float FR_W_ele = 0;
+float RL_W_ele = 0;
+float RR_W_ele = 0;
+uint8_t FL_DSM = 0; // DSM state, 100Hz refresh
+uint8_t FR_DSM = 0;
+uint8_t RL_DSM = 0;
+uint8_t RR_DSM = 0;
+CAN_HandleTypeDef hcan1;
+//From Pedal Box msg
+uint8_t RTD_HMI = 0; //1 = requesting
+uint8_t AWD_HMI = 0; //1 = requesting
-uint8_t CAN_flag_1 = 0;
-uint8_t CAN_flag_2 = 0;
+//100Hz tasking
uint8_t timer_flag = 0;
-uint8_t RTD_State = 0; //use as bool
-uint8_t SD_OK = 0; //SD OK flag
-uint8_t Logging = 0; //Logging flag
-
-int16_t Max_temp_within_BMS1 = 0; //*0.01 to degC
-int16_t Max_temp_within_BMS2 = 0; //*0.01 to degC
-int16_t Max_temp = 0; //*0.01 to degC
-
-uint16_t Module_Total_BMS1 = 0; //*0.01 to Voltage ~ 120 V
-uint16_t Module_Total_BMS2 = 0; //*0.01 to Voltage ~ 120 V
-uint16_t Module_Total = 0; //*0.01 to Voltage ~ 240 V
-uint16_t Module_Min_BMS1 = 0; //*0.0001 to Voltage ~ 3.0 V
-uint16_t Module_Min_BMS2 = 0; //*0.0001 to Voltage ~ 3.0 V
-uint16_t Module_Min = 0; //*0.0001 to Voltage ~ 3.0 V
-
-uint16_t Module_Max_BMS1 = 0; //*0.0001 to Voltage ~ 3.0 V
-uint16_t Module_Max_BMS2 = 0; //*0.0001 to Voltage ~ 3.0 V
-uint16_t Module_Max = 0; //*0.0001 to Voltage ~ 3.0 V
+void Rx_CAN1(void);
+void Rx_Decode(void);
+void Tx_Tcmd_CAN1(void);
+void Tx_Qdrv_CAN1(void);
+void Error_CAN1(void);
-void send(char ID);
-void CAN_RX1(void);
-void CAN_RX2(void);
-void Send2CAN1(void);
-
-void start_log(void);
-void stop_log(void);
-void autoname(void);
int16_t max_val(int16_t i1, int16_t i2, int16_t i3);
void timer1_interrupt(void)
{
timer_flag = 1;
}
-void timer2_interrupt(void);
int main()
{
- CAN_flag_1 = 0;
- CAN_flag_2 = 0;
+ SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off
+ can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0); // ID filter listing mode
+ can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1);
+ can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2);
+ can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3);
+ can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4);
+ 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(PSU_cmd_ID,0xFFFF,CANStandard,8);
- can1.attach(&CAN_RX1, CAN::RxIrq); //CAN1 Recieve Irq
- ticker1.attach(&timer1_interrupt, 1); //1sec
+// can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
+ can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
- //General task
+ //House keeping task
+ ticker1.attach_us(&timer1_interrupt, 10000); //10 ms
while(1) {
if(timer_flag == 1) {
-
- if(CAN_flag_1) {
- if(RTD_State) {
- Cool = 1;
- if(!Logging) {
- autoname(); //GENERATE NEW FILE EACH call fp ready or stuck
- if(SD_OK) {
- Logging = 1;
- start_log();
- }
- }
- } else {
- Cool = 0;
- if(Logging) {
- Logging = 0;
- stop_log();
- }
- }
- CAN_flag_1 = 0;
- }
-
- if(CAN_flag_2) {
- //Total
- Module_Total = Module_Total_BMS1 + Module_Total_BMS2;
-
- //Min
- if(Module_Min_BMS1 < Module_Min_BMS2) {
- Module_Min = Module_Min_BMS1;
- } else {
- Module_Min = Module_Min_BMS2;
- }
-
- //Max
- if(Module_Max_BMS1 < Module_Max_BMS2) {
- Module_Max = Module_Max_BMS2;
- } else {
- Module_Max = Module_Max_BMS1;
- }
-
- //Temp
- if(Max_temp_within_BMS1 < Max_temp_within_BMS2) {
- Max_temp = Max_temp_within_BMS2;
- } else {
- Max_temp = Max_temp_within_BMS1;
- }
-
- CAN_flag_2 = 0;
- Send2CAN1();
- }
-
+ Tx_Qdrv_CAN1();
// pc.printf("SOC: %.2f\n", Module_Total*0.01f);
// pc.printf("Min: %.3f\n", Module_Min*0.0001f);
// pc.printf("Max: %.3f\n", Module_Max*0.0001f);
@@ -136,141 +142,99 @@
}
}
-void timer2_interrupt(void)
+void Rx_CAN1(void)
+{
+ LED = 1;
+ if(can1.read(can_msg_Rx)) {
+// switch(can_msg_Rx.id) { //Filtered the input message and do corresponding action
+// case 0xF0:
+// //Internal state address
+// RTD_State = can_msg_Rx.data[7] & 0x01; //get bit "0", result ( 1 = RTD, 0 = OFF )
+// CAN_flag_1 = 1;
+// break;
+//
+// case 0xA0 :
+// //Temperature infrom address 1
+// IGBT_Temp_A = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+// IGBT_Temp_B = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+// IGBT_Temp_C = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+// IGBT_Temp = max_val(IGBT_Temp_A,IGBT_Temp_B,IGBT_Temp_C);
+// CAN_flag_1 = 1;
+// break;
+//
+// case 0xA2 :
+// //Temperature infrom address 3
+// Motor_Temp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
+// CAN_flag_1 = 1;
+// break;
+//
+// case 0xA5:
+// Motor_Speed = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
+// break;
+//
+// case 0xA6 :
+// //Current infrom address
+// DC_Current = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
+// CAN_flag_1 = 1;
+// break;
+//
+// case 0xA7 :
+// //Voltage infrom address
+// DC_Voltage = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
+// CAN_flag_1 = 1;
+// break;
+// }
+ }
+ LED = 0;
+}
+
+void Rx_Decode(void)
{
}
-void start_log(void)
-{
-
-}
-
-void stop_log(void)
+void Tx_Tcmd_CAN1(void)
{
-
-}
-
-void autoname(void)
-{
-
}
-void CAN_RX1(void)
+void Tx_Qdrv_CAN1(void)
{
- if(can1.read(can_msg_1)) {
- switch(can_msg_1.id) { //Filtered the input message and do corresponding action
- case 0xF0:
- //Internal state address
- RTD_State = can_msg_1.data[7] & 0x01; //get bit "0", result ( 1 = RTD, 0 = OFF )
- CAN_flag_1 = 1;
- break;
-
- case 0xA0 :
- //Temperature infrom address 1
- IGBT_Temp_A = can_msg_1.data[1] << 8 | can_msg_1.data[0];
- IGBT_Temp_B = can_msg_1.data[3] << 8 | can_msg_1.data[2];
- IGBT_Temp_C = can_msg_1.data[5] << 8 | can_msg_1.data[4];
- IGBT_Temp = max_val(IGBT_Temp_A,IGBT_Temp_B,IGBT_Temp_C);
- CAN_flag_1 = 1;
- break;
+ uint32_t ID = FL_HSB_ID;
+ 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;
+ can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
+ if(!can1.write(can_msg_Tx)) {
+ //Report fault
+ }
- case 0xA2 :
- //Temperature infrom address 3
- Motor_Temp = can_msg_1.data[5] << 8 | can_msg_1.data[4];
- CAN_flag_1 = 1;
- break;
-
- case 0xA5:
- Motor_Speed = can_msg_1.data[3] << 8 | can_msg_1.data[2];
- break;
-
- case 0xA6 :
- //Current infrom address
- DC_Current = can_msg_1.data[7] << 8 | can_msg_1.data[6];
- CAN_flag_1 = 1;
- break;
-
- case 0xA7 :
- //Voltage infrom address
- DC_Voltage = can_msg_1.data[1] << 8 | can_msg_1.data[0];
- CAN_flag_1 = 1;
- break;
- }
+ ID = RR_HSB_ID;
+ can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
+ if(!can1.write(can_msg_Tx)) {
+ //Report fault
}
}
-//void CAN_RX2(void)
+//void Send2CAN1(void)
//{
-// if(can2.read(can_msg_2)) {
-// switch(can_msg_2.id) {
-//// case 0xD1:
-//// //BMS1 soc
-//// Module_Total_BMS1 = can_msg_2.data[1] << 8 | can_msg_2.data[0];
-//// Module_Total_BMS1 += can_msg_2.data[3] << 8 | can_msg_2.data[2];
-//// Module_Total_BMS1 += can_msg_2.data[5] << 8 | can_msg_2.data[4];
-//// CAN_flag_2 = 1;
-//// break;
-//// case 0xD2:
-//// //BMS1 min Volt
-//// Module_Min_BMS1 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-//// CAN_flag_2 = 1;
-//// break;
-//// case 0xD3:
-//// //BMS1 max Volt
-//// Module_Max_BMS1 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-//// CAN_flag_2 = 1;
-//// break;
-// case 0xD4:
-// //BMS1 max temperature
-// Max_temp_within_BMS1 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-// CAN_flag_2 = 1;
-// break;
-//
-//// case 0xE1:
-//// //BMS2 soc
-//// Module_Total_BMS2 = can_msg_2.data[1] << 8 | can_msg_2.data[0];
-//// Module_Total_BMS2 += can_msg_2.data[3] << 8 | can_msg_2.data[2];
-//// Module_Total_BMS2 += can_msg_2.data[5] << 8 | can_msg_2.data[4];
-//// CAN_flag_2 = 1;
-//// break;
-//// case 0xE2:
-//// //BMS2 min Volt
-//// Module_Min_BMS2 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-//// CAN_flag_2 = 1;
-//// break;
-//// case 0xE3:
-//// //BMS2 max Volt
-//// Module_Max_BMS2 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-//// CAN_flag_2 = 1;
-//// break;
-// case 0xE4:
-// //BMS2 max temperature
-// Max_temp_within_BMS2 = can_msg_2.data[7] << 8 | can_msg_2.data[6];
-// CAN_flag_2 = 1;
-// break;
-// }
-// }
+// uint32_t ID = 0xF1;
+// temp_msg[0] = Module_Total;
+// temp_msg[1] = Module_Total >> 8;
+// temp_msg[2] = Module_Min;
+// temp_msg[3] = Module_Min >> 8;
+// temp_msg[4] = Module_Max;
+// temp_msg[5] = Module_Max >> 8;
+// temp_msg[6] = Max_temp;
+// temp_msg[7] = Max_temp >> 8;
+// send_CAN1();
+// can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
+// can1.write(can_msg_Tx);
//}
-void Send2CAN1(void)
-{
- data_msg[0] = Module_Total;
- data_msg[1] = Module_Total >> 8;
- data_msg[2] = Module_Min;
- data_msg[3] = Module_Min >> 8;
- data_msg[4] = Module_Max;
- data_msg[5] = Module_Max >> 8;
- data_msg[6] = Max_temp;
- data_msg[7] = Max_temp >> 8;
- send(0xF1);
-}
-
-void send(char ID)
-{
- can_msg_send = CANMessage(ID,data_msg,8,CANData,CANStandard);
- can1.write(can_msg_send);
-}
-
int16_t max_val(int16_t i1, int16_t i2, int16_t i3)
{
int16_t max = i1;