SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 6:fbe401163489
- Parent:
- 5:8116016abee0
- Child:
- 7:f674ef860c9c
--- a/main.cpp Wed Nov 13 13:56:55 2019 +0000
+++ b/main.cpp Thu Nov 14 14:29:31 2019 +0000
@@ -19,9 +19,19 @@
#define Qdrv_stat_ID 0xE1 // Tx
#define IMU_sense_ID 0xE2 // Tx
+#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
DigitalOut LED(D13, 0); //Internal LED output, general purpose
+AnalogIn AUX_1(PC_0); //Auxilaru analog sensor
+AnalogIn AUX_2(PC_4);
+AnalogIn AUX_3(PC_2);
+AnalogIn AUX_4(PC_1);
+AnalogIn SD_sense(PB_0); //Shutdown circuit driving end detection
CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
Serial pc(USBTX, USBRX, 115200);
Ticker ticker1; //100Hz task
@@ -54,94 +64,339 @@
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;
+uint8_t PSU_online = 0;
-//Decoded out data storage
-float FL_Temp_mtr = 0; // motor temperature degC, 10Hz refresh
+//Feedback data decoded out storage
+float FL_Temp_mtr = 0; // motor temperature degC, 10Hz recieving
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 FL_Temp_ivt = 0; // inverter temperature degC, 10Hz recieving
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 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 refresh
+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 refresh
+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 refresh
+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 refresh
+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 refresh
+uint8_t FL_DSM = 0; // DSM state, 100Hz recieving
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 RTD_HMI = 0; // 1 = HMI requesting
+uint8_t RST_HMI = 0; // 1 = HMI request once
+uint8_t AWD_HMI = 0; // 1 = HMI requesting
-//100Hz tasking
-uint8_t timer_flag = 0;
+//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;
-void Rx_CAN1(void);
-void Rx_Decode(void);
-void Tx_Tcmd_CAN1(void);
-void Tx_Qdrv_CAN1(void);
-void Error_CAN1(void);
+//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
-int16_t max_val(int16_t i1, int16_t i2, int16_t i3);
+//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)
{
- timer_flag = 1;
+ HSTick += 1;
+ LSTick += 1;
+ if (HSTick > 9) { // 100Hz
+ HST_EXFL = 1;
+ HSTick = 0;
+ }
+ if (LSTick > 99) { // 10Hz
+ LST_EXFL = 1;
+ LSTick = 0;
+ }
}
int main()
{
- 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);
+ //Init CAN network
+ CAN_init();
+
+ //Start House keeping task
+ printf("VDU start up, pend for module online\n");
+ ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
+ while(1) {
+
+ // Do high speed loop
+ if (HST_EXFL == 1) {
+ HST_EXFL = 0;
+
+ // Get IMU, Auxs
+ IMU_read();
+ Aux_read();
+ Module_WD(); // Module online watch dog
+
+ // Run state machine
+ switch (VDU_STAT) {
+
+ case VDU_PowerOn:
+ /* Power on state
+ * Description:
+ * Simple start up sequence will be done here
+ * Do:
+ * VDU internal POST
+ * Wait till modules + PSU online
+ * To VDU_Idle (RTD off):
+ * Prepare for 4WD main program
+ * To VDU_Fault:
+ * Run the error handling service
+ */
+
+ //Basic IMU test
+ POST();
+
+ //Check if state transition only when all module online
+ if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
+// if(1){ //Force online check pass only for debug 2019/11/14
+
+ if ( //Check if any error
+ (FL_DSM == 3U)||
+ (FR_DSM == 3U)||
+ (RL_DSM == 3U)||
+ (RR_DSM == 3U)||
+ (VDU_FLT != 0)) {
+ VDU_STAT = VDU_Fault;
+ FLT_print = 1;
+ } else {
+ //All module online & POST pass
+ VDU_STAT = VDU_Idle;
+ printf("All module online, VDU now Idle\n");
+ }
+ } //Else keep in state VDU_PowerOn
+ break;
+
+ case VDU_Idle:
+ /* Controller Idle state
+ * Description:
+ * Normal latched state, wait for RTD_HMI set from PSU
+ * 4WD in running but output mask to 0
+ * Do:
+ * 4WD controller
+ * Check:
+ * RUN faults if any
+ * To VDU_Run:
+ * Initialize parameters for start up, set RTD_cmd
+ * To VDU_Fault:
+ * Run the error handling service
+ */
+
+ //Forced RTD_HMI for debug purpose 2019/11/14
+// RTD_HMI = 1; //Should be set if can bus received data
+ //Forced RTD_HMI for debug purpose 2019/11/14
+
+ RUNT(); //Run test
+ AWD(); //AWD main program
-// can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
- can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
+ if ( //Check if any error
+ (FL_DSM == 3U)||
+ (FR_DSM == 3U)||
+ (RL_DSM == 3U)||
+ (RR_DSM == 3U)||
+ (VDU_FLT != 0)) {
+ VDU_STAT = VDU_Fault;
+ FLT_print = 1;
+ } else if (RTD_HMI != 0) { //Or command to run threw PSU
+ //Prepare to send out RTD and start motor
+ Idle2Run();
+ VDU_STAT = VDU_Run;
+ printf("Idle 2 Run\n");
+ } //Else keep in state
+ break;
+
+ case VDU_Run:
+ /* Controller Run state
+ * Description:
+ * Normal latched state, after RTD_HMI is set from PSU
+ * Same to Idle state except RTD_cmd is set
+ * Do:
+ * 4WD controller
+ * Check:
+ * RUN faults if any
+ * To VDU_Idle:
+ * Initialize parameters for idling, reset RTD_cmd
+ * To VDU_Fault:
+ * Run the error handling service
+ */
+
+ RUNT(); //Run test
+ AWD(); //AWD main program
+
+ //Temporary debug posting area 2019/11/14
+ //printf("%d,%d\n", Encoder_cnt, Encoder_del);
+ //printf("%d\n\r", (int16_t)Tmodule);//
+ //printf("%d\n\r", (int16_t)Vbus);
- //House keeping task
- ticker1.attach_us(&timer1_interrupt, 10000); //10 ms
- while(1) {
- if(timer_flag == 1) {
+ if ( //Check if any error
+ (FL_DSM == 3U)||
+ (FR_DSM == 3U)||
+ (RL_DSM == 3U)||
+ (RR_DSM == 3U)||
+ (VDU_FLT != 0)) {
+ VDU_STAT = VDU_Fault;
+ FLT_print = 1;
+ } else if (RTD_HMI != 1) { //Or command to stop threw can bus
+ Run2Idle();
+ VDU_STAT = VDU_Idle;
+ printf("Run 2 Idle\n");
+ } //Else keep in state
+ break;
+
+ case VDU_Fault:
+ /* Controller Fault state
+ * Description:
+ * Fault latched state if any faults is detected
+ * Same to Idle state except keep till RTD_HMI reset
+ * Do:
+ * Nothing, like a piece of shit
+ * Check:
+ * RUN faults if any
+ * To VDU_PowerOn:
+ * Restart VDU
+ */
+
+ RUNT(); //Run test
+
+ if (RST_HMI == 1) { //PSU reset to clear error
+ RST_cmd = 1;
+ FLT_print = 0;
+ VDU_STAT = VDU_PowerOn;
+ printf("Module reset\nVDU restarting...\n");
+ } //Else keep in state
+ break;
+ }
+
+ // Shit out torque distribution
+ if(VDU_STAT == VDU_Run) {
+ //Allow output torque
+ Tx_Tcmd_CAN1();
+ } else if(RST_cmd != 0) {
+ Tx_CLRerr_CAN1();
+ } else {
+ //Force RTD off when not in VDU_Run
+ Tx_Estop_CAN1();
+ }
+
+ }
+
+
+ // Do low speed state reporting
+ if (LST_EXFL == 1) {
+ LST_EXFL = 0;
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);
-// pc.printf("Max1: %.3f\n", Module_Max_BMS1*0.0001f);
-// pc.printf("Max2: %.3f\n", Module_Max_BMS2*0.0001f);
-// pc.printf("Temp: %.1f\n", Max_temp*0.01f);
- timer_flag = 0;
+
+ // Print out error mesagge if exist, 1Hz repeative
+ if(FLT_print != 0) {
+ FLT_print_cnt += 1;
+ if(FLT_print_cnt > 19) {
+ printf("POST FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Post,FR_FLT_Post,RL_FLT_Post,RR_FLT_Post);
+ printf("RUN FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Run,FR_FLT_Run,RL_FLT_Run,RR_FLT_Run);
+ printf("VDU Err:0x%04X\n\n", VDU_FLT);
+ FLT_print_cnt = 0;
+ }
+ }
+
}
}
}
+void Idle2Run(void)
+{
+ RTD_cmd = 1;
+}
+
+void Run2Idle(void)
+{
+ RTD_cmd = 0;
+}
+
+void POST(void)
+{
+ //Check IMU status
+ if(0) {
+ VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
+ }
+}
+
+void RUNT(void)
+{
+ POST();
+
+ //Check timeout
+ if((FL_online*FR_online*RL_online*RR_online) == 0) {
+ VDU_FLT |= MODOFL_VDUFLTCode; //Module timeout
+ }
+ if(PSU_online == 0) {
+ VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout
+ }
+
+ //Check IMU status
+ //XXX
+
+ //Check ShutDrv voltage
+ //XXX
+}
+
+void Aux_read(void)
+{
+}
+
+void IMU_read(void)
+{
+}
+
+void AWD(void)
+{
+
+}
+
void Rx_CAN1(void)
{
LED = 1;
@@ -188,17 +443,75 @@
LED = 0;
}
-void Rx_Decode(void)
+void Tx_CLRerr_CAN1(void)
{
+ RTD_cmd = 0; //disable as default
+ Tx_Tcmd_CAN1();
+ RST_cmd = 0; //on shot
+}
+
+void Tx_Estop_CAN1(void)
+{
+ RTD_cmd = 0; //force disable
+ Tx_Tcmd_CAN1();
}
-void Tx_Tcmd_CAN1(void)
+void Tx_Tcmd_CAN1(void) // 100 Hz
{
+ // Need to change ID for real case 2019/11/13
+ 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(FL_HSB_ID,temp_msg,8,CANData,CANStandard); // FL_CMD_ID, now only for testing
+ CANpendTX();
+ can1.write(can_msg_Tx);
+
+ 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(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
+
+ 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(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
+
+ 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(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
+
}
-void Tx_Qdrv_CAN1(void)
+void Tx_Qdrv_CAN1(void) // 10 Hz
{
- uint32_t ID = FL_HSB_ID;
+ // Auxilary sensor reading shitting out
temp_msg[0] = 1;
temp_msg[1] = 2;
temp_msg[2] = 3;
@@ -207,38 +520,93 @@
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
- }
+ can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ 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;
+ can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
- ID = RR_HSB_ID;
- can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
- if(!can1.write(can_msg_Tx)) {
- //Report fault
+ // 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;
+ can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
+}
+
+void CANpendTX(void)
+{
+ //Pend till TX box has empty slot, timeout will generate error
+ uint32_t timeout = 0;
+ while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
+ //Wait till non empty
+ timeout += 1;
+ if(timeout > 10000) {
+ // Put some timeout error handler
+ break;
+ }
}
}
-//void Send2CAN1(void)
-//{
-// 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 CAN_init(void)
+{
+ //Set CAN system
+ 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.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
+ can1.attach(&Rx_CAN1, CAN::RxIrq); //CAN1 Recieve Irq
+}
-int16_t max_val(int16_t i1, int16_t i2, int16_t i3)
+void Module_WD(void)
+{
+ if (FL_online != 0) {
+ FL_online -= 1;
+ }
+ if (FR_online != 0) {
+ FR_online -= 1;
+ }
+ if (RL_online != 0) {
+ RL_online -= 1;
+ }
+ if (RR_online != 0) {
+ RR_online -= 1;
+ }
+ if (PSU_online != 0) {
+ PSU_online -= 1;
+ }
+}
+
+int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
int16_t max = i1;
if(i2 > max) max = i2;
if(i3 > max) max = i3;
+ if(i4 > max) max = i4;
return max;
-}
\ No newline at end of file
+}
+// pc.printf("SOC: %.2f\n", Module_Total*0.01f);