hybrid controller: split calculation of cost and gearshift penalty for debugging purpose

allow emergencyshift if ICE is off so that EM speed stays with in range of ICE

VectoCommon/VectoCommon/Models/HybridStrategyResponse.cs

@@ -48,13 +48,15 @@ namespace TUGraz.VectoCommon.Models {
 		{
 			get
 			{
-				var cost = (FuelCosts + EquivalenceFactor * (BatCosts + ICEStartPenalty1) * SoCPenalty + ICEStartPenalty2 +
+				Cost = (FuelCosts + EquivalenceFactor * (BatCosts + ICEStartPenalty1) * SoCPenalty + ICEStartPenalty2 +
 				RampUpPenalty);
-				var gearshift = cost.IsSmaller(0) ? GearshiftPenalty : 1 / GearshiftPenalty;
-				return cost * gearshift;
+				var gearshift = Cost.IsSmaller(0) ? GearshiftPenalty : 1 / GearshiftPenalty;
+				return Cost * gearshift;
 			}
 		}
 
+		public double Cost { get; set; }
+
 		public double FuelCosts { get; set; }
 
 		public double BatCosts { get; set; }

VectoCore/VectoCore/Models/SimulationComponent/Impl/HybridController.cs

@@ -181,6 +181,17 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
 					continue;
 				}
 
+				var gear = DataBus.GearboxInfo.Gear;
+				var maxSpeed = VectoMath.Min(DataBus.GearboxInfo.GetGearData(gear.Gear).MaxSpeed,
+					DataBus.EngineInfo.EngineN95hSpeed);
+				if (!dryRun && retVal is ResponseSuccess && DataBus.GearboxInfo.GearEngaged(absTime) && retVal.Gearbox.InputSpeed.IsGreater(maxSpeed)) {
+					Strategy.AllowEmergencyShift = true;
+					retryCount++;
+					retry = true;
+					Strategy.OperatingpointChangedDuringRequest(absTime, dt, outTorque, outAngularVelocity, false, retVal);
+					continue;
+				}
+
 				if (!dryRun && strategySettings.CombustionEngineOn && retVal is ResponseEngineSpeedTooHigh && !strategySettings.ProhibitGearshift) {
 					retryCount++;
 					retry = true;