From fd294101ec81e122b768faf78c5756d3affb4482 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 23 Mar 2019 14:29:18 +1100
Subject: [PATCH] AP_Common: Location: move sanitize to be a method on location

---
 libraries/AP_Common/Location.cpp | 30 ++++++++++++++++++++++++++++++
 libraries/AP_Common/Location.h   |  5 +++++
 2 files changed, 35 insertions(+)

diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index 3dc11ce701..9c5231cb4f 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -238,5 +238,35 @@ float Location::longitude_scale() const
     return constrain_float(scale, 0.01f, 1.0f);
 }
 
+/*
+ * convert invalid waypoint with useful data. return true if location changed
+ */
+bool Location::sanitize(const Location &defaultLoc)
+{
+    bool has_changed = false;
+    // convert lat/lng=0 to mean current point
+    if (lat == 0 && lng == 0) {
+        lat = defaultLoc.lat;
+        lng = defaultLoc.lng;
+        has_changed = true;
+    }
+
+    // convert relative alt=0 to mean current alt
+    if (alt == 0 && relative_alt) {
+        relative_alt = false;
+        alt = defaultLoc.alt;
+        has_changed = true;
+    }
+
+    // limit lat/lng to appropriate ranges
+    if (!check_latlng(lat, lng)) {
+        lat = defaultLoc.lat;
+        lng = defaultLoc.lng;
+        has_changed = true;
+    }
+
+    return has_changed;
+}
+
 // make sure we know what size the Location object is:
 assert_storage_size<Location, 16> _assert_storage_size_Location;
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index 50f80c57bd..3ea1226b41 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -85,6 +85,11 @@ public:
 
     void zero(void);
 
+    /*
+     * convert invalid waypoint with useful data. return true if location changed
+     */
+    bool sanitize(const struct Location &defaultLoc);
+
 private:
     static AP_Terrain *_terrain;
 };