AP_Math: added check_latlng helper

libraries/AP_Math/location.cpp

@@ -308,3 +308,17 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
   llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
   llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
 }
+
+// return true when lat and lng are within range
+bool check_latlng(float lat, float lng)
+{
+    return (fabsf(lat) <= 90) && (fabsf(lng) <= 180);
+}
+bool check_latlng(int32_t lat, int32_t lng)
+{
+    return (labs(lat) <= 90*1e7) && (labs(lng) <= 180*1e7);
+}
+bool check_latlng(Location loc)
+{
+    return check_latlng(loc.lat, loc.lng);
+}

libraries/AP_Math/location.h

@@ -79,3 +79,8 @@ void        wgsllh2ecef(const Vector3d &llh, Vector3d &ecef);
 // coordinates (lat, lon, height)
 void        wgsecef2llh(const Vector3d &ecef, Vector3d &llh);
 
+// return true when lat and lng are within range
+bool        check_latlng(float lat, float lng);
+bool        check_latlng(int32_t lat, int32_t lng);
+bool        check_latlng(Location loc);
+