From 4b18b7d41bc3d1d53e14875312bd61a3e10f0fe5 Mon Sep 17 00:00:00 2001
From: TheCyberBrick <TheCyberBrick@users.noreply.github.com>
Date: Wed, 20 Dec 2023 16:16:32 +0100
Subject: [PATCH] Add setting to start azimuth alignment at opposite side

---
 AutoPolarAlignment.cs | 34 ++++++++++++++++++++++++++++++----
 Settings.cs           |  1 +
 Simulation.cs         |  2 +-
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/AutoPolarAlignment.cs b/AutoPolarAlignment.cs
index 18a315b..815437d 100644
--- a/AutoPolarAlignment.cs
+++ b/AutoPolarAlignment.cs
@@ -14,6 +14,8 @@ public class AutoPolarAlignment
 
         public Axis Azimuth { get; } = new Axis("azimuth");
 
+        protected Vec2 lastMeasuredOffset;
+
         protected readonly Settings settings;
 
         public AutoPolarAlignment(IPolarAlignmentMount mount, IPolarAlignmentSolver solver, Settings settings)
@@ -140,7 +142,7 @@ protected bool AlignToTarget()
                     {
                         double prev = Altitude.BacklashCompensation;
 
-                        Altitude.BacklashCompensation = Math.Max(0, Altitude.BacklashCompensation - Math.Abs(correction.Altitude) * 2);
+                        Altitude.BacklashCompensation = Math.Max(0, Altitude.BacklashCompensation - Math.Abs(correction.Altitude));
 
                         Console.WriteLine("Adjusted altitude backlash: " + Altitude.BacklashCompensation + " (" + (Altitude.BacklashCompensation - prev).ToString("+0.###;-0.###") + ")");
                     }
@@ -149,7 +151,7 @@ protected bool AlignToTarget()
                     {
                         double prev = Azimuth.BacklashCompensation;
 
-                        Azimuth.BacklashCompensation = Math.Max(0, Azimuth.BacklashCompensation - Math.Abs(correction.Azimuth) * 2);
+                        Azimuth.BacklashCompensation = Math.Max(0, Azimuth.BacklashCompensation - Math.Abs(correction.Azimuth));
 
                         Console.WriteLine("Adjusted azimuth backlash: " + Azimuth.BacklashCompensation + " (" + (Azimuth.BacklashCompensation - prev).ToString("+0.###;-0.###") + ")");
                     }
@@ -200,12 +202,17 @@ public bool Calibrate()
         private Vec2 MeasureCurrentOffset()
         {
             var offset = new Vec2();
+
             for (int i = 0; i < settings.SamplesPerMeasurement; ++i)
             {
                 solver.Solve();
                 offset += solver.AlignmentOffset;
             }
+
             offset /= settings.SamplesPerMeasurement;
+
+            lastMeasuredOffset = offset;
+
             return offset;
         }
 
@@ -213,12 +220,31 @@ private bool CalibrateAltitude()
         {
             // Reverse altitude calibration dir to later help
             // StartAtLowAltitude if enabled
-            return CalibrateAxis(Altitude, settings.AltitudeBacklashCalibration, reverse: true, margin: settings.StartAtLowAltitude ? -Math.Max(Altitude.BacklashCompensation, settings.AltitudeBacklash) : 0);
+            return CalibrateAxis(Altitude, settings.AltitudeBacklashCalibration, reverse: true, margin: settings.StartAtLowAltitude ? -settings.AltitudeCalibrationDistance * 0.25 : 0);
         }
 
         private bool CalibrateAzimuth()
         {
-            return CalibrateAxis(Azimuth, settings.AzimuthBacklashCalibration);
+            double margin = 0;
+            bool reverse = false;
+
+            if (settings.StartAtOppositeAzimuth && Altitude.CalibratedMagnitude > double.Epsilon && Altitude.CalibratedDirection.Length > double.Epsilon && lastMeasuredOffset.Length > double.Epsilon)
+            {
+                // Calibrate azimuth axis towards pole such that the maximum distance
+                // from the pole during calibration is minimized.
+                // Assumes that azimuth axis is orthogonal to altitude axis and that
+                // positive azimuth points to the right when positive altitude points
+                // up.
+
+                var estimatedAzimuthDirection = new Vec2(Altitude.CalibratedDirection.Y, -Altitude.CalibratedDirection.X);
+
+                int estimatedAzimuthOffsetDir = Math.Sign(estimatedAzimuthDirection.Dot(lastMeasuredOffset));
+
+                reverse = estimatedAzimuthOffsetDir > 0;
+                margin = -estimatedAzimuthOffsetDir * settings.AzimuthCalibrationDistance * 0.25;
+            }
+
+            return CalibrateAxis(Azimuth, settings.AzimuthBacklashCalibration, reverse: reverse, margin: margin);
         }
 
         private bool CalibrateAxis(Axis axis, bool calibrateBacklash, bool reverse = false, double margin = 0)
diff --git a/Settings.cs b/Settings.cs
index b43a83c..0223701 100644
--- a/Settings.cs
+++ b/Settings.cs
@@ -12,6 +12,7 @@ public class Settings
         public double AltitudeLimit = 600;
         public bool ResistDirectionChange = true;
         public bool StartAtLowAltitude = true;
+        public bool StartAtOppositeAzimuth = true;
         public int SamplesPerCalibration = 1;
         public int SamplesPerMeasurement = 6;
         public int MaxAlignmentIterations = 32;
diff --git a/Simulation.cs b/Simulation.cs
index 3ed51fc..3e72aeb 100644
--- a/Simulation.cs
+++ b/Simulation.cs
@@ -91,7 +91,7 @@ public static void Run()
                 Vec2 initialAlignmentOffset = new Vec2(42.0f, -87.0f);
 
                 Vec2 altAxis = new Vec2(1, 1);
-                Vec2 azAxis = new Vec2(-1, 1);
+                Vec2 azAxis = new Vec2(1, -1);
 
                 float altAxisScale = 3.0f;
                 float azAxisScale = 2.0f;