aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Terrain: added a TERRAIN_MARGIN parameter

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2021-08-13 09:54:48 +10:00
parent df7321c0da
commit 4e98636a53
3 changed files with 26 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -59,6 +59,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
// @Bitmask: 0:Disable Download // @Bitmask: 0:Disable Download
// @User: Advanced // @User: Advanced
AP_GROUPINFO("OPTIONS", 2, AP_Terrain, options, 0), 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 AP_GROUPEND
}; };

3
libraries/AP_Terrain/AP_Terrain.h
View File

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

18
libraries/AP_Terrain/tools/create_terrain.py
View File

@ -50,14 +50,28 @@ def longitude_scale(lat):
scale = to_float32(math.cos(to_float32(math.radians(lat)))) scale = to_float32(math.cos(to_float32(math.radians(lat))))
return max(scale, 0.01) 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): def get_distance_NE_e7(lat1, lon1, lat2, lon2):
'''get distance tuple between two positions in 1e7 format''' '''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): def add_offset(lat_e7, lon_e7, ofs_north, ofs_east):
'''add offset in meters to a position''' '''add offset in meters to a position'''
dlat = int(float(ofs_north) * LOCATION_SCALING_FACTOR_INV) 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)) return (int(lat_e7+dlat), int(lon_e7+dlng))
def east_blocks(lat_e7, lon_e7): def east_blocks(lat_e7, lon_e7):