AC_AttitudeControl: AC_PosControl: calculate cross track

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
 ///

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