diff --git a/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs b/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
index a9fae590ee58c0dd636a2911d2410af85d5a1e10..2e7cd898b892ce249c9415d8f2f418779504dfd8 100644
--- a/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
+++ b/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
@@ -17,6 +17,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
{
public class DefaultDriverStrategy : LoggingObject, IDriverStrategy
{
+ protected DrivingBehaviorEntry NextDrivingAction;
+
public enum DrivingMode
{
DrivingModeDrive,
@@ -40,27 +42,27 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
public IResponse Request(Second absTime, Meter ds, MeterPerSecond targetVelocity, Radian gradient)
{
- DrivingBehaviorEntry nextAction = null;
+ //DrivingBehaviorEntry nextAction = null;
switch (CurrentDrivingMode) {
case DrivingMode.DrivingModeDrive:
var currentDistance = Driver.DataBus.Distance;
- nextAction = GetNextDrivingAction(currentDistance);
- if (nextAction != null) {
- if (currentDistance.IsEqual(nextAction.ActionDistance,
+ UpdateDrivingAction(currentDistance);
+ if (NextDrivingAction != null) {
+ if (currentDistance.IsEqual(NextDrivingAction.ActionDistance,
Constants.SimulationSettings.DriverActionDistanceTolerance)) {
CurrentDrivingMode = DrivingMode.DrivingModeBrake;
DrivingModes[CurrentDrivingMode].ResetMode();
Log.Debug("Switching to DrivingMode BRAKE");
- BrakeTrigger = nextAction;
+ BrakeTrigger = NextDrivingAction;
break;
}
- if ((currentDistance + ds).IsGreater(nextAction.ActionDistance)) {
+ if ((currentDistance + ds).IsGreater(NextDrivingAction.ActionDistance)) {
Log.Debug("Current simulation interval exceeds next action distance at {0}. reducing maxDistance to {1}",
- nextAction.ActionDistance, nextAction.ActionDistance - currentDistance);
+ NextDrivingAction.ActionDistance, NextDrivingAction.ActionDistance - currentDistance);
return new ResponseDrivingCycleDistanceExceeded() {
Source = this,
- MaxDistance = nextAction.ActionDistance - currentDistance
+ MaxDistance = NextDrivingAction.ActionDistance - currentDistance
};
}
}
@@ -76,7 +78,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
var retVal = DrivingModes[CurrentDrivingMode].Request(absTime, ds, targetVelocity, gradient);
- if (nextAction == null || !(retVal is ResponseSuccess)) {
+ if (NextDrivingAction == null || !(retVal is ResponseSuccess)) {
return retVal;
}
@@ -89,32 +91,32 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
// if the speed at the end of the simulation interval is below the next target speed
// we are fine (no need to brake right now)
var v2 = Driver.DataBus.VehicleSpeed + retVal.Acceleration * retVal.SimulationInterval;
- if (v2 <= nextAction.NextTargetSpeed) {
+ if (v2 <= NextDrivingAction.NextTargetSpeed) {
return retVal;
}
Meter newds;
- switch (nextAction.Action) {
+ switch (NextDrivingAction.Action) {
case DrivingBehavior.Coasting:
- var coastingDistance = Formulas.DecelerationDistance(v2, nextAction.NextTargetSpeed,
+ var coastingDistance = Formulas.DecelerationDistance(v2, NextDrivingAction.NextTargetSpeed,
Driver.DriverData.LookAheadCoasting.Deceleration);
// if the distance at the end of the simulation interval is smaller than the new ActionDistance
// we are safe - go ahead...
- if ((Driver.DataBus.Distance + ds).IsSmallerOrEqual(nextAction.TriggerDistance - coastingDistance,
+ if ((Driver.DataBus.Distance + ds).IsSmallerOrEqual(NextDrivingAction.TriggerDistance - coastingDistance,
Constants.SimulationSettings.DriverActionDistanceTolerance)) {
return retVal;
}
- newds = EstimateAccelerationDistanceBeforeBrake(retVal, nextAction);
+ newds = EstimateAccelerationDistanceBeforeBrake(retVal, NextDrivingAction);
break;
case DrivingBehavior.Braking:
var brakingDistance = Driver.DriverData.AccelerationCurve.ComputeAccelerationDistance(v2,
- nextAction.NextTargetSpeed);
- if ((Driver.DataBus.Distance + ds).IsSmaller(nextAction.TriggerDistance - brakingDistance)) {
+ NextDrivingAction.NextTargetSpeed);
+ if ((Driver.DataBus.Distance + ds).IsSmaller(NextDrivingAction.TriggerDistance - brakingDistance)) {
return retVal;
}
- newds = (nextAction.TriggerDistance - brakingDistance) - Driver.DataBus.Distance;
+ newds = (NextDrivingAction.TriggerDistance - brakingDistance) - Driver.DataBus.Distance;
break;
default:
return retVal;
@@ -122,13 +124,59 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
if (newds.IsEqual(0, 1e-3) || ds.IsEqual(newds, 1e-3.SI<Meter>())) {
return retVal;
}
- Log.Debug("Exceeding next ActionDistance at {0}. Reducing max Distance to {1}", nextAction.ActionDistance, newds);
+ Log.Debug("Exceeding next ActionDistance at {0}. Reducing max Distance to {1}", NextDrivingAction.ActionDistance,
+ newds);
return new ResponseDrivingCycleDistanceExceeded() {
Source = this,
MaxDistance = newds,
};
}
+ private void UpdateDrivingAction(Meter currentDistance)
+ {
+ var nextAction = GetNextDrivingAction(currentDistance);
+ if (NextDrivingAction == null) {
+ if (nextAction != null) {
+ // take the new action
+ NextDrivingAction = nextAction;
+ }
+ } else {
+ // update action distance for current 'next action'
+ switch (NextDrivingAction.Action) {
+ case DrivingBehavior.Coasting:
+ var coastingDistance = Formulas.DecelerationDistance(Driver.DataBus.VehicleSpeed,
+ NextDrivingAction.NextTargetSpeed,
+ Driver.DriverData.LookAheadCoasting.Deceleration);
+ NextDrivingAction.ActionDistance = NextDrivingAction.TriggerDistance - coastingDistance;
+ break;
+ case DrivingBehavior.Braking:
+ var brakingDistance = Driver.ComputeDecelerationDistance(NextDrivingAction.NextTargetSpeed);
+ NextDrivingAction.ActionDistance = NextDrivingAction.TriggerDistance - brakingDistance;
+ break;
+ default:
+ throw new ArgumentOutOfRangeException();
+ }
+
+ if (nextAction != null) {
+ if (nextAction.HasEqualTrigger(NextDrivingAction)) {
+ // if the action changes and the vehicle has not yet exceeded the action distance => update the action
+ // otherwise do nothing, NextDrivingAction's action distance has already been updated
+ if (nextAction.Action != NextDrivingAction.Action && nextAction.ActionDistance > currentDistance) {
+ NextDrivingAction = nextAction;
+ }
+ } else {
+ // hmm, we've got a new action that is closer to what we got before?
+ if (nextAction.ActionDistance < NextDrivingAction.ActionDistance) {
+ NextDrivingAction = nextAction;
+ }
+ }
+ } else {
+ NextDrivingAction = null;
+ }
+ }
+ Log.Debug("Next Driving Action: {0}", NextDrivingAction);
+ }
+
public IResponse Request(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
{
@@ -178,8 +226,9 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
? entry.VehicleTargetSpeed + Driver.DriverData.OverSpeedEcoRoll.OverSpeed
: entry.VehicleTargetSpeed;
if (nextTargetSpeed < currentSpeed) {
+ // TODO @@@quam currentSpeed ? targetSpeed? nextTargetSpeed?
if (!Driver.DriverData.LookAheadCoasting.Enabled ||
- entry.VehicleTargetSpeed < Driver.DriverData.LookAheadCoasting.MinSpeed) {
+ currentSpeed < Driver.DriverData.LookAheadCoasting.MinSpeed) {
var brakingDistance = Driver.ComputeDecelerationDistance(nextTargetSpeed);
Log.Debug("adding 'Braking' starting at distance {0}. brakingDistance: {1}, triggerDistance: {2}",
entry.Distance - brakingDistance, brakingDistance, entry.Distance);
@@ -461,5 +510,16 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
public MeterPerSecond NextTargetSpeed;
public Meter TriggerDistance;
public Meter ActionDistance;
+
+ public bool HasEqualTrigger(DrivingBehaviorEntry other)
+ {
+ return TriggerDistance.IsEqual(other.TriggerDistance) && NextTargetSpeed.IsEqual(other.NextTargetSpeed);
+ }
+
+ public override string ToString()
+ {
+ return string.Format("action: {0} @ {1}. trigger: {2} targetSpeed: {3}", Action, ActionDistance, TriggerDistance,
+ NextTargetSpeed);
+ }
}
}
\ No newline at end of file
diff --git a/VectoCoreTest/Integration/DriverStrategy/DriverStrategyTestTruck.cs b/VectoCoreTest/Integration/DriverStrategy/DriverStrategyTestTruck.cs
index 60be166ca0e277e1f411d0b91cfcba926636c8c5..4f770352f4e1bbb7f2351d34732a1c575decf742 100644
--- a/VectoCoreTest/Integration/DriverStrategy/DriverStrategyTestTruck.cs
+++ b/VectoCoreTest/Integration/DriverStrategy/DriverStrategyTestTruck.cs
@@ -713,8 +713,8 @@ namespace TUGraz.VectoCore.Tests.Integration.DriverStrategy
var data = new string[] {
// <s>,<v>,<grad>,<stop>
" 0, 49.9, -5, 0",
- "100, 52, -5, 0",
- "200, 0, -5, 2",
+ "200, 52, -5, 0",
+ "300, 0, -5, 2",
};
var cycle = SimpleDrivingCycles.CreateCycleData(data);