From a7eb2ac28c31273188fd084c3caff8bfda1462c8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Jul 2014 10:55:00 +1000 Subject: [PATCH] AP_Terrain: implement height_amsl() --- libraries/AP_Terrain/AP_Terrain.cpp | 146 +++++++++++++++++++++++++++- libraries/AP_Terrain/AP_Terrain.h | 60 ++++++++---- 2 files changed, 188 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 5e37640d4d..6083e6dd91 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "AP_Terrain.h" extern const AP_HAL::HAL& hal; @@ -33,7 +34,7 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = { }; // constructor -AP_Terrain::AP_Terrain(const AP_AHRS &_ahrs) : +AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) : ahrs(_ahrs), last_grid_spacing(0), grids_allocated(0), @@ -92,3 +93,146 @@ void AP_Terrain::update(void) // re-allocate if need be allocate(); } + +/* + given a location, calculate the 5x5 grid NW corner, plus the + grid index and grid square fraction +*/ +void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const +{ + // grids start on integer degrees. This makes storing terrain data + // on the SD card a bit easier + info.lat_degrees = loc.lat / 10*1000*1000UL; + info.lon_degrees = loc.lng / 10*1000*1000UL; + + // create reference position. Longitude scaling is taken from this point + Location ref; + ref.lat = info.lat_degrees; + ref.lng = info.lon_degrees; + + // find offset from reference + Vector2f offset = location_diff(ref, loc); + + // work out how many 5x5 grid squares we are in. x is north, y is east + info.idx_x = ((uint16_t)(offset.x / grid_spacing))/5; + info.idx_y = ((uint16_t)(offset.y / grid_spacing))/5; + + // work out fractional (0 to 1) position within grid square. + info.frac_x = (offset.x - (info.idx_x * 5.0f * grid_spacing)) / grid_spacing; + info.frac_y = (offset.y - (info.idx_y * 5.0f * grid_spacing)) / grid_spacing; + + // calculate lat/lon of SW corner of 5x5 grid + location_offset(ref, info.idx_x*grid_spacing, info.idx_y*grid_spacing); + info.grid_lat = ref.lat; + info.grid_lon = ref.lng; +} + +/* + find a grid structure given a location and offset in meters + */ +const AP_Terrain::grid *AP_Terrain::find_grid(const Location &loc, uint16_t ofs_north, uint16_t ofs_east) const +{ + struct grid_info info; + Location loc2 = loc; + location_offset(loc2, ofs_north, ofs_east); + calculate_grid_info(loc2, info); + + // see if we have that grid + for (uint16_t i=0; iheight[0][info.idx_y]; + h11 = grid2->height[0][info.idx_y+1]; + } else if (info.idx_x < max_idx && info.idx_y == max_idx) { + // we need the grid to the right of this one + const grid *grid2 = find_grid(loc, 0, grid_sep); + if (grid2 == NULL) { + return false; + } + h01 = grid2->height[info.idx_x][0]; + h10 = grids[i].height[info.idx_x+1][info.idx_y]; + h11 = grid2->height[info.idx_x+1][0]; + } else { + // we need to find 3 more grids, above, right and above-right + const grid *grid_x = find_grid(loc, grid_sep, 0); + const grid *grid_y = find_grid(loc, 0, grid_sep); + const grid *grid_xy = find_grid(loc, grid_sep, grid_sep); + if (grid_x == NULL || grid_y == NULL || grid_xy == NULL) { + return false; + } + h01 = grid_y->height[info.idx_x][0]; + h10 = grid_x->height[0][info.idx_y]; + h11 = grid_xy->height[0][0]; + } + + float avg1 = (1.0f-info.frac_x) * h00 + info.frac_x * h10; + float avg2 = (1.0f-info.frac_x) * h01 + info.frac_x * h11; + float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2; + + height = avg; + return true; +} diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index e5507b1c48..487b0e343d 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -25,7 +25,7 @@ class AP_Terrain { public: - AP_Terrain(const AP_AHRS &_ahrs); + AP_Terrain(AP_AHRS &_ahrs); // parameters AP_Int8 enable; @@ -39,21 +39,7 @@ public: // return terrain height in meters above sea level for a location // return false if not available - bool height_amsl(const Location &loc); - -private: - // allocate the terrain subsystem data - void allocate(void); - - // reference to AHRS, so we can ask for our position, - // heading and speed - const AP_AHRS &ahrs; - - // the grid spacing for the current data - uint16_t last_grid_spacing; - - // number of grids in array - uint16_t grids_allocated; + bool height_amsl(const Location &loc, float &height); // a single 5x5 grid, matching a single MAVLink TERRAIN_DATA message struct grid { @@ -61,7 +47,47 @@ private: int32_t lon; int16_t height[5][5]; bool valid:1; - } grid; + }; + +private: + // allocate the terrain subsystem data + void allocate(void); + + /* + grid_info is a broken down representation of a Location, giving + the index terms for finding the right grid + */ + struct grid_info { + // rounded latitude/longitude in degrees. + int8_t lat_degrees; + uint8_t lon_degrees; + + // lat and lon of SW corner of 5x5 grid + int32_t grid_lat; + int32_t grid_lon; + // indexes into 5x5 grid. x is north, y is east + uint8_t idx_x; + uint8_t idx_y; + // fraction (0..1) within grid square. x is north, y is east + float frac_x; + float frac_y; + }; + + // given a location, fill a grid_info structure + void calculate_grid_info(const Location &loc, struct grid_info &info) const; + + // find a grid + const struct grid *find_grid(const Location &loc, uint16_t ofs_north, uint16_t ofs_east) const; + + // reference to AHRS, so we can ask for our position, + // heading and speed + AP_AHRS &ahrs; + + // the grid spacing for the current data + uint16_t last_grid_spacing; + + // number of grids in array + uint16_t grids_allocated; // current grids struct grid *grids;