diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
index 207f17c59015785251f2b258189b341a30a84534..dbae9245a3542a478016f308e9015468cad9c090 100644
--- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
+++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
@@ -29,525 +29,527 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using TUGraz.VectoCommon.Exceptions;
-using TUGraz.VectoCommon.Models;
-using TUGraz.VectoCommon.Utils;
-using TUGraz.VectoCore.Configuration;
-using TUGraz.VectoCore.Models.Connector.Ports;
-using TUGraz.VectoCore.Models.Connector.Ports.Impl;
-using TUGraz.VectoCore.Models.Simulation;
-using TUGraz.VectoCore.Models.Simulation.Data;
-using TUGraz.VectoCore.Models.SimulationComponent.Data;
-using TUGraz.VectoCore.OutputData;
-using TUGraz.VectoCore.Utils;
-
-namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
-{
- /// <summary>
- /// Class representing one Distance Based Driving Cycle
- /// </summary>
- public sealed class DistanceBasedDrivingCycle : StatefulProviderComponent
- <DistanceBasedDrivingCycle.DrivingCycleState, ISimulationOutPort, IDrivingCycleInPort, IDrivingCycleOutPort>,
- IDrivingCycle, ISimulationOutPort, IDrivingCycleInPort, IDisposable
- {
- private const double LookaheadTimeSafetyMargin = 1.5;
- internal readonly IDrivingCycleData Data;
- internal readonly DrivingCycleEnumerator CycleIntervalIterator;
- private bool _intervalProlonged;
- internal IdleControllerSwitcher IdleController;
-
- private DrivingCycleData.DrivingCycleEntry Left
- {
- get { return CycleIntervalIterator.LeftSample; }
- }
-
- private DrivingCycleData.DrivingCycleEntry Right
- {
- get { return CycleIntervalIterator.RightSample; }
- }
-
- public DistanceBasedDrivingCycle(IVehicleContainer container, IDrivingCycleData cycle) : base(container)
- {
- Data = cycle;
- CycleIntervalIterator = new DrivingCycleEnumerator(Data);
- CycleStartDistance = Data.Entries.Count > 0 ? Data.Entries.First().Distance : 0.SI<Meter>();
-
- var first = Data.Entries.First();
- PreviousState = new DrivingCycleState {
- AbsTime = 0.SI<Second>(),
- WaitTime = 0.SI<Second>(),
- Distance = first.Distance,
- Altitude = first.Altitude,
- };
- CurrentState = PreviousState.Clone();
- }
-
- public IResponse Initialize()
- {
- if (Left.VehicleTargetSpeed.IsEqual(0)) {
- var retVal = NextComponent.Initialize(DataBus.StartSpeed,
- Left.RoadGradient, DataBus.StartAcceleration);
- if (!(retVal is ResponseSuccess)) {
- throw new UnexpectedResponseException("DistanceBasedDrivingCycle.Initialize: Couldn't find start gear.", retVal);
- }
- }
-
- return NextComponent.Initialize(Left.VehicleTargetSpeed,
- Left.RoadGradient);
- }
-
- public IResponse Request(Second absTime, Second dt)
- {
- throw new NotImplementedException("Distance Based Driving Cycle does not support time requests.");
- }
-
- public IResponse Request(Second absTime, Meter ds)
- {
- if (Left.Distance.IsEqual(PreviousState.Distance.Value())) {
- // we are exactly on an entry in the cycle.
- var stopTime = Left.PTOActive && IdleController != null
- ? Left.StoppingTime + IdleController.Duration
- : Left.StoppingTime;
-
- if (stopTime.IsGreater(0) && PreviousState.WaitTime.IsSmaller(stopTime)) {
- // stop for certain time unless we've already waited long enough ...
-
- // we are stopping: ensure that velocity is 0.
- if (!Left.VehicleTargetSpeed.IsEqual(0)) {
- Log.Warn("Stopping Time requested in cycle but target-velocity not zero. distance: {0}, target speed: {1}",
- Left.StoppingTime, Left.VehicleTargetSpeed);
- throw new VectoSimulationException("Stopping Time only allowed when target speed is zero!");
- }
- var dt = GetStopTimeInterval();
- if (dt == null) {
- CurrentState.WaitPhase++;
- dt = GetStopTimeInterval();
- }
- CurrentState.Response = DriveTimeInterval(absTime, dt);
- return CurrentState.Response;
- }
- }
- if (CycleIntervalIterator.LastEntry && PreviousState.Distance.IsEqual(Right.Distance)) {
- CurrentState.Response = new ResponseCycleFinished();
- return CurrentState.Response;
- }
-
- var nextSpeedChange = GetSpeedChangeWithinSimulationInterval(ds);
- if (nextSpeedChange == null || ds.IsSmallerOrEqual(nextSpeedChange - PreviousState.Distance)) {
- if (nextSpeedChange == null || DataBus.VehicleSpeed.IsEqual(0.SI<MeterPerSecond>())) {
- CurrentState.Response = DriveDistance(absTime, ds);
- return CurrentState.Response;
- }
- var remainingDistance = nextSpeedChange - PreviousState.Distance - ds;
- var estimatedRemainingTime = remainingDistance / DataBus.VehicleSpeed;
- if (_intervalProlonged || remainingDistance.IsEqual(0.SI<Meter>()) ||
- estimatedRemainingTime.IsGreater(Constants.SimulationSettings.LowerBoundTimeInterval)) {
- CurrentState.Response = DriveDistance(absTime, ds);
- return CurrentState.Response;
- }
- Log.Debug("Extending distance by {0} to next sample point. ds: {1} new ds: {2}", remainingDistance, ds,
- nextSpeedChange - PreviousState.Distance);
- _intervalProlonged = true;
- CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = nextSpeedChange - PreviousState.Distance
- };
- return CurrentState.Response;
- }
- // only drive until next sample point in cycle with speed change
- Log.Debug("Limiting distance to next sample point {0}",
- Right.Distance - PreviousState.Distance);
- CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = nextSpeedChange - PreviousState.Distance
- };
- return CurrentState.Response;
- }
-
- private Second GetStopTimeInterval()
- {
- if (!Left.PTOActive || IdleController == null) {
- return Left.StoppingTime.IsGreater(3 * Constants.SimulationSettings.TargetTimeInterval)
- ? GetStopTimeIntervalThreePhases()
- : Left.StoppingTime;
- }
- if (Left.StoppingTime.IsGreater(6 * Constants.SimulationSettings.TargetTimeInterval)) {
- // 7 phases
- return GetStopTimeIntervalSevenPhasesPTO();
- }
- if (Left.StoppingTime.IsGreater(0)) {
- // 3 phases
- return GetStopTimeIntervalThreePhasesPTO();
- }
- // we have a pto cycle without stopping time.
- IdleController.ActivatePTO();
- return IdleController.GetNextCycleTime();
- }
-
- private Second GetStopTimeIntervalThreePhases()
- {
- switch (CurrentState.WaitPhase) {
- case 1:
- case 3:
- CurrentState.WaitPhase++;
- return Constants.SimulationSettings.TargetTimeInterval;
- case 2:
- CurrentState.WaitPhase++;
- return Left.StoppingTime - 2 * Constants.SimulationSettings.TargetTimeInterval;
- }
- return null;
- }
-
- private Second GetStopTimeIntervalThreePhasesPTO()
- {
- switch (CurrentState.WaitPhase) {
- case 1:
- case 3:
- CurrentState.WaitPhase++;
- IdleController.ActivateIdle();
- PTOActive = false;
- return Left.StoppingTime / 2;
- case 2:
- PTOActive = true;
- IdleController.ActivatePTO();
- return IdleController.GetNextCycleTime();
- }
- return null;
- }
-
- private Second GetStopTimeIntervalSevenPhasesPTO()
- {
- switch (CurrentState.WaitPhase) {
- case 1:
- case 5:
- CurrentState.WaitPhase++;
- IdleController.ActivateIdle();
- PTOActive = false;
- return Constants.SimulationSettings.TargetTimeInterval;
- case 3:
- case 7:
- CurrentState.WaitPhase++;
- return Constants.SimulationSettings.TargetTimeInterval;
- case 2:
- case 6:
- CurrentState.WaitPhase++;
- return Left.StoppingTime / 2 - 2 * Constants.SimulationSettings.TargetTimeInterval;
- case 4:
- PTOActive = true;
- IdleController.ActivatePTO();
- return IdleController.GetNextCycleTime();
- }
- return null;
- }
-
- private IResponse DriveTimeInterval(Second absTime, Second dt)
- {
- CurrentState.AbsTime = absTime;
- CurrentState.WaitTime = PreviousState.WaitTime + dt;
- CurrentState.Gradient = ComputeGradient(0.SI<Meter>());
- CurrentState.VehicleTargetSpeed = Left.VehicleTargetSpeed;
-
- return NextComponent.Request(absTime, dt, Left.VehicleTargetSpeed, CurrentState.Gradient);
- }
-
- private IResponse DriveDistance(Second absTime, Meter ds)
- {
- 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}",
- nextSpeedChanges.First().Distance - PreviousState.Distance);
- return new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = Constants.SimulationSettings.BrakeNextTargetDistance
- };
- }
-
- CurrentState.WaitPhase = 0;
- //CurrentState.Distance = PreviousState.Distance + ds;
- CurrentState.SimulationDistance = ds;
- CurrentState.VehicleTargetSpeed = Left.VehicleTargetSpeed;
- CurrentState.Gradient = ComputeGradient(ds);
-
- var retVal = NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
- retVal.Switch()
- .Case<ResponseFailTimeInterval>(
- r => {
- retVal = NextComponent.Request(absTime, r.DeltaT, 0.SI<MeterPerSecond>(), CurrentState.Gradient);
- retVal = NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
- });
- CurrentState.AbsTime = absTime;
- if (retVal is ResponseSuccess) {
- CurrentState.Distance = PreviousState.Distance + retVal.SimulationDistance;
- }
- return retVal;
- }
-
- protected override void DoWriteModalResults(IModalDataContainer container)
- {
- container[ModalResultField.dist] = CurrentState.Distance; // (CurrentState.Distance + PreviousState.Distance) / 2.0;
- container[ModalResultField.simulationDistance] = CurrentState.SimulationDistance;
- container[ModalResultField.v_targ] = CurrentState.VehicleTargetSpeed;
- container[ModalResultField.grad] = (Math.Tan(CurrentState.Gradient.Value()) * 100).SI<Scalar>();
- container[ModalResultField.altitude] = CurrentState.Altitude;
-
- if (IdleController != null) {
- IdleController.CommitSimulationStep(container);
- }
- }
-
- protected override void DoCommitSimulationStep()
- {
- if (!(CurrentState.Response is ResponseSuccess)) {
- throw new VectoSimulationException("Previous request did not succeed!");
- }
-
- PreviousState = CurrentState;
- CurrentState = CurrentState.Clone();
- _intervalProlonged = false;
-
- var stopTime = Left.PTOActive && IdleController != null
- ? Left.StoppingTime + IdleController.Duration
- : Left.StoppingTime;
-
- if (!stopTime.IsEqual(0) && stopTime.IsEqual(PreviousState.WaitTime)) {
- // we needed to stop at the current interval in the cycle and have already waited enough time, move on..
- if (IdleController != null) {
- IdleController.ActivateIdle();
- }
- CycleIntervalIterator.MoveNext();
- }
-
- stopTime = Left.PTOActive && IdleController != null ? Left.StoppingTime + IdleController.Duration : Left.StoppingTime;
-
- // separately test for equality and greater than to have tolerance for equality comparison
- if (stopTime.IsEqual(0)) {
- while (stopTime.IsEqual(0) && CurrentState.Distance.IsGreaterOrEqual(Right.Distance) &&
- !CycleIntervalIterator.LastEntry) {
- // we have reached the end of the current interval in the cycle, move on...
- CycleIntervalIterator.MoveNext();
-
- stopTime = Left.PTOActive && IdleController != null
- ? Left.StoppingTime + IdleController.Duration
- : Left.StoppingTime;
- }
- } else {
- if (stopTime.IsEqual(PreviousState.WaitTime)) {
- // we needed to stop at the current interval in the cycle and have already waited enough time, move on..
- if (IdleController != null) {
- IdleController.ActivateIdle();
- }
- CycleIntervalIterator.MoveNext();
- }
- }
- }
-
- private Radian ComputeGradient(Meter ds)
- {
- var cycleIterator = CycleIntervalIterator.Clone();
- while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && !cycleIterator.LastEntry) {
- cycleIterator.MoveNext();
- }
- var leftSamplePoint = cycleIterator.LeftSample;
- var rightSamplePoint = cycleIterator.RightSample;
-
- if (leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
- return leftSamplePoint.RoadGradient;
- }
- if (ds.IsEqual(0.SI<Meter>())) {
- return leftSamplePoint.RoadGradient;
- }
- CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
- leftSamplePoint.Altitude, rightSamplePoint.Altitude, PreviousState.Distance + ds);
-
- var gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
- ds).Value());
- //return 0.SI<Radian>();
- return gradient;
- }
-
- private Meter GetSpeedChangeWithinSimulationInterval(Meter ds)
- {
- var leftSamplePoint = Left;
- 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());
- if (cycleIterator.LastEntry) {
- return cycleIterator.RightSample.Distance;
- }
- return null;
- }
-
- /// <summary>
- /// Progress of the distance in the driving cycle.
- /// </summary>
- public double Progress
- {
- get {
- return Data.Entries.Count > 0
- ? (CurrentState.Distance.Value() - Data.Entries.First().Distance.Value()) /
- (Data.Entries.Last().Distance.Value() - Data.Entries.First().Distance.Value())
- : 0;
- }
- }
-
- public Meter CycleStartDistance { get; internal set; }
-
- public IReadOnlyList<DrivingCycleData.DrivingCycleEntry> LookAhead(Meter lookaheadDistance)
- {
- var retVal = new List<DrivingCycleData.DrivingCycleEntry>();
-
- var cycleIterator = CycleIntervalIterator.Clone();
- var velocity = cycleIterator.LeftSample.VehicleTargetSpeed;
-
- do {
- if (cycleIterator.RightSample.VehicleTargetSpeed.IsEqual(velocity)) {
- continue;
- }
- var lookaheadEntry = retVal.Find(x => x.Distance == cycleIterator.RightSample.Distance);
- if (lookaheadEntry != null) {
- // an entry may occur twice when vehicle stops (one entry with v=0 and the other with drive on after stop)
- // only use the one with min. speed
- if (cycleIterator.RightSample.VehicleTargetSpeed < lookaheadEntry.VehicleTargetSpeed) {
- retVal.Remove(lookaheadEntry);
- retVal.Add(cycleIterator.RightSample); // TODO: MQ 2016-05-13: use clone of iterator here?
- }
- } else {
- retVal.Add(cycleIterator.RightSample); // TODO: MQ 2016-05-13: use clone of iterator here?
- }
- velocity = cycleIterator.RightSample.VehicleTargetSpeed;
- if (velocity.IsEqual(0.KMPHtoMeterPerSecond())) {
- // do not look beyond vehicle stop
- break;
- }
- } 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;
- }
-
- public IReadOnlyList<DrivingCycleData.DrivingCycleEntry> LookAhead(Second time)
- {
- return LookAhead(LookaheadTimeSafetyMargin * DataBus.VehicleSpeed * time);
- }
-
- public void FinishSimulation()
- {
- Data.Finish();
- }
-
- public CycleData CycleData
- {
- get {
- return new CycleData {
- AbsTime = CurrentState.AbsTime,
- AbsDistance = CurrentState.Distance,
- LeftSample = Left,
- RightSample = CycleIntervalIterator.RightSample
- };
- }
- }
-
- public bool PTOActive { get; private set; }
-
- public DrivingCycleData.DrivingCycleEntry CycleLookAhead(Meter distance)
- {
- var absDistance = CurrentState.Distance + distance;
- var myIterator = CycleIntervalIterator.Clone();
-
- if (absDistance > Data.Entries.Last().Distance) {
- return ExtrapolateCycleEntry(absDistance, Data.Entries.Last());
- }
- while (myIterator.RightSample.Distance < absDistance) {
- myIterator.MoveNext();
- }
-
- return InterpolateCycleEntry(absDistance, myIterator.RightSample);
- }
-
- private DrivingCycleData.DrivingCycleEntry InterpolateCycleEntry(Meter absDistance,
- DrivingCycleData.DrivingCycleEntry lookahead)
- {
- var retVal = new DrivingCycleData.DrivingCycleEntry(lookahead) {
- Distance = absDistance,
- Altitude = VectoMath.Interpolate(CurrentState.Distance, lookahead.Distance, CurrentState.Altitude,
- lookahead.Altitude, absDistance)
- };
-
- retVal.RoadGradient =
- ((retVal.Altitude - CurrentState.Altitude) / (absDistance - CurrentState.Distance)).Value().SI<Radian>();
-
- return retVal;
- }
-
- private DrivingCycleData.DrivingCycleEntry ExtrapolateCycleEntry(Meter absDistance,
- DrivingCycleData.DrivingCycleEntry lookahead)
- {
- var retVal = new DrivingCycleData.DrivingCycleEntry(lookahead) {
- Distance = absDistance,
- Altitude = lookahead.Altitude + lookahead.RoadGradient * (absDistance - lookahead.Distance),
- };
-
- retVal.RoadGradient =
- ((retVal.Altitude - CurrentState.Altitude) / (absDistance - CurrentState.Distance)).Value().SI<Radian>();
-
- return retVal;
- }
-
- public Meter Altitude
- {
- get { return PreviousState.Altitude; }
- }
-
- public sealed class DrivingCycleState
- {
- public DrivingCycleState Clone()
- {
- return new DrivingCycleState {
- AbsTime = AbsTime,
- Distance = Distance,
- VehicleTargetSpeed = VehicleTargetSpeed,
- Altitude = Altitude,
- WaitPhase = WaitPhase,
- // WaitTime is not cloned on purpose!
- WaitTime = 0.SI<Second>(),
- Response = null
- };
- }
-
- public Second AbsTime;
-
- public Meter Distance;
-
- public Second WaitTime;
-
- public uint WaitPhase;
-
- public MeterPerSecond VehicleTargetSpeed;
-
- public Meter Altitude;
-
- public Radian Gradient;
-
- public IResponse Response;
-
- public bool RequestToNextSamplePointDone;
-
- public Meter SimulationDistance;
- }
-
- public void Dispose()
- {
- CycleIntervalIterator.Dispose();
- }
- }
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using TUGraz.VectoCommon.Exceptions;
+using TUGraz.VectoCommon.Models;
+using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Configuration;
+using TUGraz.VectoCore.Models.Connector.Ports;
+using TUGraz.VectoCore.Models.Connector.Ports.Impl;
+using TUGraz.VectoCore.Models.Simulation;
+using TUGraz.VectoCore.Models.Simulation.Data;
+using TUGraz.VectoCore.Models.SimulationComponent.Data;
+using TUGraz.VectoCore.OutputData;
+using TUGraz.VectoCore.Utils;
+
+namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
+{
+ /// <summary>
+ /// Class representing one Distance Based Driving Cycle
+ /// </summary>
+ public sealed class DistanceBasedDrivingCycle : StatefulProviderComponent
+ <DistanceBasedDrivingCycle.DrivingCycleState, ISimulationOutPort, IDrivingCycleInPort, IDrivingCycleOutPort>,
+ IDrivingCycle, ISimulationOutPort, IDrivingCycleInPort, IDisposable
+ {
+ private const double LookaheadTimeSafetyMargin = 1.5;
+ internal readonly IDrivingCycleData Data;
+ internal readonly DrivingCycleEnumerator CycleIntervalIterator;
+ private bool _intervalProlonged;
+ internal IdleControllerSwitcher IdleController;
+
+ private DrivingCycleData.DrivingCycleEntry Left
+ {
+ get { return CycleIntervalIterator.LeftSample; }
+ }
+
+ private DrivingCycleData.DrivingCycleEntry Right
+ {
+ get { return CycleIntervalIterator.RightSample; }
+ }
+
+ public DistanceBasedDrivingCycle(IVehicleContainer container, IDrivingCycleData cycle) : base(container)
+ {
+ Data = cycle;
+ CycleIntervalIterator = new DrivingCycleEnumerator(Data);
+ CycleStartDistance = Data.Entries.Count > 0 ? Data.Entries.First().Distance : 0.SI<Meter>();
+
+ var first = Data.Entries.First();
+ PreviousState = new DrivingCycleState {
+ AbsTime = 0.SI<Second>(),
+ WaitTime = 0.SI<Second>(),
+ Distance = first.Distance,
+ Altitude = first.Altitude,
+ };
+ CurrentState = PreviousState.Clone();
+ }
+
+ public IResponse Initialize()
+ {
+ if (Left.VehicleTargetSpeed.IsEqual(0)) {
+ var retVal = NextComponent.Initialize(DataBus.StartSpeed,
+ Left.RoadGradient, DataBus.StartAcceleration);
+ if (!(retVal is ResponseSuccess)) {
+ throw new UnexpectedResponseException("DistanceBasedDrivingCycle.Initialize: Couldn't find start gear.", retVal);
+ }
+ }
+
+ return NextComponent.Initialize(Left.VehicleTargetSpeed,
+ Left.RoadGradient);
+ }
+
+ public IResponse Request(Second absTime, Second dt)
+ {
+ throw new NotImplementedException("Distance Based Driving Cycle does not support time requests.");
+ }
+
+ public IResponse Request(Second absTime, Meter ds)
+ {
+ if (Left.Distance.IsEqual(PreviousState.Distance.Value())) {
+ // we are exactly on an entry in the cycle.
+ var stopTime = Left.PTOActive && IdleController != null
+ ? Left.StoppingTime + IdleController.Duration
+ : Left.StoppingTime;
+
+ if (stopTime.IsGreater(0) && PreviousState.WaitTime.IsSmaller(stopTime)) {
+ // stop for certain time unless we've already waited long enough ...
+
+ // we are stopping: ensure that velocity is 0.
+ if (!Left.VehicleTargetSpeed.IsEqual(0)) {
+ Log.Warn("Stopping Time requested in cycle but target-velocity not zero. distance: {0}, target speed: {1}",
+ Left.StoppingTime, Left.VehicleTargetSpeed);
+ throw new VectoSimulationException("Stopping Time only allowed when target speed is zero!");
+ }
+ var dt = GetStopTimeInterval();
+ if (dt == null) {
+ CurrentState.WaitPhase++;
+ dt = GetStopTimeInterval();
+ }
+ CurrentState.Response = DriveTimeInterval(absTime, dt);
+ return CurrentState.Response;
+ }
+ }
+ if (CycleIntervalIterator.LastEntry && PreviousState.Distance.IsEqual(Right.Distance)) {
+ CurrentState.Response = new ResponseCycleFinished();
+ return CurrentState.Response;
+ }
+
+ var nextSpeedChange = GetSpeedChangeWithinSimulationInterval(ds);
+ if (nextSpeedChange == null || ds.IsSmallerOrEqual(nextSpeedChange - PreviousState.Distance)) {
+ if (nextSpeedChange == null || DataBus.VehicleSpeed.IsEqual(0.SI<MeterPerSecond>())) {
+ CurrentState.Response = DriveDistance(absTime, ds);
+ return CurrentState.Response;
+ }
+ var remainingDistance = nextSpeedChange - PreviousState.Distance - ds;
+ var estimatedRemainingTime = remainingDistance / DataBus.VehicleSpeed;
+ if (_intervalProlonged || remainingDistance.IsEqual(0.SI<Meter>()) ||
+ estimatedRemainingTime.IsGreater(Constants.SimulationSettings.LowerBoundTimeInterval)) {
+ CurrentState.Response = DriveDistance(absTime, ds);
+ return CurrentState.Response;
+ }
+ Log.Debug("Extending distance by {0} to next sample point. ds: {1} new ds: {2}", remainingDistance, ds,
+ nextSpeedChange - PreviousState.Distance);
+ _intervalProlonged = true;
+ CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = nextSpeedChange - PreviousState.Distance
+ };
+ return CurrentState.Response;
+ }
+ // only drive until next sample point in cycle with speed change
+ Log.Debug("Limiting distance to next sample point {0}",
+ Right.Distance - PreviousState.Distance);
+ CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = nextSpeedChange - PreviousState.Distance
+ };
+ return CurrentState.Response;
+ }
+
+ private Second GetStopTimeInterval()
+ {
+ if (!Left.PTOActive || IdleController == null) {
+ return Left.StoppingTime.IsGreater(3 * Constants.SimulationSettings.TargetTimeInterval)
+ ? GetStopTimeIntervalThreePhases()
+ : Left.StoppingTime;
+ }
+ if (Left.StoppingTime.IsGreater(6 * Constants.SimulationSettings.TargetTimeInterval)) {
+ // 7 phases
+ return GetStopTimeIntervalSevenPhasesPTO();
+ }
+ if (Left.StoppingTime.IsGreater(0)) {
+ // 3 phases
+ return GetStopTimeIntervalThreePhasesPTO();
+ }
+ // we have a pto cycle without stopping time.
+ IdleController.ActivatePTO();
+ return IdleController.GetNextCycleTime();
+ }
+
+ private Second GetStopTimeIntervalThreePhases()
+ {
+ switch (CurrentState.WaitPhase) {
+ case 1:
+ case 3:
+ CurrentState.WaitPhase++;
+ return Constants.SimulationSettings.TargetTimeInterval;
+ case 2:
+ CurrentState.WaitPhase++;
+ return Left.StoppingTime - 2 * Constants.SimulationSettings.TargetTimeInterval;
+ }
+ return null;
+ }
+
+ private Second GetStopTimeIntervalThreePhasesPTO()
+ {
+ switch (CurrentState.WaitPhase) {
+ case 1:
+ case 3:
+ CurrentState.WaitPhase++;
+ IdleController.ActivateIdle();
+ PTOActive = false;
+ return Left.StoppingTime / 2;
+ case 2:
+ PTOActive = true;
+ IdleController.ActivatePTO();
+ return IdleController.GetNextCycleTime();
+ }
+ return null;
+ }
+
+ private Second GetStopTimeIntervalSevenPhasesPTO()
+ {
+ switch (CurrentState.WaitPhase) {
+ case 1:
+ case 5:
+ CurrentState.WaitPhase++;
+ IdleController.ActivateIdle();
+ PTOActive = false;
+ return Constants.SimulationSettings.TargetTimeInterval;
+ case 3:
+ case 7:
+ CurrentState.WaitPhase++;
+ return Constants.SimulationSettings.TargetTimeInterval;
+ case 2:
+ case 6:
+ CurrentState.WaitPhase++;
+ return Left.StoppingTime / 2 - 2 * Constants.SimulationSettings.TargetTimeInterval;
+ case 4:
+ PTOActive = true;
+ IdleController.ActivatePTO();
+ return IdleController.GetNextCycleTime();
+ }
+ return null;
+ }
+
+ private IResponse DriveTimeInterval(Second absTime, Second dt)
+ {
+ CurrentState.AbsTime = absTime;
+ CurrentState.WaitTime = PreviousState.WaitTime + dt;
+ CurrentState.Gradient = ComputeGradient(0.SI<Meter>());
+ CurrentState.VehicleTargetSpeed = Left.VehicleTargetSpeed;
+
+ return NextComponent.Request(absTime, dt, Left.VehicleTargetSpeed, CurrentState.Gradient);
+ }
+
+ private IResponse DriveDistance(Second absTime, Meter ds)
+ {
+ 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}",
+ nextSpeedChanges.First().Distance - PreviousState.Distance);
+ return new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = Constants.SimulationSettings.BrakeNextTargetDistance
+ };
+ }
+
+ CurrentState.WaitPhase = 0;
+ //CurrentState.Distance = PreviousState.Distance + ds;
+ CurrentState.SimulationDistance = ds;
+ CurrentState.VehicleTargetSpeed = Left.VehicleTargetSpeed;
+ CurrentState.Gradient = ComputeGradient(ds);
+
+ var retVal = NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
+ retVal.Switch()
+ .Case<ResponseFailTimeInterval>(
+ r => {
+ retVal = NextComponent.Request(absTime, r.DeltaT, 0.SI<MeterPerSecond>(), CurrentState.Gradient);
+ retVal = NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
+ });
+ CurrentState.AbsTime = absTime;
+ if (retVal is ResponseSuccess) {
+ CurrentState.Distance = PreviousState.Distance + retVal.SimulationDistance;
+ }
+ return retVal;
+ }
+
+ protected override void DoWriteModalResults(IModalDataContainer container)
+ {
+ container[ModalResultField.dist] = CurrentState.Distance; // (CurrentState.Distance + PreviousState.Distance) / 2.0;
+ container[ModalResultField.simulationDistance] = CurrentState.SimulationDistance;
+ container[ModalResultField.v_targ] = CurrentState.VehicleTargetSpeed;
+ container[ModalResultField.grad] = (Math.Tan(CurrentState.Gradient.Value()) * 100).SI<Scalar>();
+ container[ModalResultField.altitude] = CurrentState.Altitude;
+
+ if (IdleController != null) {
+ IdleController.CommitSimulationStep(container);
+ }
+ }
+
+ protected override void DoCommitSimulationStep()
+ {
+ if (!(CurrentState.Response is ResponseSuccess)) {
+ throw new VectoSimulationException("Previous request did not succeed!");
+ }
+
+ PreviousState = CurrentState;
+ CurrentState = CurrentState.Clone();
+ _intervalProlonged = false;
+
+ //var stopTime = Left.PTOActive && IdleController != null
+ // ? Left.StoppingTime + IdleController.Duration
+ // : Left.StoppingTime;
+
+ //if (!stopTime.IsEqual(0) && stopTime.IsEqual(PreviousState.WaitTime)) {
+ // // we needed to stop at the current interval in the cycle and have already waited enough time, move on..
+ // if (IdleController != null) {
+ // IdleController.ActivateIdle();
+ // }
+ // CycleIntervalIterator.MoveNext();
+ //}
+
+ var stopTime = Left.PTOActive && IdleController != null
+ ? Left.StoppingTime + IdleController.Duration
+ : Left.StoppingTime;
+
+ // separately test for equality and greater than to have tolerance for equality comparison
+ if (stopTime.IsEqual(0)) {
+ while (stopTime.IsEqual(0) && CurrentState.Distance.IsGreaterOrEqual(Right.Distance) &&
+ !CycleIntervalIterator.LastEntry) {
+ // we have reached the end of the current interval in the cycle, move on...
+ CycleIntervalIterator.MoveNext();
+
+ stopTime = Left.PTOActive && IdleController != null
+ ? Left.StoppingTime + IdleController.Duration
+ : Left.StoppingTime;
+ }
+ } else {
+ if (stopTime.IsEqual(PreviousState.WaitTime)) {
+ // we needed to stop at the current interval in the cycle and have already waited enough time, move on..
+ if (IdleController != null) {
+ IdleController.ActivateIdle();
+ }
+ CycleIntervalIterator.MoveNext();
+ }
+ }
+ }
+
+ private Radian ComputeGradient(Meter ds)
+ {
+ var cycleIterator = CycleIntervalIterator.Clone();
+ while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && !cycleIterator.LastEntry) {
+ cycleIterator.MoveNext();
+ }
+ var leftSamplePoint = cycleIterator.LeftSample;
+ var rightSamplePoint = cycleIterator.RightSample;
+
+ if (leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
+ return leftSamplePoint.RoadGradient;
+ }
+ if (ds.IsEqual(0.SI<Meter>())) {
+ return leftSamplePoint.RoadGradient;
+ }
+ CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
+ leftSamplePoint.Altitude, rightSamplePoint.Altitude, PreviousState.Distance + ds);
+
+ var gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
+ ds).Value());
+ //return 0.SI<Radian>();
+ return gradient;
+ }
+
+ private Meter GetSpeedChangeWithinSimulationInterval(Meter ds)
+ {
+ var leftSamplePoint = Left;
+ 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());
+ if (cycleIterator.LastEntry) {
+ return cycleIterator.RightSample.Distance;
+ }
+ return null;
+ }
+
+ /// <summary>
+ /// Progress of the distance in the driving cycle.
+ /// </summary>
+ public double Progress
+ {
+ get {
+ return Data.Entries.Count > 0
+ ? (CurrentState.Distance.Value() - Data.Entries.First().Distance.Value()) /
+ (Data.Entries.Last().Distance.Value() - Data.Entries.First().Distance.Value())
+ : 0;
+ }
+ }
+
+ public Meter CycleStartDistance { get; internal set; }
+
+ public IReadOnlyList<DrivingCycleData.DrivingCycleEntry> LookAhead(Meter lookaheadDistance)
+ {
+ var retVal = new List<DrivingCycleData.DrivingCycleEntry>();
+
+ var cycleIterator = CycleIntervalIterator.Clone();
+ var velocity = cycleIterator.LeftSample.VehicleTargetSpeed;
+
+ do {
+ if (cycleIterator.RightSample.VehicleTargetSpeed.IsEqual(velocity)) {
+ continue;
+ }
+ var lookaheadEntry = retVal.Find(x => x.Distance == cycleIterator.RightSample.Distance);
+ if (lookaheadEntry != null) {
+ // an entry may occur twice when vehicle stops (one entry with v=0 and the other with drive on after stop)
+ // only use the one with min. speed
+ if (cycleIterator.RightSample.VehicleTargetSpeed < lookaheadEntry.VehicleTargetSpeed) {
+ retVal.Remove(lookaheadEntry);
+ retVal.Add(cycleIterator.RightSample); // TODO: MQ 2016-05-13: use clone of iterator here?
+ }
+ } else {
+ retVal.Add(cycleIterator.RightSample); // TODO: MQ 2016-05-13: use clone of iterator here?
+ }
+ velocity = cycleIterator.RightSample.VehicleTargetSpeed;
+ if (velocity.IsEqual(0.KMPHtoMeterPerSecond())) {
+ // do not look beyond vehicle stop
+ break;
+ }
+ } 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;
+ }
+
+ public IReadOnlyList<DrivingCycleData.DrivingCycleEntry> LookAhead(Second time)
+ {
+ return LookAhead(LookaheadTimeSafetyMargin * DataBus.VehicleSpeed * time);
+ }
+
+ public void FinishSimulation()
+ {
+ Data.Finish();
+ }
+
+ public CycleData CycleData
+ {
+ get {
+ return new CycleData {
+ AbsTime = CurrentState.AbsTime,
+ AbsDistance = CurrentState.Distance,
+ LeftSample = Left,
+ RightSample = CycleIntervalIterator.RightSample
+ };
+ }
+ }
+
+ public bool PTOActive { get; private set; }
+
+ public DrivingCycleData.DrivingCycleEntry CycleLookAhead(Meter distance)
+ {
+ var absDistance = CurrentState.Distance + distance;
+ var myIterator = CycleIntervalIterator.Clone();
+
+ if (absDistance > Data.Entries.Last().Distance) {
+ return ExtrapolateCycleEntry(absDistance, Data.Entries.Last());
+ }
+ while (myIterator.RightSample.Distance < absDistance) {
+ myIterator.MoveNext();
+ }
+
+ return InterpolateCycleEntry(absDistance, myIterator.RightSample);
+ }
+
+ private DrivingCycleData.DrivingCycleEntry InterpolateCycleEntry(Meter absDistance,
+ DrivingCycleData.DrivingCycleEntry lookahead)
+ {
+ var retVal = new DrivingCycleData.DrivingCycleEntry(lookahead) {
+ Distance = absDistance,
+ Altitude = VectoMath.Interpolate(CurrentState.Distance, lookahead.Distance, CurrentState.Altitude,
+ lookahead.Altitude, absDistance)
+ };
+
+ retVal.RoadGradient =
+ ((retVal.Altitude - CurrentState.Altitude) / (absDistance - CurrentState.Distance)).Value().SI<Radian>();
+
+ return retVal;
+ }
+
+ private DrivingCycleData.DrivingCycleEntry ExtrapolateCycleEntry(Meter absDistance,
+ DrivingCycleData.DrivingCycleEntry lookahead)
+ {
+ var retVal = new DrivingCycleData.DrivingCycleEntry(lookahead) {
+ Distance = absDistance,
+ Altitude = lookahead.Altitude + lookahead.RoadGradient * (absDistance - lookahead.Distance),
+ };
+
+ retVal.RoadGradient =
+ ((retVal.Altitude - CurrentState.Altitude) / (absDistance - CurrentState.Distance)).Value().SI<Radian>();
+
+ return retVal;
+ }
+
+ public Meter Altitude
+ {
+ get { return PreviousState.Altitude; }
+ }
+
+ public sealed class DrivingCycleState
+ {
+ public DrivingCycleState Clone()
+ {
+ return new DrivingCycleState {
+ AbsTime = AbsTime,
+ Distance = Distance,
+ VehicleTargetSpeed = VehicleTargetSpeed,
+ Altitude = Altitude,
+ WaitPhase = WaitPhase,
+ // WaitTime is not cloned on purpose!
+ WaitTime = 0.SI<Second>(),
+ Response = null
+ };
+ }
+
+ public Second AbsTime;
+
+ public Meter Distance;
+
+ public Second WaitTime;
+
+ public uint WaitPhase;
+
+ public MeterPerSecond VehicleTargetSpeed;
+
+ public Meter Altitude;
+
+ public Radian Gradient;
+
+ public IResponse Response;
+
+ public bool RequestToNextSamplePointDone;
+
+ public Meter SimulationDistance;
+ }
+
+ public void Dispose()
+ {
+ CycleIntervalIterator.Dispose();
+ }
+ }
}
\ No newline at end of file