ATC Dimetronic: ATO init

src/Devices/AtcDimetronic.cs

@@ -53,6 +53,23 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>The current state of the track.</summary>
         private TrackStates TrackState;
 
+        /// <summary>Represents the state of ATO.</summary>
+        private enum AtoStates
+        {
+            Inactive,
+            AwaitingStartup,
+            IncreaseSpeed,
+            MaintainSpeed,
+            ReduceSpeed,
+            StopApproach
+        }
+
+        /// <summary>The current state of ATO.</summary>
+        private AtoStates AtoState;
+
+        /// <summary>Whether ATO knows the precise position of the train or not.</summary>
+        private bool AtoPositionKnown;
+
         /// <summary>The current position of the train.</summary>
         private double TrainLocation;
 
@@ -90,13 +107,13 @@ namespace OpenbveFcmbTrainPlugin
             internal Speed WarningOn { get; private set; }
             internal Speed WarningOff { get; private set; }
             internal Speed TargetLimit { get; private set; }
-            internal double TargetDistance { get; private set; }
+            internal double TargetPosition { get; private set; }
 
             internal SpeedCode(Speed limit, Speed target, double distance)
             {
                 CurrentLimit = limit;
                 TargetLimit = target;
-                TargetDistance = distance;
+                TargetPosition = distance;
                 if (limit == target)
                 {
                     // Constant speed
@@ -116,10 +133,10 @@ namespace OpenbveFcmbTrainPlugin
             }
 
             /// <summary>Updates the ATC speed code.</summary>
-            /// <param name="distance">The distance to the end of the signalling block.</param>
+            /// <param name="position">The position of the end of the signalling block.</param>
-            internal void Update(double distance)
+            internal void Update(double position)
             {
-                TargetDistance = distance;
+                TargetPosition = position;
             }
 
         }
@@ -227,13 +244,19 @@ namespace OpenbveFcmbTrainPlugin
             // Update train location
             TrainLocation = train.State.Location;
 
+            // If ATO is not the current mode, it should be in inactive state
+            if (DeviceState != DeviceStates.ATO)
+            {
+                AtoState = AtoStates.Inactive;
+            }
+
             double speed = train.State.Speed.KilometersPerHour;
             bool stopped = speed < 0.05;
 
             if (DeviceState == DeviceStates.YARD || DeviceState == DeviceStates.ATP || DeviceState == DeviceStates.ATO)
             {
                 // Speed limit enforcement
-                if (AtcControlState == AtcControlStates.Released && speed > CurrentSpeedCode.WarningOn.KilometersPerHour)
+                if (AtcControlState == AtcControlStates.Released && speed > CurrentSpeedCode.WarningOn.KilometersPerHour && DeviceState != DeviceStates.ATO)
                 {
                     // Cut power above warning on threshold and play alarm
                     AtcControlState = AtcControlStates.NoPowerAlarm;
@@ -278,6 +301,16 @@ namespace OpenbveFcmbTrainPlugin
                     // The train is rolling unintendedly, reset driving mode
                     DeviceState = DeviceStates.NoMode;
                 }
+
+                // Accidental door opening protection
+                if (train.DoorState != DoorStates.None)
+                {
+                    RequestedBrakeNotch = stopped ? train.Specs.BrakeNotches + 1 : train.ServiceBrakeNotch;
+                }
+                else
+                {
+                    RequestedBrakeNotch = -1;
+                }
             }
 
             // Brake application
@@ -320,28 +353,29 @@ namespace OpenbveFcmbTrainPlugin
                 case DeviceStates.YARD:
                     train.ContinuousProtection = false;
                     train.VigilanceOverride = false;
-                    // Apply brake if any door opens
-                    if (train.DoorState != DoorStates.None)
-                    {
-                        RequestedBrakeNotch = stopped ? train.Specs.BrakeNotches + 1 : train.ServiceBrakeNotch;
-                    }
-                    // If the train is not moving, brakes are released
-                    if (stopped)
-                    {
-                        AtcControlState = AtcControlStates.Released;
-                    }
                     break;
                 // ATC device is in ATP (M+ATP) driving mode
                 case DeviceStates.ATP:
                     train.ContinuousProtection = true;
                     train.VigilanceOverride = false;
                     CurrentSpeedCode = GetSpeedCodeFromAspect();
