SHENG-HEN HSIEH
/
VDU_2021
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 18:780366f2534c, committed 2019-12-28
- Comitter:
- open4416
- Date:
- Sat Dec 28 14:16:21 2019 +0000
- Parent:
- 17:dc46c794a8db
- Child:
- 19:d68f21173c23
- Commit message:
- Add idiot blinky ability
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Dec 20 06:35:07 2019 +0000
+++ b/main.cpp Sat Dec 28 14:16:21 2019 +0000
@@ -172,6 +172,7 @@
RUNT(); //Run test
if (RST_HMI == 1) { //PSU reset to clear error
+ RST_HMI = 0;
RST_cmd = 1;
FLT_print = 0; //Stop error printing
VDU_STAT = VDU_PowerOn;
@@ -203,21 +204,62 @@
// Do low speed state reporting 10 Hz
if (LST_EXFL == 1) {
LST_EXFL = 0;
+ //Cooling
Cooler();
+
+ //Print low speed data on CAN
Tx_Qdrv_CAN1();
- // Print out error mesagge if exist, 1Hz repeative
+ // Print out error mesagge if exist, 0.5Hz repeative
if(FLT_print != 0) {
+ //Merge Faults
+ FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post;
+ FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run;
+
+ //UART
FLT_print_cnt += FLT_print;
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\n0x%04X\n\n", VDU_FLT);
+ if(FLT_Post!=0)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);
+ if(FLT_Run!=0)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);
+ if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT);
FLT_print_cnt = 0;
}
+
+ //LED
+ if(Ind_refresh) {
+ // Refresh data for LED indication after run threw
+ FLT_Post_ind = FLT_Post;
+ FLT_Run_ind = FLT_Run;
+ VDU_FLT_ind = VDU_FLT;
+ Ind_refresh = 0; // Set after run threw all error bits
+ }
+ } else {
+ // Clear & stop LED when no faults
+ FLT_Post_ind = 0;
+ FLT_Run_ind = 0;
+ VDU_FLT_ind = 0;
+ Repeat = 0U;
+ Phase = 0U;
+ Blk_n = 0U;
}
- LED = !LED;
+ // Blinky output
+ if (VDU_STAT != VDU_PowerOn) {
+ // Case after poweron (all module online) or fault(s) occur
+ Ind_refresh = IndicationKernel(
+ &Pattern,
+ &Repeat,
+ &Phase,
+ &FLT_Post_ind,
+ &FLT_Run_ind,
+ &VDU_FLT_ind);
+ LED = Indication(Pattern, &Repeat, &Blk_n);
+ Fault_Ind = LED;
+ } else {
+ // Case no fault while waiting for all module online
+ LED = !LED; //Fast 5Hz blinky indicate the activity
+ Fault_Ind = LED;
+ }
}
// End of low speed state reporting
@@ -237,7 +279,7 @@
void POST(void)
{
//Check IMU status abnormal
- if(fabsf(YR_imu) > 250) { //half turn per sec, strange
+ if(fabsf(YR_imu) > 250) { //half turn per sec, strange
VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
}
}
@@ -276,7 +318,7 @@
void IMU_read(void)
{
- //Get readings threw back ground, direction not checked 2019/12/20
+ //Get readings threw back ground, direction not checked 2019/12/20
YR_imu = imu.imuProcessedData.rate[2];
Ax_imu = imu.imuProcessedData.accel[0];
Ay_imu = imu.imuProcessedData.accel[1];
@@ -313,7 +355,7 @@
void Rx_CAN1(void)
{
- LED = 1;
+// LED = 1;
int16_t tmp;
if(can1.read(can_msg_Rx)) {
@@ -329,6 +371,10 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
FL_Trq_est = tmp * 0.01f;
FL_online = 3;
+ //If fault
+ if(FL_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case FR_HSB_ID://2
@@ -341,6 +387,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
FR_Trq_est = tmp * 0.01f;
FR_online = 3;
+ if(FR_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case RL_HSB_ID://3
@@ -353,6 +402,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
RL_Trq_est = tmp * 0.01f;
RL_online = 3;
+ if(RL_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case RR_HSB_ID://4
@@ -365,6 +417,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
RR_Trq_est = tmp * 0.01f;
RR_online = 3;
+ if(RR_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case HMI_cmd_ID://5
@@ -423,7 +478,7 @@
// end of 10Hz msg
}
}
- LED = 0;
+// LED = 0;
}
void Tx_CLRerr_CAN1(void)
@@ -579,6 +634,112 @@
PSU_online -= 1;
}
}
+uint8_t Indication( // Blink indicator in pattern * repeat
+ uint8_t pattern,
+ uint16_t*repeat,
+ uint8_t*blk_n)
+{
+ uint8_t out = 0;
+ if(*repeat==0) return out; // reject repeat = 0 case, out = 0
+ out = (pattern>>(*blk_n)) & 1U; // blink from LSB to MSB
+ if(*blk_n < 7U) {
+ *blk_n += 1U;
+ } else { // a full pattern was passed
+ *blk_n = 0U;
+ *repeat >>= 1U;
+ }
+ return out;
+}
+
+uint8_t IndicationKernel( // Generate blink pattern, return 1 if all ind cleared
+ uint8_t*pattern,
+ uint16_t*repeat,
+ uint8_t*phase,
+ uint16_t*Post_ind,
+ uint16_t*Run_ind,
+ uint16_t*VDU_ind)
+{
+ //Blink indicator in pattern
+ //If FLT_Run = 0b0010-0110, pattern will be --......--...--..
+ uint8_t refresh = 0;
+ if(*repeat!=0) return refresh; // skip straight to Indication()
+
+ if(*Post_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = Post_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit
+ *Post_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ if(*Run_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = Run_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*Run_ind)&(-*Run_ind); // extract LSB bit
+ *Run_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ if(*VDU_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = VDU_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*VDU_ind)&(-*VDU_ind); // extract LSB bit
+ *VDU_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ // else all XXX_ind is cleared out, set refresh
+ refresh = 1U;
+ return refresh;
+}
void Cooler(void)
{
--- a/main.h Fri Dec 20 06:35:07 2019 +0000
+++ b/main.h Sat Dec 28 14:16:21 2019 +0000
@@ -4,7 +4,7 @@
#define HCG 0.204f //Height of CG
#define Mtot 320.0f //Total weight with driver
#define g0 9.81f //gravity
-#define EG 0.000f // <0: over steer, >0:under steer
+#define EG 0.000f //EG<0: over steer, EG>0:under steer
#define K_yaw 0.000f //Gain for yaw regulator
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define FL_HSB_ID 0xA0 // Rx, 100Hz
@@ -28,6 +28,7 @@
#define PSUOFL_VDUFLTCode 0x0002U //Pedal unit timeout after once online
#define IMUSTA_VDUFLTCode 0x0004U //IMU module abnormal
#define ShDVol_VDUFLTCode 0x0008U //Shutdown voltage abnormal
+#define DSM_VDUFLTCode 0x0010U //One or more slave report DSM_Fault
//CAN msg bank
char temp_msg[8] = {0,0,0,0,0,0,0,0};
@@ -76,10 +77,14 @@
uint16_t FR_FLT_Run = 0;
uint16_t RL_FLT_Run = 0;
uint16_t RR_FLT_Run = 0;
+uint16_t FLT_Run = 0;
+uint16_t FLT_Run_ind = 0; // for indication
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;
+uint16_t FLT_Post = 0;
+uint16_t FLT_Post_ind = 0; // for indication
float FL_Trq_fil3 = 0; // Internal Tcmd, 100Hz recieving
float FR_Trq_fil3 = 0;
float RL_Trq_fil3 = 0;
@@ -117,6 +122,7 @@
uint16_t AUX_4_u16 = 0x0;
float SDn_voltage = 0.0f;
float Brk_voltage = 0.0f;
+
//VDU states
typedef enum {
VDU_PowerOn = 0U,
@@ -124,8 +130,25 @@
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
+VDU_STATE_TYPE VDU_STAT = VDU_PowerOn; // VDU current state
+uint16_t VDU_FLT = 0; // VDU internal fault code
+uint16_t VDU_FLT_ind = 0; // A copy for indication
+
+//Indicator pattern generation
+uint8_t Ind_refresh = 0; // Flag to copy error bits for indicator
+uint8_t Pattern = 0U;
+uint16_t Repeat = 0U; // Bitwise repeative variable
+uint8_t Phase = 0U; // 0:Type ind, 1:Num ind, 2:Blank
+uint8_t Blk_n = 0U;
+//Category repetive code
+#define Post_rep 0b00000001;
+#define Run_rep 0b00000010;
+#define VDU_rep 0b00000100;
+//Blink Pattern
+#define F_Blink 0b01010101; // blink from LSB to MSB
+#define L_Pulse 0b01111110;
+#define S_Pulse 0b01000000;
+#define N_Pulse 0b00000000;
//Prototype
void CAN_init(void); //Initial CAN frequency filter...
@@ -148,3 +171,14 @@
void CANpendTX(void); //Helper function for CAN Tx
int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
float max_fval(float i1, float i2, float i3, float i4);
+uint8_t Indication( //Blink indicator in pattern * repeat
+ uint8_t pattern,
+ uint16_t*repeat,
+ uint8_t*blk_n);
+uint8_t IndicationKernel( //Generate blink pattern, return 1 if all done
+ uint8_t*pattern,
+ uint16_t*repeat,
+ uint8_t*phase,
+ uint16_t*Post_ind,
+ uint16_t*Run_ind,
+ uint16_t*VDU_ind);
\ No newline at end of file