diff --git a/src/Devices/AtcDimetronic.cs b/src/Devices/AtcDimetronic.cs
index 32428b9..1060fe6 100644
--- a/src/Devices/AtcDimetronic.cs
+++ b/src/Devices/AtcDimetronic.cs
@@ -84,7 +84,7 @@ namespace OpenbveFcmbTrainPlugin
         private readonly bool AtoAvailable;
 
         /// <summary>The delay before ATO changes notches.</summary>
-        private readonly double AtoNotchDelay = 500;
+        private readonly double AtoNotchDelay = 250;
 
         /// <summary>The counter for the ATO notch delay.</summary>
         private double AtoNotchDelayCounter;
@@ -483,6 +483,8 @@ namespace OpenbveFcmbTrainPlugin
                         {
                             if (AtoState == AtoStates.AwaitingStartup)
                             {
+                                // Release brakes to start ATO
+                                AtoBrakeNotch = 0;
                                 AtoState = AtoStates.IncreaseSpeed;
                             }
                         }
@@ -706,6 +708,9 @@ namespace OpenbveFcmbTrainPlugin
             Speed target = CurrentSpeedCode.TargetLimit;
             double distance = CurrentSpeedCode.TargetPosition - TrainLocation;
 
+            // Pitch multiplier, used to alter when state changes occur
+            double pitchMulti = train.State.Pitch / 20;
+
             // Update notch delay counter
             AtoNotchDelayCounter += elapsedTime.Milliseconds;
 
@@ -732,30 +737,41 @@ namespace OpenbveFcmbTrainPlugin
                     AtoStartupPosition = TrainLocation;
                     break;
                 case AtoStates.IncreaseSpeed:
-                    AtoBrakeNotch = 0;
-                    if (train.Acceleration < 0.2 && AtoPowerNotch < train.Specs.PowerNotches && AtoNotchDelayCounter >= AtoNotchDelay)
+                    if (train.Acceleration < 0.5 && AtoNotchDelayCounter >= AtoNotchDelay)
                     {
-                        AtoPowerNotch++;
+                        if (AtoBrakeNotch > 0)
+                        {
+                            AtoBrakeNotch--;
+                        }
+                        else if (AtoPowerNotch < train.Specs.PowerNotches)
+                        {
+                            AtoPowerNotch++;
+                        }
                         AtoNotchDelayCounter = 0;
                     }
-                    if (train.State.Speed.KilometersPerHour >= target.KilometersPerHour - 5)
+                    if (train.State.Speed.KilometersPerHour >= target.KilometersPerHour - 5 + pitchMulti)
                     {
                         AtoState = AtoStates.MaintainSpeed;
                     }
                     break;
                 case AtoStates.MaintainSpeed:
-                    AtoBrakeNotch = 0;
-                    if (train.Acceleration > 0 && AtoNotchDelayCounter >= AtoNotchDelay)
+                    if ((train.Acceleration > 0 || (AtoPowerNotch > 0 && train.State.Pitch <= 0)) && AtoNotchDelayCounter >= AtoNotchDelay)
                     {
+                        // Train is accelerating, try to reduce speed
                         if (AtoPowerNotch > 0)
                         {
                             AtoPowerNotch--;
+                            AtoNotchDelayCounter = 0;
                         }
-                        else if (AtoBrakeNotch < train.Specs.BrakeNotches)
+                        else if (AtoBrakeNotch < train.Specs.BrakeNotches && train.State.Pitch < 0 && train.State.Speed.KilometersPerHour >= target.KilometersPerHour - 3 + pitchMulti)
                         {
                             AtoBrakeNotch++;
+                            AtoNotchDelayCounter = 0;
                         }
-                        AtoNotchDelayCounter = 0;
+                    }
+                    if (train.State.Speed.KilometersPerHour <= target.KilometersPerHour - 7 + pitchMulti)
+                    {
+                        AtoState = AtoStates.IncreaseSpeed;
                     }
                     break;
             }
@@ -767,8 +783,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;
-                DeviceState = DeviceStates.NoMode;
+                //AtoState = AtoStates.Alarm;
+                //DeviceState = DeviceStates.NoMode;
             }
         }
     }
diff --git a/src/Train/Train.cs b/src/Train/Train.cs
index 3bcd09c..4b67e22 100644
--- a/src/Train/Train.cs
+++ b/src/Train/Train.cs
@@ -54,8 +54,11 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>The current acceleration of the train.</summary>
         internal double Acceleration { get; private set; }
 
-        /// <summary>The time when acceleration was calculated.</summary>
-        private Time AccelerationTime = new Time(0);
+        /// <summary>The time elapsed since acceleration was last calculated.</summary>
+        private double AccelerationTimer;
+
+        /// <summary>The maximum time elapsed between acceleration calculations.</summary>
+        private const double AccelerationMaximumTimer = 100;
 
         /// <summary>The speed when acceleration was calculated.</summary>
         private Speed AccelerationSpeed = new Speed(0);
@@ -133,16 +136,14 @@ namespace OpenbveFcmbTrainPlugin
             data.Handles.BrakeNotch = data.Handles.BrakeNotch;
 
             // Calculate acceleration
-            if (OpenbveFcmbTrainPlugin.Initializing)
+            AccelerationTimer += data.ElapsedTime.Milliseconds;
+            if (AccelerationTimer >= AccelerationMaximumTimer)
             {
-                AccelerationTime = data.TotalTime;
-            }
-            if (data.TotalTime.Milliseconds - AccelerationTime.Milliseconds > 200)
-            {
-                Acceleration = Math.Round((State.Speed.MetersPerSecond - AccelerationSpeed.MetersPerSecond) / (data.TotalTime.Seconds - AccelerationTime.Seconds), 4);
-                AccelerationTime = data.TotalTime;
-                AccelerationSpeed = State.Speed;
+                Acceleration = Math.Round((data.Vehicle.Speed.MetersPerSecond - AccelerationSpeed.MetersPerSecond) / AccelerationTimer * 1000, 4);
+                AccelerationTimer = 0;
+                AccelerationSpeed = data.Vehicle.Speed;
             }
+            data.DebugMessage = Acceleration.ToString();
 
             // Retrieve data from all devices
             foreach (Device dev in Devices)