AP_Terrain: added a TERRAIN_MARGIN parameter

this sets the acceptance margin for GCS generated terrain data. You
can raise this to allow old data generated with the less accurate
longitude scaling to be used

libraries/AP_Terrain/AP_Terrain.cpp

@@ -59,6 +59,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
     // @Bitmask: 0:Disable Download
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",   2, AP_Terrain, options, 0),
+
+    // @Param: MARGIN
+    // @DisplayName: Acceptance margin
+    // @Description: Margin in centi-meters to accept terrain data from the GCS. This can be used to allow older terrain data generated with less accurate latitude/longitude scaling to be used
+    // @Units: cm
+    // @Range: 2 30000
+    // @User: Advanced
+    AP_GROUPINFO("MARGIN",   3, AP_Terrain, margin, 2),
     
     AP_GROUPEND
 };

libraries/AP_Terrain/AP_Terrain.h

@@ -62,7 +62,7 @@
 // we allow for a 2cm discrepancy in the grid corners. This is to
 // account for different rounding in terrain DAT file generators using
 // different programming languages
-#define TERRAIN_LATLON_EQUAL(v1, v2) (labs((v1)-(v2)) <= 2)
+#define TERRAIN_LATLON_EQUAL(v1, v2) (labs((v1)-(v2)) <= unsigned(margin.get()))
 
 #if TERRAIN_DEBUG
 #include <assert.h>
@@ -352,6 +352,7 @@ private:
 
     // parameters
     AP_Int8  enable;
+    AP_Int16 margin;
     AP_Int16 grid_spacing; // meters between grid points
     AP_Int16 options; // option bits
 

libraries/AP_Terrain/tools/create_terrain.py

@@ -50,14 +50,28 @@ def longitude_scale(lat):
     scale = to_float32(math.cos(to_float32(math.radians(lat))))
     return max(scale, 0.01)
 
+def diff_longitude_E7(lon1, lon2):
+    '''get longitude difference, handling wrap'''
+    if lon1 * lon2 >= 0:
+        # common case of same sign
+        return lon1 - lon2
+    dlon = lon1 - lon2
+    if dlon > 1800000000:
+        dlon -= 3600000000
+    elif dlon < -1800000000:
+        dlon += 3600000000
+    return dlon
+
 def get_distance_NE_e7(lat1, lon1, lat2, lon2):
     '''get distance tuple between two positions in 1e7 format'''
-    return ((lat2 - lat1) * LOCATION_SCALING_FACTOR, (lon2 - lon1) * LOCATION_SCALING_FACTOR * longitude_scale(lat1*1.0e-7))
+    dlat = lat2 - lat1
+    dlng = diff_longitude_E7(lon2,lon1) * longitude_scale((lat1+lat2)*0.5*1.0e-7)
+    return (dlat * LOCATION_SCALING_FACTOR, dlng * LOCATION_SCALING_FACTOR)
 
 def add_offset(lat_e7, lon_e7, ofs_north, ofs_east):
     '''add offset in meters to a position'''
     dlat = int(float(ofs_north) * LOCATION_SCALING_FACTOR_INV)
-    dlng = int((float(ofs_east) * LOCATION_SCALING_FACTOR_INV) / longitude_scale(lat_e7*1.0e-7))
+    dlng = int((float(ofs_east) * LOCATION_SCALING_FACTOR_INV) / longitude_scale((lat_e7+dlat*0.5)*1.0e-7))
     return (int(lat_e7+dlat), int(lon_e7+dlng))
 
 def east_blocks(lat_e7, lon_e7):