diff --git a/VectoCore/Models/SimulationComponent/Data/DrivingCycleData.cs b/VectoCore/Models/SimulationComponent/Data/DrivingCycleData.cs
index a583c086361679e9460d7529c0642f65dd9af324..c333385350ee7e7aa31dfeb5a109451471cba0d7 100644
--- a/VectoCore/Models/SimulationComponent/Data/DrivingCycleData.cs
+++ b/VectoCore/Models/SimulationComponent/Data/DrivingCycleData.cs
@@ -54,16 +54,21 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Data
var distance = current.Distance;
var altitude = current.Altitude;
foreach (var entry in entries) {
- altitude += (entry.Distance - distance) * Math.Sin(entry.RoadGradient.Value());
- distance = entry.Distance;
+ entry.Altitude = altitude;
if (!CycleEntriesAreEqual(current, entry)) {
entry.Altitude = altitude;
filtered.Add(entry);
current = entry;
}
+ if (entry.StoppingTime.IsEqual(0) && !entry.VehicleTargetSpeed.IsEqual(0)) {
+ altitude += (entry.Distance - distance) * entry.RoadGradientPercent / 100.0;
+ }
+ distance = entry.Distance;
}
log.Info(string.Format("Data loaded. Number of Entries: {0}, filtered Entries: {1}", entries.Count, filtered.Count));
entries = filtered;
+
+ AdjustDistanceAfterStop(entries);
}
var cycle = new DrivingCycleData {
@@ -73,8 +78,25 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Data
return cycle;
}
+ private static void AdjustDistanceAfterStop(List<DrivingCycleEntry> entries)
+ {
+ var currentIt = entries.GetEnumerator();
+ var nextIt = entries.GetEnumerator();
+ nextIt.MoveNext();
+ while (currentIt.MoveNext() && nextIt.MoveNext()) {
+ if (currentIt.Current != null && !currentIt.Current.StoppingTime.IsEqual(0)) {
+ if (nextIt.Current != null) {
+ nextIt.Current.Distance = currentIt.Current.Distance;
+ }
+ }
+ }
+ }
+
private static bool CycleEntriesAreEqual(DrivingCycleEntry first, DrivingCycleEntry second)
{
+ if (first.Distance.IsEqual(second.Distance)) {
+ return true;
+ }
var retVal = first.VehicleTargetSpeed == second.VehicleTargetSpeed;
retVal = retVal &&
first.RoadGradient.IsEqual(second.RoadGradient, Constants.SimulationSettings.DrivingCycleRoadGradientTolerance);
diff --git a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
index 731627d4fbb2bb7fa4683e3ee4fcd43d9027ccd6..fda1bbc67d00b4d8fb80972d2ff01b77a9c906d4 100644
--- a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
+++ b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
@@ -61,26 +61,35 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
IResponse ISimulationOutPort.Request(TimeSpan absTime, Meter ds)
{
- var currentCycleEntry = Data.Entries[_currentState.CycleIndex];
- var nextCycleEntry = Data.Entries[_currentState.CycleIndex + 1];
+ var retVal = DoHandleRequest(absTime, ds);
+
+ //var success = retVal as ResponseSuccess;
+ //switch (retVal.ResponseType) {}
+ return retVal;
+ }
+
+ private IResponse DoHandleRequest(TimeSpan absTime, Meter ds)
+ {
+ var currentCycleEntry = Data.Entries[_previousState.CycleIndex];
+ var nextCycleEntry = Data.Entries[_previousState.CycleIndex + 1];
- IResponse retVal = null;
if (currentCycleEntry.Distance.IsEqual(_previousState.Distance.Value())) {
// exactly on an entry in the cycle...
- if (!currentCycleEntry.StoppingTime.IsEqual(0)) {
- // stop for certain time...
+ if (!currentCycleEntry.StoppingTime.IsEqual(0)
+ && currentCycleEntry.StoppingTime.Value() > _previousState.WaitTime.TotalSeconds) {
+ // stop for certain time unless we've already waited long enough...
if (!currentCycleEntry.VehicleTargetSpeed.IsEqual(0)) {
Log.WarnFormat("Stopping Time requested in cycle but target-velocity not zero. distance: {0}, target speed: {1}",
currentCycleEntry.StoppingTime, currentCycleEntry.VehicleTargetSpeed);
throw new VectoSimulationException("Stopping Time only allowed when target speed is zero!");
}
- retVal = _outPort.Request(absTime, TimeSpan.FromSeconds(currentCycleEntry.StoppingTime.Value()),
- currentCycleEntry.VehicleTargetSpeed, currentCycleEntry.RoadGradient);
- //retVal = OutPort().Request(absTime, TimeSpan.FromSeconds(currentCycleEntry.StoppingTime.Value()));
+ var dt = TimeSpan.FromSeconds(currentCycleEntry.StoppingTime.Value()) - _previousState.WaitTime;
+ return DriveTimeInterval(absTime, dt);
}
}
if (_previousState.Distance + ds > nextCycleEntry.Distance) {
+ // only drive until next sample point in cycle
// only drive until next sample point in cycle
return new ResponseDrivingCycleDistanceExceeded() {
MaxDistance = nextCycleEntry.Distance - _previousState.Distance
@@ -88,15 +97,26 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
}
- retVal = DriveDistance(absTime, ds);
- //throw new NotImplementedException("Distance based Cycle is not yet implemented.");
+ return DriveDistance(absTime, ds);
+ }
+
+ private IResponse DriveTimeInterval(TimeSpan absTime, TimeSpan dt)
+ {
+ _currentState.AbsTime += dt;
+ _currentState.WaitTime = _previousState.WaitTime + dt;
+
+ var currentCycleEntry = Data.Entries[_currentState.CycleIndex];
+
+ _currentState.CycleIndex++;
+
+ return _outPort.Request(absTime, dt,
+ currentCycleEntry.VehicleTargetSpeed, ComputeGradient());
}
private IResponse DriveDistance(TimeSpan absTime, Meter ds)
{
- _currentState = _previousState.Clone();
_currentState.Distance += ds;
- while (_currentState.Distance < Data.Entries[_currentState.CycleIndex + 1].Distance) {
+ while (_currentState.Distance >= Data.Entries[_currentState.CycleIndex + 1].Distance) {
_currentState.CycleIndex++;
}
@@ -107,11 +127,14 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
_currentState.VehicleTargetSpeed = Data.Entries[_currentState.CycleIndex].VehicleTargetSpeed;
- var gradient = ((_currentState.Altitude - _previousState.Altitude) /
- (_currentState.Distance - _previousState.Distance)).GradientPercent.Cast<Radian>();
-
+ return _outPort.Request(absTime, ds, _currentState.VehicleTargetSpeed, ComputeGradient());
+ }
- return _outPort.Request(absTime, ds, _currentState.VehicleTargetSpeed, gradient);
+ private Radian ComputeGradient()
+ {
+ var gradient = VectoMath.InclinationToAngle(((_currentState.Altitude - _previousState.Altitude) /
+ (_currentState.Distance - _previousState.Distance)).Value());
+ return gradient;
}
IResponse ISimulationOutPort.Request(TimeSpan absTime, TimeSpan dt)
@@ -124,10 +147,12 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
var first = Data.Entries.First();
_previousState = new DrivingCycleState() {
AbsTime = TimeSpan.FromSeconds(0),
+ WaitTime = TimeSpan.FromSeconds(0),
Distance = first.Distance,
Altitude = first.Altitude,
CycleIndex = 0,
};
+ _currentState = _previousState.Clone();
return new ResponseSuccess();
//TODO: return _outPort.Initialize();
}
@@ -160,7 +185,9 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
Distance = Distance,
VehicleTargetSpeed = VehicleTargetSpeed,
Altitude = Altitude,
- CycleIndex = CycleIndex
+ CycleIndex = CycleIndex,
+ // WaitTime is not cloned on purpose!
+ WaitTime = TimeSpan.FromSeconds(0)
};
}
@@ -168,6 +195,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
public Meter Distance;
+ public TimeSpan WaitTime;
+
public MeterPerSecond VehicleTargetSpeed;
public Meter Altitude;
diff --git a/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs b/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
index 9fbc718a8b409e087ea6ff05ea73020e99a40e56..fe53799cc28aa462766b8b0453174cbd54d2b588 100644
--- a/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
+++ b/VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs
@@ -30,12 +30,23 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
cycle.OutPort().Initialize();
- var response = cycle.OutPort().Request(TimeSpan.FromSeconds(0), 1.SI<Meter>());
+ var absTime = TimeSpan.FromSeconds(0);
+ var response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
Assert.AreEqual(0, driver.LastRequest.TargetVelocity.Value(), Tolerance);
Assert.AreEqual(0.028416069495827, driver.LastRequest.Gradient.Value(), 1E-12);
+ Assert.AreEqual(40, driver.LastRequest.dt.TotalSeconds, Tolerance);
+
+ vehicleContainer.CommitSimulationStep(absTime.TotalSeconds, response.SimulationInterval.TotalSeconds);
+ absTime += response.SimulationInterval;
+
+
+ response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
+
+ Assert.AreEqual(5.SI<MeterPerSecond>().Value(), driver.LastRequest.TargetVelocity.Value(), Tolerance);
+ Assert.AreEqual(0.02667562971628240, driver.LastRequest.Gradient.Value(), 1E-12);
}
}
}
\ No newline at end of file
diff --git a/VectoCoreTest/Utils/MockDriver.cs b/VectoCoreTest/Utils/MockDriver.cs
index a3ac2db29b7e2e6e14c60e7280921dc9479a8291..4077d31095938640bc67dccba0eab0bff7cba6a3 100644
--- a/VectoCoreTest/Utils/MockDriver.cs
+++ b/VectoCoreTest/Utils/MockDriver.cs
@@ -38,14 +38,15 @@ namespace TUGraz.VectoCore.Tests.Utils
{
LastRequest = new RequestData() { AbsTime = absTime, ds = ds, Gradient = gradient, TargetVelocity = targetVelocity };
var acc = 0.SI<MeterPerSquareSecond>();
- return new ResponseSuccess(); //_next.Request(absTime, TimeSpan.FromSeconds(0), acc, 0.SI<Radian>());
+ var dt = TimeSpan.FromSeconds(1);
+ return new ResponseSuccess() {SimulationInterval = dt}; //_next.Request(absTime, TimeSpan.FromSeconds(0), acc, 0.SI<Radian>());
}
public IResponse Request(TimeSpan absTime, TimeSpan dt, MeterPerSecond targetVelocity, Radian gradient)
{
LastRequest = new RequestData() { AbsTime = absTime, dt = dt, Gradient = gradient, TargetVelocity = targetVelocity };
var acc = 0.SI<MeterPerSquareSecond>();
- return new ResponseSuccess(); //_next.Request(absTime, dt, acc, gradient);
+ return new ResponseSuccess() {SimulationInterval = dt}; //_next.Request(absTime, dt, acc, gradient);
}
public void Connect(IDriverDemandOutPort other)
diff --git a/VectoCoreTest/Utils/SITest.cs b/VectoCoreTest/Utils/SITest.cs
index 48b3da99910d088e82bf9a14d10abcf4d0b9edf1..0d816f0fcff956079d097df0d16cfdb2fdb1e682 100644
--- a/VectoCoreTest/Utils/SITest.cs
+++ b/VectoCoreTest/Utils/SITest.cs
@@ -148,7 +148,7 @@ namespace TUGraz.VectoCore.Tests.Utils
//Assert.AreEqual(67.975.ToString("F3") + " [Percent]", percent.ToString("F3"));
//Assert.AreEqual(67.975, percent.Value(), 0.001);
- Assert.AreEqual(45.0 / 180.0 * Math.PI, 1.SI().GradientPercent.Cast<Radian>().Value(), 0.000001);
+ Assert.AreEqual(45.0 / 180.0 * Math.PI, VectoMath.InclinationToAngle(1).Value(), 0.000001);
}
[TestMethod]