+                    // If reverser is not forward, unselect mode
+                    if (train.PhysicalHandles.Reverser != 1)
+                    {
+                        DeviceState = DeviceStates.NoMode;
+                    }
                     break;
                 // ATC device is in ATO driving mode
                 case DeviceStates.ATO:
                     train.ContinuousProtection = true;
                     train.VigilanceOverride = true;
                     CurrentSpeedCode = GetSpeedCodeFromAspect();
+                    ProcessAto(train, route);
+                    // If reverser is not forward or emergency brake is applied, unselect mode
+                    if (train.PhysicalHandles.Reverser != 1 || train.PhysicalHandles.BrakeNotch > train.Specs.BrakeNotches)
+                    {
+                        DeviceState = DeviceStates.NoMode;
+                    }
                     break;
             }
 
@@ -389,6 +423,7 @@ namespace OpenbveFcmbTrainPlugin
                                 if (train.PhysicalHandles.Reverser == 1 || train.PhysicalHandles.Reverser == -1)
                                 {
                                     DeviceState = DeviceStates.YARD;
+                                    AtcControlState = AtcControlStates.Released;
                                     // Update ATC speed code
                                     CurrentSpeedCode = new SpeedCode(YardMaximumSpeed, YardMaximumSpeed);
                                 }
@@ -416,7 +451,7 @@ namespace OpenbveFcmbTrainPlugin
                             // Allow change when the train is stopped in ATP mode
                             if (DeviceState == DeviceStates.ATP && stopped && AtoAvailable)
                             {
-                                if (train.PhysicalHandles.Reverser == 1 && TrackState > TrackStates.Unprotected)
+                                if (train.PhysicalHandles.Reverser == 1 && TrackState > TrackStates.Unprotected && train.PhysicalHandles.BrakeNotch <= train.Specs.BrakeNotches)
                                 {
                                     DeviceState = DeviceStates.ATO;
                                 }
@@ -510,6 +545,16 @@ namespace OpenbveFcmbTrainPlugin
             {
                 data.Handles.Reverser = 1;
                 data.Response = AIResponse.Short;
+                return;
+            }
+
+            // Acknowledge overspeed alarm
+            if (AtcControlState == AtcControlStates.NoPowerAlarm)
+            {
+                KeyChange(VirtualKeys.B1, true, train);
+                data.Response = AIResponse.Long;
+                KeyChange(VirtualKeys.B1, false, train);
+                return;
             }
 
             switch (DeviceState)
@@ -524,6 +569,7 @@ namespace OpenbveFcmbTrainPlugin
                         KeyChange(VirtualKeys.I, true, train);
                         data.Response = AIResponse.Short;
                         KeyChange(VirtualKeys.I, false, train);
+                        return;
                     }
                     break;
                 case DeviceStates.YARD:
@@ -537,7 +583,7 @@ namespace OpenbveFcmbTrainPlugin
                                 KeyChange(VirtualKeys.J, true, train);
                                 data.Response = AIResponse.Short;
                                 KeyChange(VirtualKeys.J, false, train);
-                                break;
+                                return;
                         }
                     }
                     CalculateAiNotches(data, train, route);
@@ -553,7 +599,7 @@ namespace OpenbveFcmbTrainPlugin
                                 KeyChange(VirtualKeys.I, true, train);
                                 data.Response = AIResponse.Short;
                                 KeyChange(VirtualKeys.I, false, train);
-                                break;
+                                return;
                         }
                     }
                     CalculateAiNotches(data, train, route);
@@ -626,5 +672,31 @@ namespace OpenbveFcmbTrainPlugin
             }
             return new SpeedCode(new Speed(0), new Speed(0));
         }
+
+        /// <summary>Processes ATO orders.</summary>
+        private void ProcessAto(Train train, Route route)
+        {
+            double speed = train.State.Speed.KilometersPerHour;
+            bool stopped = speed < 0.05;
+
+            Speed limit = CurrentSpeedCode.CurrentLimit;
+            Speed target = CurrentSpeedCode.TargetLimit;
+            double distance = CurrentSpeedCode.TargetPosition - TrainLocation;
+
+            switch (AtoState)
+            {
+                case AtoStates.Inactive:
+                    // Keep train still
+                    RequestedPowerNotch = 0;
+                    RequestedBrakeNotch = train.Specs.BrakeNotches + 1;
+
+                    // If the train is stopped, doors are closed and signal is clear, change to startup state
+                    if (stopped && train.DoorState == DoorStates.None && target.KilometersPerHour > 0)
+                    {
+                        AtoState = AtoStates.AwaitingStartup;
+                    }
+                    break;
+            }
+        }
     }
 }