From f130503cc73b7b1b4933e44700eb2039701f6467 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Wed, 21 Jul 2021 10:07:00 +0930
Subject: [PATCH] AC_AttitudeControl: AC_PosControl: calculate cross track

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 16 ++++++++++++++++
 libraries/AC_AttitudeControl/AC_PosControl.h   |  3 +++
 2 files changed, 19 insertions(+)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index dc95c979af..2321ff6364 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1172,6 +1172,22 @@ void AC_PosControl::write_log()
     }
 }
 
+/// crosstrack_error - returns horizontal error to the closest point to the current track
+float AC_PosControl::crosstrack_error() const
+{
+    const Vector3f& curr_pos = _inav.get_position();
+    const Vector2f pos_error = curr_pos.xy() - (_pos_target.xy()).tofloat();
+    if (is_zero(_vel_desired.xy().length_squared())) {
+        // crosstrack is the horizontal distance to target when stationary
+        return pos_error.length();
+    } else {
+        // crosstrack is the horizontal distance to the closest point to the current track
+        const Vector2f vel_unit = _vel_desired.xy().normalized();
+        const float dot_error = pos_error * vel_unit;
+        return safe_sqrt(pos_error.length_squared() - sq(dot_error));
+    }
+}
+
 ///
 /// private methods
 ///
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index f579882d35..5f72dcde4a 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -350,6 +350,9 @@ public:
     /// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
     float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
 
+    /// crosstrack_error - returns horizontal error to the closest point to the current track
+    float crosstrack_error() const;
+
     /// standby_xyz_reset - resets I terms and removes position error
     ///     This function will let Loiter and Alt Hold continue to operate
     ///     in the event that the flight controller is in control of the