diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs index 25bfe10a4832b2cc99eed9ca2d0fec6db87e4d6b..0f9ad3c174e88057abbcafff70d48062b96e218e 100644 --- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs +++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs @@ -52,6 +52,15 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl { public class DefaultDriverStrategy : LoggingObject, IDriverStrategy { + public enum PCCStates + { + OutsideSegment, + WithinSegment, + UseCase1, + UseCase2, + PCCinterrupt + } + public static readonly SIBase<Meter> BrakingSafetyMargin = 0.1.SI<Meter>(); protected internal DrivingBehaviorEntry NextDrivingAction; @@ -71,6 +80,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl protected EcoRoll EcoRollState; protected PCCSegments PCCSegments; + protected internal PCCStates PCCState; public DefaultDriverStrategy(IVehicleContainer container = null) @@ -97,14 +107,12 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl PCCSegments = new PCCSegments(); if (data?.VehicleData.ADAS.PredictiveCruiseControl != PredictiveCruiseControlType.None) { - // create a dummy powertrain for pre-processing and estimatins var modData = new ModalDataContainer(data, null, new[] { FuelData.Diesel }, null, false); var builder = new PowertrainBuilder(modData); var testContainer = new SimplePowertrainContainer(data); builder.BuildSimplePowertrain(data, testContainer); - container?.AddPreprocessor(new PCCSegmentPreprocessor(testContainer, PCCSegments, data?.DriverData.PCC)); } } @@ -156,6 +164,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl val = -5; } container.SetDataValue("PCCSegment", val); + container.SetDataValue("PCCState", (int)PCCState); } } @@ -172,11 +181,12 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl { VehicleHaltTimestamp = null; - if (ADAS.EcoRoll != EcoRollType.None) { + if (ADAS.PredictiveCruiseControl != PredictiveCruiseControlType.None) { + HandlePCC(absTime, targetVelocity); + } + if ((ADAS.PredictiveCruiseControl != PredictiveCruiseControlType.None && (PCCState == PCCStates.OutsideSegment || PCCState == PCCStates.WithinSegment)) || + ADAS.EcoRoll != EcoRollType.None) { HandleEcoRoll(absTime, targetVelocity); - } else { - EngineOffTimestamp = null; - Driver.DataBus.IgnitionOn = true; } if (CurrentDrivingMode == DrivingMode.DrivingModeBrake) { @@ -230,6 +240,99 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl return retVal; } + private void HandlePCC(Second absTime, MeterPerSecond targetVelocity) + { + var dataBus = Driver.DataBus; + var distance = dataBus.Distance; + if (PCCSegments.Current.StartDistance < distance && PCCSegments.Current.EndDistance > distance) { + // within pcc-segment + PCCState = PCCStates.WithinSegment; + } else { + PCCState = PCCStates.OutsideSegment; + } + var vehicleSpeed = dataBus.VehicleSpeed; + if (PCCState == PCCStates.WithinSegment) { + var currentEnergy = CalculateEnergy(dataBus.Altitude, vehicleSpeed, dataBus.TotalMass); + + var end = PCCSegments.Current.EndDistance; + var endEnergy = PCCSegments.Current.EnergyEnd; + if ((distance + Driver.DriverData.PCC.PreviewDistance).IsSmallerOrEqual(end)) { + end = distance + Driver.DriverData.PCC.PreviewDistance; + var endCycleEntry = dataBus.CycleLookAhead(Driver.DriverData.PCC.PreviewDistance); + endEnergy = CalculateEnergy(endCycleEntry.Altitude, endCycleEntry.VehicleTargetSpeed, dataBus.TotalMass); + } + var lowEnergy = PCCSegments.Current.EnergyMinSpeed; + + var airDragForce = Driver.DataBus.AirDragResistance(vehicleSpeed, targetVelocity); + + //var rollResistanceForce = Driver.DataBus.RollingResistance( + // ((targetAltitude - vehicleAltitude) / (actionEntry.Distance - Driver.DataBus.Distance)) + // .Value().SI<Radian>()); + var rollResistanceForce = Driver.DataBus.RollingResistance(dataBus.RoadGradient); + var engineDragLoss = ADAS.EcoRoll == EcoRollType.None + ? Driver.DataBus.EngineDragPower(Driver.DataBus.EngineSpeed) + : 0.SI<Watt>(); + var gearboxLoss = Driver.DataBus.GearboxLoss(); + var axleLoss = Driver.DataBus.AxlegearLoss(); + + var coastingForce = airDragForce + rollResistanceForce + + (gearboxLoss + axleLoss - engineDragLoss) / vehicleSpeed; + + var energyCoastingLow = (coastingForce * (PCCSegments.Current.DistanceMinSpeed - distance)).Cast<Joule>(); + var energyCoastingEnd = (coastingForce * (end - distance)).Cast<Joule>(); + + var speedSufficient = vehicleSpeed.IsGreaterOrEqual(targetVelocity - Driver.DriverData.PCC.UnderSpeed); + var currentEnergyHigherThanMin = currentEnergy.IsGreaterOrEqual(lowEnergy + energyCoastingLow); + var currentEnergyHigherThanEnd = currentEnergy.IsGreaterOrEqual(endEnergy + energyCoastingEnd); + if (speedSufficient && currentEnergyHigherThanEnd && currentEnergyHigherThanMin) { + PCCState = PCCStates.UseCase1; + } + } + if (PCCState == PCCStates.UseCase1) { + if (vehicleSpeed <= targetVelocity - Driver.DriverData.PCC.UnderSpeed * 1.05) { + PCCState = PCCStates.PCCinterrupt; + } + if (vehicleSpeed >= targetVelocity + 1.KMPHtoMeterPerSecond()) { + PCCState = PCCStates.WithinSegment; + } + } + if (PCCState == PCCStates.PCCinterrupt) { + if (vehicleSpeed >= targetVelocity - Driver.DriverData.PCC.UnderSpeed * 0.95) { + PCCState = PCCStates.UseCase1; + } + } + + switch (PCCState) { + case PCCStates.UseCase1: + switch (ADAS.EcoRoll) { + case EcoRollType.None: break; + case EcoRollType.WithoutEngineStop: + (dataBus as IGearboxControl).DisengageGearbox = true; + break; + case EcoRollType.WithEngineStop: + (dataBus as IGearboxControl).DisengageGearbox = true; + dataBus.IgnitionOn = false; + break; + default: throw new ArgumentOutOfRangeException(); + } + + break; + case PCCStates.UseCase2: break; + case PCCStates.OutsideSegment: + case PCCStates.WithinSegment: + case PCCStates.PCCinterrupt: + (dataBus as IGearboxControl).DisengageGearbox = false; + dataBus.IgnitionOn = true; + break; + default: throw new ArgumentOutOfRangeException(); + } + } + + private Joule CalculateEnergy(Meter altitude, MeterPerSecond velocity, Kilogram mass) + { + return (mass * Physics.GravityAccelleration * altitude).Cast<Joule>() + mass * velocity * velocity / 2; + } + private void HandleEcoRoll(Second absTime, MeterPerSecond targetVelocity) { var dBus = Driver.DataBus; @@ -786,6 +889,9 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl Second absTime, Meter ds, MeterPerSecond targetVelocity, Radian gradient, bool prohibitOverspeed, MeterPerSecond velocityWithOverspeed, DebugData debug) { + if (DriverStrategy.PCCState == DefaultDriverStrategy.PCCStates.UseCase1) { + return Driver.DrivingActionCoast(absTime, ds, targetVelocity, gradient); + } IResponse first; if (DriverStrategy.OverspeedAllowed(targetVelocity, prohibitOverspeed) && DataBus.VehicleSpeed.IsEqual(targetVelocity)) {