From 4e98636a53f900ed5a724caf305517c7c11cf4cc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Aug 2021 09:54:48 +1000 Subject: [PATCH] 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 | 8 ++++++++ libraries/AP_Terrain/AP_Terrain.h | 3 ++- libraries/AP_Terrain/tools/create_terrain.py | 18 ++++++++++++++++-- 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 547668e0cc..f66848e8e6 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/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 }; diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index a53dce5bab..747518c3f9 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/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 @@ -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 diff --git a/libraries/AP_Terrain/tools/create_terrain.py b/libraries/AP_Terrain/tools/create_terrain.py index 510cf84d2f..52aa031618 100755 --- a/libraries/AP_Terrain/tools/create_terrain.py +++ b/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):