diff --git a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
index 71ecb2ab45217c6e6228472a78fdf0202f9d653d..f32fc8e0dca155930ffbd6b00cccec9b68f0a3bd 100644
--- a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
+++ b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
@@ -111,25 +111,24 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
return new ResponseCycleFinished();
}
- if ((PreviousState.Distance + ds).IsGreater(CycleIntervalIterator.RightSample.Distance)) {
- // only drive until next sample point in cycle
- Log.Debug("Limiting distance to next sample point {0}",
- CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
- return new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = CycleIntervalIterator.RightSample.Distance - PreviousState.Distance
- };
+ var nextSpeedChange = GetSpeedChangeWithinSimulationInterval(ds);
+ if (nextSpeedChange == null || ds.IsSmallerOrEqual(nextSpeedChange - PreviousState.Distance)) {
+ return DriveDistance(absTime, ds);
}
-
-
- return DriveDistance(absTime, ds);
+ // only drive until next sample point in cycle with speed change
+ Log.Debug("Limiting distance to next sample point {0}",
+ CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
+ return new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = nextSpeedChange - PreviousState.Distance
+ };
}
private IResponse DriveTimeInterval(Second absTime, Second dt)
{
CurrentState.AbsTime = PreviousState.AbsTime + dt;
CurrentState.WaitTime = PreviousState.WaitTime + dt;
- CurrentState.Gradient = ComputeGradient();
+ CurrentState.Gradient = ComputeGradient(0.SI<Meter>());
CurrentState.VehicleTargetSpeed = CycleIntervalIterator.LeftSample.VehicleTargetSpeed;
return NextComponent.Request(absTime, dt, CycleIntervalIterator.LeftSample.VehicleTargetSpeed, CurrentState.Gradient);
@@ -137,42 +136,62 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
private IResponse DriveDistance(Second absTime, Meter ds)
{
- if (!CurrentState.RequestToNextSamplePointDone &&
- (CycleIntervalIterator.RightSample.Distance - PreviousState.Distance) <
- Constants.SimulationSettings.BrakeNextTargetDistance) {
+ var nextSpeedChanges = LookAhead(Constants.SimulationSettings.BrakeNextTargetDistance);
+ if (nextSpeedChanges.Count > 0 && !CurrentState.RequestToNextSamplePointDone) {
CurrentState.RequestToNextSamplePointDone = true;
Log.Debug("current distance is close to the next speed change: {0}",
- CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
+ nextSpeedChanges.First().Distance - PreviousState.Distance);
return new ResponseDrivingCycleDistanceExceeded {
Source = this,
MaxDistance = Constants.SimulationSettings.BrakeNextTargetDistance
};
}
+
CurrentState.Distance = PreviousState.Distance + ds;
CurrentState.VehicleTargetSpeed = CycleIntervalIterator.LeftSample.VehicleTargetSpeed;
- CurrentState.Gradient = ComputeGradient();
+ CurrentState.Gradient = ComputeGradient(ds);
return NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
}
- private Radian ComputeGradient()
+ private Radian ComputeGradient(Meter ds)
{
var leftSamplePoint = CycleIntervalIterator.LeftSample;
- var rightSamplePoint = CycleIntervalIterator.RightSample;
- var gradient = leftSamplePoint.RoadGradient;
+ var cycleIterator = CycleIntervalIterator.Clone();
+ while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && !cycleIterator.LastEntry) {
+ cycleIterator.MoveNext();
+ }
+ var rightSamplePoint = cycleIterator.RightSample;
- if (!leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
- CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
- leftSamplePoint.Altitude, rightSamplePoint.Altitude, CurrentState.Distance);
+ var gradient = leftSamplePoint.RoadGradient;
- gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
- (CurrentState.Distance - PreviousState.Distance)).Value());
+ if (leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
+ return gradient;
}
+
+ CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
+ leftSamplePoint.Altitude, rightSamplePoint.Altitude, PreviousState.Distance + ds);
+
+ gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
+ (ds)).Value());
//return 0.SI<Radian>();
return gradient;
}
+ private Meter GetSpeedChangeWithinSimulationInterval(Meter ds)
+ {
+ var leftSamplePoint = CycleIntervalIterator.LeftSample;
+ var cycleIterator = CycleIntervalIterator.Clone();
+
+ do {
+ if (!leftSamplePoint.VehicleTargetSpeed.IsEqual(cycleIterator.RightSample.VehicleTargetSpeed)) {
+ return cycleIterator.RightSample.Distance;
+ }
+ } while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && cycleIterator.MoveNext());
+ return null;
+ }
+
IResponse ISimulationOutPort.Request(Second absTime, Second dt)
{
throw new NotImplementedException();
@@ -239,10 +258,17 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
}
// separately test for equality and greater than to have tolerance for equality comparison
- if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(0) &&
- CurrentState.Distance.IsGreaterOrEqual(CycleIntervalIterator.RightSample.Distance)) {
- // we have reached the end of the current interval in the cycle, move on...
- CycleIntervalIterator.MoveNext();
+ if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(0)) {
+ while (CurrentState.Distance.IsGreaterOrEqual(CycleIntervalIterator.RightSample.Distance) &&
+ !CycleIntervalIterator.LastEntry) {
+ // we have reached the end of the current interval in the cycle, move on...
+ CycleIntervalIterator.MoveNext();
+ }
+ } else {
+ if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(PreviousState.WaitTime)) {
+ // we needed to stop at the current interval in the cycle and have already waited enough time, move on..
+ CycleIntervalIterator.MoveNext();
+ }
}
}
@@ -262,6 +288,10 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
retVal.Add(cycleIterator.RightSample);
velocity = cycleIterator.RightSample.VehicleTargetSpeed;
} while (cycleIterator.MoveNext() && cycleIterator.RightSample.Distance < PreviousState.Distance + lookaheadDistance);
+ if (retVal.Count > 0) {
+ retVal = retVal.Where(x => x.Distance <= PreviousState.Distance + lookaheadDistance).ToList();
+ retVal.Sort((x, y) => x.Distance.CompareTo(y.Distance));
+ }
return retVal;
}
diff --git a/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs b/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
index adcb0c8ee4d107ee6880a45d3fc45b3a42532231..ea8c6d08faf9a8d89683a076383211b968551f79 100644
--- a/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
+++ b/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
@@ -6,6 +6,7 @@ using TUGraz.VectoCore.Models.Connector.Ports.Impl;
using TUGraz.VectoCore.Models.Simulation.Impl;
using TUGraz.VectoCore.Models.SimulationComponent.Data;
using TUGraz.VectoCore.Models.SimulationComponent.Impl;
+using TUGraz.VectoCore.Tests.Integration;
using TUGraz.VectoCore.Tests.Utils;
using TUGraz.VectoCore.Utils;
@@ -18,6 +19,96 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
public const double Tolerance = 0.0001;
+
+ [TestMethod]
+ public void TestLimitRequst()
+ {
+ var data = new string[] {
+ // <s>,<v>,<grad>,<stop>
+ " 0, 20, 0, 0",
+ " 1, 20, -0.1, 0",
+ " 2, 20, -0.3, 0",
+ " 10, 40, -0.3, 0",
+ " 20, 30, -0.1, 0"
+ };
+ var cycleData = SimpleDrivingCycles.CreateCycleData(data);
+ var container = new VehicleContainer();
+ var cycle = new DistanceBasedDrivingCycle(container, cycleData);
+
+ var gbx = new MockGearbox(container);
+
+ var driver = new MockDriver(container);
+ cycle.InPort().Connect(driver.OutPort());
+
+ cycle.OutPort().Initialize();
+
+ // just in test mock driver
+ driver.VehicleStopped = false;
+
+ var absTime = 0.SI<Second>();
+
+ // a request up to 10m succeeds, no speed change for the next 10m
+
+ var response = cycle.OutPort().Request(absTime, 0.3.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ response = cycle.OutPort().Request(absTime, 1.3.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ response = cycle.OutPort().Request(absTime, 2.7.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ response = cycle.OutPort().Request(absTime, 3.5.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ // a request with 12m exceeds the speed change at 10m -> maxDistance == 10m
+
+ response = cycle.OutPort().Request(absTime, 12.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+ Assert.AreEqual(10, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+ response = cycle.OutPort().Request(absTime, 10.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ // drive 10m
+ container.CommitSimulationStep(absTime, response.SimulationInterval);
+ absTime += response.SimulationInterval;
+
+ // - - - - - - - -
+ // request with 8m succeeds
+
+ response = cycle.OutPort().Request(absTime, 8.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+ container.CommitSimulationStep(absTime, response.SimulationInterval);
+ absTime += response.SimulationInterval;
+
+ // - - - - - - - -
+ // request with 3m more -> distance exceeded. maxDistance == 2m (approach next speed change, we are within 5m radius)
+
+ response = cycle.OutPort().Request(absTime, 3.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+ Assert.AreEqual(2, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+ // - - - - - - - -
+ // request with 1m (18 -> 19m) => response exceeded, drive up to next sample point (at least 5m)
+ response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
+ Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+ Assert.AreEqual(5, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+ // next request with 5m, as suggested => distance exceeded. maxDistance == 2m (next speed change)....
+ response = cycle.OutPort().Request(absTime, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance);
+ Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+ Assert.AreEqual(2, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+ // ok
+ response = cycle.OutPort().Request(absTime, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance);
+ Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+ }
+
[TestMethod]
public void TestDistanceRequest()
{
@@ -36,7 +127,7 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
// just in test mock driver
driver.VehicleStopped = false;
- var startDistance = cycleData.Entries.First().Distance.Value();
+ var startDistance = container.CycleStartDistance.Value();
var absTime = 0.SI<Second>();
// waiting time of 40 seconds is split up to 3 steps: 0.5, 39, 0.5
@@ -74,7 +165,7 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
container.CommitSimulationStep(absTime, response.SimulationInterval);
absTime += response.SimulationInterval;
-
+
response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
Assert.IsInstanceOfType(response, typeof(ResponseSuccess));