|
@@ -12,7 +12,7 @@ GroupSpeedCtrl::GroupSpeedCtrl()
|
|
|
|
|
|
|
|
|
double GroupSpeedCtrl::Getmindec(iv::group::groupinfo & xgroupinfo,
|
|
|
- const std::vector<GPSData> & gpsMapLine,int PathPoint)
|
|
|
+ const std::vector<GPSData> & gpsMapLine,int PathPoint ,double fsecspeednow)
|
|
|
{
|
|
|
|
|
|
iv::group::vehicleinfo * pvehaffter = NULL; //a vehicle after current vehicle
|
|
@@ -91,6 +91,21 @@ double GroupSpeedCtrl::Getmindec(iv::group::groupinfo & xgroupinfo,
|
|
|
double dis_startdec = mfDis_StartDec_Next; //if > 50 start wait
|
|
|
double dis_maxdec = mfDis_MaxDec_Next; //if > 60 max dec
|
|
|
|
|
|
+ double fslowdis = fdistocur - dis_startdec;
|
|
|
+
|
|
|
+ if(fslowdis > 0)
|
|
|
+ {
|
|
|
+ double fafterspeed = pvehaffter->mgpsimu().speed();
|
|
|
+ if(fafterspeed > fsecspeednow)
|
|
|
+ {
|
|
|
+ if((pow((fafterspeed - fsecspeednow),2)/2.0) > fslowdis)
|
|
|
+ {
|
|
|
+ return 0.0; //If after vechicle speed is fast
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
if(fdistocur>=dis_maxdec)return decmax;
|
|
|
else
|
|
|
{
|