AP_Terrain: implement height_amsl()

libraries/AP_Terrain/AP_Terrain.cpp

@@ -17,6 +17,7 @@
 #include <AP_HAL.h>
 #include <AP_Common.h>
 #include <AP_Math.h>
+#include <GCS_MAVLink.h>
 #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; i<grids_allocated; i++) {
+        if (grids[i].valid && 
+            grids[i].lat == info.grid_lat && 
+            grids[i].lon == info.grid_lon) {
+            return &grids[i];
+        }
+    }
+
+    // not found
+    return NULL;
+}
+
+
+/*
+  return terrain height in meters above average sea level (WGS84) for
+  a given position
+ */
+bool AP_Terrain::height_amsl(const Location &loc, float &height)
+{
+    if (!enable || grids == NULL) {
+        return false;
+    }
+
+    struct grid_info info;
+    calculate_grid_info(loc, info);
+
+    // see if we have that grid
+    uint16_t i;
+    for (i=0; i<grids_allocated; i++) {
+        if (grids[i].valid && 
+            grids[i].lat == info.grid_lat && 
+            grids[i].lon == info.grid_lon) {
+            // found it, interpolate within the grid
+            break;
+        }
+    }
+    if (i == grids_allocated) {
+        // not found
+        return false;
+    }
+
+    // hXY are the heights of the 4 surrounding grid points
+    int16_t h00, h01, h10, h11;
+
+    // we can get h00 now
+    h00 = grids[i].height[info.idx_x][info.idx_y];
+
+    // when finding neighbouring grids ask for grid_spacing*2
+    // to cope with rounding resulting in gaps between grids
+    const uint16_t grid_sep = grid_spacing*2;
+
+    // maximum index within a grid
+    const uint8_t max_idx = 5-1;
+
+    // do we cross into another grid?
+    if (info.idx_x < max_idx && info.idx_y < max_idx) {
+        h01 = grids[i].height[info.idx_x][info.idx_y+1];
+        h10 = grids[i].height[info.idx_x+1][info.idx_y];
+        h11 = grids[i].height[info.idx_x+1][info.idx_y+1];
+    } else if (info.idx_x == max_idx && info.idx_y < max_idx) {
+        // we need the grid above this one
+        // note that we use 
+        const grid *grid2 = find_grid(loc, grid_sep, 0);
+        if (grid2 == NULL) {
+            return false;
+        }
+        h01 = grids[i].height[info.idx_x][info.idx_y+1];
+        h10 = grid2->height[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;
+}

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;