YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1、修复了上班去传感器灵敏度低的问题

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-11-03 16:44:59 +08:00
parent 7dfff142c2
commit 89939cf1e1
3 changed files with 64 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+6 -1
View File
@@ -120,7 +120,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>JL2CM3</Key> <Key>JL2CM3</Key>
<Name>-U4294967295 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name> <Name>-U2147483647 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -TO18 -TC10000000 -TP21 -TDS8027 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@@ -215,6 +215,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText> <ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLowShieldVPT,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
Project/HC32F460/source/User/Inc/main.h
+2 -2
View File
@@ -12,12 +12,12 @@
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 10000 #define SHIELD_VPT 4000
#define EME_STOP_VPT 4000 #define EME_STOP_VPT 4000
#define STOP_TIME_MS 0 #define STOP_TIME_MS 0
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define EME_START_MS 1000 #define EME_START_MS 2000
#define EME_STOP_MS 2000 #define EME_STOP_MS 2000
#define SLOPE_START_MS 3000 #define SLOPE_START_MS 3000
Project/HC32F460/source/User/Src/main.c
+56 -59
View File
@@ -266,8 +266,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(abs(Oll_diff1) > abs(Orl_diff1)) if(abs(Oll_diff1) > abs(Orl_diff1))
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1)))
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
@@ -275,9 +274,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
if((Oll_diff1+Orl_diff1) > App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1)) if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
@@ -285,13 +283,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if(abs(Oll_diff1) < abs(Orl_diff1)) else if(abs(Oll_diff1) < abs(Orl_diff1))
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1))) if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
@@ -299,9 +296,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
if((Oll_diff1+Orl_diff1) > App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1)) if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
@@ -309,7 +305,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -323,16 +318,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0)) && (App.SlopeCollDelay1mSCnt <= 0))
|| ( || ((Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
(Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff3 >= App.ADTrack.LeftUpVPT*5/6)
&& (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)))) && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_UP(); BDC_UP();
@@ -341,7 +331,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -351,16 +340,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& ((App.SlopeCollDelay1mSCnt <= 0))) && ((App.SlopeCollDelay1mSCnt <= 0)))
|| ( || ((Oll_diff1 >= App.ADTrack.LeftLowVPT)
(Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT)
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT) && (Oll_diff1 >= App.ADTrack.LeftLowVPT)
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT))) && (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
{ {
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_DOWN(); BDC_DOWN();
@@ -369,7 +353,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = false; App.StopFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -383,15 +366,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0)) && (App.SlopeCollDelay1mSCnt <= 0))
|| ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6) || ((Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6)
&& (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)))) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))))
{ {
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
) )
{ {
BDC_UP(); BDC_UP();
@@ -400,7 +379,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -410,16 +388,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0)) && (App.SlopeCollDelay1mSCnt <= 0))
|| ((Oru_diff1 <= -App.ADTrack.RightUpVPT) || ((Orl_diff1 >= App.ADTrack.RightLowVPT)
&& (Oru_diff2 <= -App.ADTrack.RightUpVPT)
&& (Oru_diff3 <= -App.ADTrack.RightUpVPT)
&& (Orl_diff1 >= App.ADTrack.RightLowVPT)
&& (Orl_diff2 >= App.ADTrack.RightLowVPT) && (Orl_diff2 >= App.ADTrack.RightLowVPT)
&& (Orl_diff3 >= App.ADTrack.RightLowVPT)) && (Orl_diff3 >= App.ADTrack.RightLowVPT))
) )
{ {
if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
) )
{ {
BDC_DOWN(); BDC_DOWN();
@@ -428,7 +402,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -438,7 +411,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(App.LeftRunFalg == true) if(App.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -447,9 +420,14 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT)))) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))
))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -458,10 +436,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3) && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -472,7 +451,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
else if(App.RightRunFalg == true) else if(App.RightRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (rl_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (rl_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -481,9 +460,14 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT)))) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))
))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -492,10 +476,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT) else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT) && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -509,7 +494,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(App.LeftRunFalg == true) if(App.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -518,9 +503,14 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -App.ADTrack.Stop_VPT))) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))
))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -529,10 +519,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3) && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -543,7 +534,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
else if(App.RightRunFalg == true) else if(App.RightRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -552,9 +543,14 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT)))) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))
))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -563,10 +559,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT) else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT) && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();