ATC Dimetronic: Implement basic ATO braking

src/Devices/AtcDimetronic.cs

@@ -86,6 +86,9 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>The delay before ATO changes notches.</summary>
         private readonly double AtoNotchDelay = 250;
 
+        /// <summary>The deceleration considered by ATO to calculate braking curves, in m/s2.</summary>
+        private readonly double AtoDeceleration = 0.33;
+
         /// <summary>The counter for the ATO notch delay.</summary>
         private double AtoNotchDelayCounter;
 
@@ -107,6 +110,9 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>The maximum speed in YARD mode.</summary>
         private readonly Speed YardMaximumSpeed;
 
+        /// <summary>The maximum speed when ATO mode is first initialized.</summary>
+        private readonly Speed AtoInitSpeed = new Speed(25 / 3.6);
+
         /// <summary>The distance after which the runback protection is triggered.</summary>
         private readonly double RunbackDistance;
 
@@ -418,7 +424,6 @@ namespace OpenbveFcmbTrainPlugin
             }
         }
 
-
         /// <summary>Is called when the state of a key changes.</summary>
         /// <param name="key">The key.</param>
         /// <param name="pressed">Whether the key is pressed or released.</param>
@@ -545,14 +550,17 @@ namespace OpenbveFcmbTrainPlugin
             switch (type)
             {
                 case -16777215:
+                    // ATC track state
+                    if (!(beacon.Optional >= 0 & beacon.Optional <= 3))
                     {
-                        if (!(beacon.Optional >= 0 & beacon.Optional <= 3))
+                        break;
-                        {
-                            break;
-                        }
-                        TrackState = (TrackStates)beacon.Optional;
-                        return;
                     }
+                    TrackState = (TrackStates)beacon.Optional;
+                    return;
+                case 36900:
+                    // ATC Dimetronic ATO beacon
+                    AtoPositionKnown = true;
+                    return;
             }
         }
 
@@ -704,10 +712,18 @@ namespace OpenbveFcmbTrainPlugin
             double speed = train.State.Speed.KilometersPerHour;
             bool stopped = speed < 0.05;
 
-            Speed limit = CurrentSpeedCode.CurrentLimit;
+            // Set limits from speed code, unless position is unknown
-            Speed target = CurrentSpeedCode.TargetLimit;
+            Speed limit = AtoPositionKnown ? CurrentSpeedCode.CurrentLimit : AtoInitSpeed;
+            Speed target = AtoPositionKnown ? CurrentSpeedCode.TargetLimit : AtoInitSpeed;
             double distance = CurrentSpeedCode.TargetPosition - TrainLocation;
 
+            // Calculate required deceleration
+            double requiredDeceleration = 0;
+            if (target.KilometersPerHour < limit.KilometersPerHour)
+            {
+                requiredDeceleration = (train.State.Speed.MetersPerSecond - target.MetersPerSecond) / (distance - 8) * 10;
+            }
+
             // Pitch multiplier, used to alter when state changes occur
             double pitchMulti = train.State.Pitch / 20;
 
@@ -724,7 +740,7 @@ namespace OpenbveFcmbTrainPlugin
                     AtoStartupPosition = TrainLocation;
 
                     // 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)
+                    if (stopped && train.DoorState == DoorStates.None && limit.KilometersPerHour > 0)
                     {
                         AtoState = AtoStates.AwaitingStartup;
                     }
@@ -749,10 +765,16 @@ namespace OpenbveFcmbTrainPlugin
                         }
                         AtoNotchDelayCounter = 0;
                     }
-                    if (train.State.Speed.KilometersPerHour >= target.KilometersPerHour - 5 + pitchMulti)
+                    if (train.State.Speed.KilometersPerHour >= limit.KilometersPerHour - 5 + pitchMulti)
                     {
+                        // Train is near the limit, maintain speed
                         AtoState = AtoStates.MaintainSpeed;
                     }
+                    if (requiredDeceleration >= AtoDeceleration)
+                    {
+                        // Train is near a speed change, reduce speed
+                        AtoState = AtoStates.ReduceSpeed;
+                    }
                     break;
                 case AtoStates.MaintainSpeed:
                     if ((train.Acceleration > 0 || (AtoPowerNotch > 0 && train.State.Pitch <= 0)) && AtoNotchDelayCounter >= AtoNotchDelay)
@@ -769,10 +791,45 @@ namespace OpenbveFcmbTrainPlugin
                             AtoNotchDelayCounter = 0;
                         }
                     }
-                    if (train.State.Speed.KilometersPerHour <= target.KilometersPerHour - 7 + pitchMulti)
+                    if (train.State.Speed.KilometersPerHour <= limit.KilometersPerHour - 7 + pitchMulti && target.KilometersPerHour >= limit.KilometersPerHour)
                     {
+                        // Train is far below the limit, increase speed
                         AtoState = AtoStates.IncreaseSpeed;
                     }
+                    if (requiredDeceleration >= AtoDeceleration)
+                    {
+                        // Train is near a speed change, reduce speed
+                        AtoState = AtoStates.ReduceSpeed;
+                    }
+                    break;
+                case AtoStates.ReduceSpeed:
+                    if (train.Acceleration > -requiredDeceleration && AtoNotchDelayCounter >= AtoNotchDelay)
+                    {
+                        // Train is travelling too fast, reduce speed
+                        if (AtoPowerNotch > 0)
+                        {
+                            AtoPowerNotch--;
+                            AtoNotchDelayCounter = 0;
+                        }
+                        else if (AtoBrakeNotch < train.Specs.BrakeNotches)
+                        {
+                            AtoBrakeNotch++;
+                            AtoNotchDelayCounter = 0;
+                        }
+                    }
+                    else if (train.Acceleration < -requiredDeceleration && AtoNotchDelayCounter >= AtoNotchDelay)
+                    {
+                        // Reducing speed too fast, release brakes
+                        if (AtoBrakeNotch > 0)
+                        {
+                            AtoBrakeNotch--;
+                            AtoNotchDelayCounter = 0;
+                        }
+                    }
+                    if (train.State.Speed.KilometersPerHour <= target.KilometersPerHour - 3 + pitchMulti)
+                    {
+                        AtoState = AtoStates.MaintainSpeed;
+                    }
                     break;
             }
 
@@ -783,8 +840,8 @@ namespace OpenbveFcmbTrainPlugin
             // ATO is driving but does not know the position of the train, unselect driving mode and set alarm state
             if (TrainLocation - AtoStartupPosition > 400 && !AtoPositionKnown)
             {
-                //AtoState = AtoStates.Alarm;
+                AtoState = AtoStates.Alarm;
-                //DeviceState = DeviceStates.NoMode;
+                DeviceState = DeviceStates.NoMode;
             }
         }
     }

src/Train/Train.cs

@@ -143,7 +143,6 @@ namespace OpenbveFcmbTrainPlugin
                 AccelerationTimer = 0;
                 AccelerationSpeed = data.Vehicle.Speed;
             }
-            data.DebugMessage = Acceleration.ToString();
 
             // Retrieve data from all devices
             foreach (Device dev in Devices)