AP_Terrain: added mavlink transport of terrain data

This commit is contained in:
Andrew Tridgell 2014-07-22 08:23:35 +10:00
parent e23ee34d95
commit 09518d2d91
2 changed files with 301 additions and 122 deletions

View File

@ -18,7 +18,12 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <GCS.h>
#include "AP_Terrain.h" #include "AP_Terrain.h"
#include <assert.h>
#include <stdio.h>
#if HAVE_AP_TERRAIN
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -43,106 +48,121 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = {
// constructor // constructor
AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) : AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) :
ahrs(_ahrs), ahrs(_ahrs),
grid_cache(NULL),
last_request_time_ms(0) last_request_time_ms(0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
/* /*
given a location, calculate the 45x45 grid SW corner, plus the calculate bit number in grid_block bitmap. This corresponds to a
grid index and grid square fraction bit representing a 4x4 mavlink transmitted block
*/
uint8_t AP_Terrain::grid_bitnum(uint8_t idx_x, uint8_t idx_y)
{
ASSERT_RANGE(idx_x,0,27);
ASSERT_RANGE(idx_y,0,31);
uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE;
uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE;
ASSERT_RANGE(subgrid_x,0,TERRAIN_GRID_BLOCK_MUL_X-1);
ASSERT_RANGE(subgrid_y,0,TERRAIN_GRID_BLOCK_MUL_Y-1);
return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x;
}
/*
given a grid_info check that a given idx_x/idx_y is available (set
in the bitmap)
*/
bool AP_Terrain::check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y)
{
uint8_t bitnum = grid_bitnum(idx_x, idx_y);
return (grid.bitmap & (((uint64_t)1U)<<bitnum)) != 0;
}
/*
given a location, calculate the 32x28 grid SW corner, plus the
grid indices
*/ */
void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const
{ {
// grids start on integer degrees. This makes storing terrain data // grids start on integer degrees. This makes storing terrain data
// on the SD card a bit easier // on the SD card a bit easier
info.lat_degrees = loc.lat / 10*1000*1000UL; info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
info.lon_degrees = loc.lng / 10*1000*1000UL; info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L);
// create reference position. Longitude scaling is taken from this point // create reference position for this rounded degree position
Location ref; Location ref;
ref.lat = info.lat_degrees; ref.lat = info.lat_degrees*10*1000*1000L;
ref.lng = info.lon_degrees; ref.lng = info.lon_degrees*10*1000*1000L;
// find offset from reference // find offset from reference
Vector2f offset = location_diff(ref, loc); Vector2f offset = location_diff(ref, loc);
// work out how many 45x45 grid squares we are in. x is north, y is east // get indices in terms of grid_spacing elements
info.idx_x = ((uint16_t)(offset.x / grid_spacing))/TERRAIN_GRID_BLOCK_SIZE; uint32_t idx_x = offset.x / grid_spacing;
info.idx_y = ((uint16_t)(offset.y / grid_spacing))/TERRAIN_GRID_BLOCK_SIZE; uint32_t idx_y = offset.y / grid_spacing;
// work out fractional (0 to 1) position within grid square. // find indexes into 32*28 grids for this degree reference. Note
info.frac_x = (offset.x - (info.idx_x * (float)TERRAIN_GRID_BLOCK_SIZE * grid_spacing)) / grid_spacing; // the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
info.frac_y = (offset.y - (info.idx_y * (float)TERRAIN_GRID_BLOCK_SIZE * grid_spacing)) / grid_spacing; // overlap between grids
uint16_t grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X;
uint16_t grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y;
// calculate lat/lon of SW corner of 45x45 grid_block // find the indices within the 32*28 grid
location_offset(ref, info.idx_x*grid_spacing, info.idx_y*grid_spacing); info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X;
info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;
// find the fraction (0..1) within the square
info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing;
info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing;
// calculate lat/lon of SW corner of 32*28 grid_block
location_offset(ref,
grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing,
grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing);
info.grid_lat = ref.lat; info.grid_lat = ref.lat;
info.grid_lon = ref.lng; info.grid_lon = ref.lng;
// calculate bit number in grid_block bitmap ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
info.bitnum = (info.idx_y/TERRAIN_GRID_MAVLINK_SIZE)*TERRAIN_GRID_BLOCK_MUL + info.idx_x/TERRAIN_GRID_MAVLINK_SIZE; ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1);
ASSERT_RANGE(info.frac_x,0,1);
ASSERT_RANGE(info.frac_y,0,1);
} }
/*
given a location and offset, calculate the 45x45 grid SW corner, plus
the grid index and grid square fraction
*/
void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info, int16_t ofs_north, int16_t ofs_east) const
{
Location loc2 = loc;
location_offset(loc2, ofs_north, ofs_east);
calculate_grid_info(loc2, info);
}
/* /*
find a grid structure given a grid_info find a grid structure given a grid_info
*/ */
AP_Terrain::grid_block &AP_Terrain::find_grid(const struct grid_info &info) const AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info)
{ {
uint16_t oldest_i = 0; uint16_t oldest_i = 0;
// see if we have that grid // see if we have that grid
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (grid_cache[i].lat == info.grid_lat && if (cache[i].grid.lat == info.grid_lat &&
grid_cache[i].lon == info.grid_lon) { cache[i].grid.lon == info.grid_lon) {
grid_cache[i].last_access_ms = hal.scheduler->millis(); cache[i].last_access_ms = hal.scheduler->millis();
return &grid_cache[i]; return cache[i];
} }
if (grid_cache[i].last_access_ms < grid_cache[oldest_i].last_access_ms) { if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
oldest_i = i; oldest_i = i;
} }
} }
// Not found. Use the oldest grid and make it this grid, // Not found. Use the oldest grid and make it this grid,
// unpopulated // initially unpopulated
struct grid_block &grid = grid_cache[oldest_i]; struct grid_cache &grid = cache[oldest_i];
memset(&grid, 0, sizeof(grid)); memset(&grid, 0, sizeof(grid));
grid.lat = info.grid_lat; grid.grid.lat = info.grid_lat;
grid.lon = info.grid_lon; grid.grid.lon = info.grid_lon;
grid.spacing = grid_spacing; grid.grid.spacing = grid_spacing;
grid.last_access_ms = hal.scheduler->millis();
return grid; return grid;
} }
/*
return terrain height in meters above average sea level (WGS84) for
a grid_info, returning the height for the SW corner of the grid square
*/
bool AP_Terrain::height_sw_corner(const struct grid_info &info, int16_t &height)
{
struct grid_block &grid = find_grid(info);
if (grid.bitmask[info.bitnum/8] & (1U<<(info.bitnum%8))) {
// we have the height
height = grid.height[info.idx_x][info.idx_y];
return true;
}
return false;
}
/* /*
return terrain height in meters above average sea level (WGS84) for return terrain height in meters above average sea level (WGS84) for
a given position a given position
@ -153,37 +173,137 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
return false; return false;
} }
struct grid_info info00, info01, info10, info11; struct grid_info info;
// we push the spacing up a bit to cope with changes in longitude calculate_grid_info(loc, info);
// scaling between grids
uint16_t spacing2 = spacing*1.2f; // find the grid
calculate_grid_info(loc, info00); const struct grid_block &grid = find_grid(info).grid;
calculate_grid_info(loc, info01, 0, spacing2);
calculate_grid_info(loc, info10, spacing2, 0); /*
calculate_grid_info(loc, info11, spacing2, spacing2); note that we rely on the one square overlap to ensure these
calculations don't go past the end of the arrays
*/
ASSERT_RANGE(info.idx_x, 0, TERRAIN_GRID_BLOCK_SIZE_X-2);
ASSERT_RANGE(info.idx_y, 0, TERRAIN_GRID_BLOCK_SIZE_Y-2);
// check we have all 4 required heights
if (!check_bitmap(grid, info.idx_x, info.idx_y) ||
!check_bitmap(grid, info.idx_x, info.idx_y+1) ||
!check_bitmap(grid, info.idx_x+1, info.idx_y) ||
!check_bitmap(grid, info.idx_x+1, info.idx_y+1)) {
return false;
}
// hXY are the heights of the 4 surrounding grid points // hXY are the heights of the 4 surrounding grid points
int16_t h00, h01, h10, h11; int16_t h00, h01, h10, h11;
if (!height_sw_corner(info00, h00) || h00 = grid.height[info.idx_x+0][info.idx_y+0];
!height_sw_corner(info01, h01) || h01 = grid.height[info.idx_x+0][info.idx_y+1];
!height_sw_corner(info10, h10) || h10 = grid.height[info.idx_x+1][info.idx_y+0];
!height_sw_corner(info11, h11)) { h11 = grid.height[info.idx_x+1][info.idx_y+1];
// we don't have the data on all sided
return false;
}
// TODO: cope with crossing degree boundaries 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 avg1 = (1.0f-info00.frac_x) * h00 + info00.frac_x * h10; float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
float avg2 = (1.0f-info00.frac_x) * h01 + info00.frac_x * h11;
float avg = (1.0f-info00.frac_y) * avg1 + info00.frac_y * avg2;
height = avg; height = avg;
return true; return true;
} }
/*
request any missing 4x4 grids from a block
*/
bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info)
{
// find the grid
struct grid_block &grid = find_grid(info).grid;
// see if it is fully populated
const uint64_t mask = (((uint64_t)1U)<<(TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y)) - 1;
if ((grid.bitmap & mask) == mask) {
// it is fully populated, nothing to do
return false;
}
/*
ask the GCS to send a set of 4x4 grids
*/
mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, mask & ~grid.bitmap);
last_request_time_ms = hal.scheduler->millis();
return true;
}
/*
send any pending terrain request to the GCS
*/
void AP_Terrain::send_request(mavlink_channel_t chan)
{
if (enable == 0) {
// not enabled
return;
}
// did we request recently?
if (hal.scheduler->millis() - last_request_time_ms < 2000) {
// too soon to request again
return;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return;
}
// request any missing 4x4 blocks in the current grid
struct grid_info info;
calculate_grid_info(loc, info);
if (request_missing(chan, info)) {
return;
}
}
/*
handle terrain data from GCS
*/
void AP_Terrain::handle_data(mavlink_message_t *msg)
{
mavlink_terrain_data_t packet;
mavlink_msg_terrain_data_decode(msg, &packet);
uint16_t i;
for (i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (cache[i].grid.lat == packet.lat &&
cache[i].grid.lon == packet.lon &&
cache[i].grid.spacing == packet.grid_spacing &&
packet.gridbit < 56) {
break;
}
}
if (i == TERRAIN_GRID_BLOCK_CACHE_SIZE) {
// we don't have that grid, ignore data
return;
}
struct grid_cache &gcache = cache[i];
struct grid_block &grid = gcache.grid;
uint8_t idx_x = (packet.gridbit / TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
uint8_t idx_y = (packet.gridbit % TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
ASSERT_RANGE(idx_x,0,(TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE);
ASSERT_RANGE(idx_y,0,(TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE);
for (uint8_t x=0; x<TERRAIN_GRID_MAVLINK_SIZE; x++) {
for (uint8_t y=0; y<TERRAIN_GRID_MAVLINK_SIZE; y++) {
grid.height[idx_x+x][idx_y+y] = packet.data[x*TERRAIN_GRID_MAVLINK_SIZE+y];
ASSERT_RANGE(grid.height[idx_x+x][idx_y+y], 1, 20000);
}
}
hal.console->printf("Filled bit %u idx_x=%u idx_y=%u\n",
(unsigned)packet.gridbit, (unsigned)idx_x, (unsigned)idx_y);
gcache.grid.bitmap |= ((uint64_t)1) << packet.gridbit;
}
/* /*
update terrain data. Check if we need to request more grids. This update terrain data. Check if we need to request more grids. This
@ -191,26 +311,15 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
*/ */
void AP_Terrain::update(void) void AP_Terrain::update(void)
{ {
if (enable == 0) { float height;
// not enabled
return;
}
Location loc; Location loc;
if (!ahrs.get_position(loc)) { if (!ahrs.get_position(loc)) {
// we don't know where we are // we don't know where we are
return; return;
} }
if (height_amsl(loc, height)) {
// find any missing 5x5 blocks in the current grid printf("height %.2f\n", height);
struct grid_info info;
calculate_grid_info(loc, info);
if (request_missing(info)) {
return;
} }
} }
void AP_Terrain::request_missing(struct grid_info &info) #endif // HAVE_AP_TERRAIN
{
}

View File

@ -19,20 +19,49 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_HAL.h> #include <AP_HAL.h>
#if HAL_OS_POSIX_IO
#define HAVE_AP_TERRAIN 1
#else
#define HAVE_AP_TERRAIN 0
#endif
#if HAVE_AP_TERRAIN
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
// MAVLink sends 5x5 grids // MAVLink sends 4x4 grids
#define TERRAIN_GRID_MAVLINK_SIZE 5 #define TERRAIN_GRID_MAVLINK_SIZE 4
// a 2k grid_block contains 9x9 of the mavlink grids // a 2k grid_block on disk contains 8x7 of the mavlink grids. Each
#define TERRAIN_GRID_BLOCK_MUL 9 // grid block overlaps by one with its neighbour. This ensures that
// the altitude at any point can be calculated from a single grid
// block
#define TERRAIN_GRID_BLOCK_MUL_X 7
#define TERRAIN_GRID_BLOCK_MUL_Y 8
// giving a total grid size of a grid_block of 45x45 // this is the spacing between 32x28 grid blocks, in grid_spacing units
#define TERRAIN_GRID_BLOCK_SIZE (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL) #define TERRAIN_GRID_BLOCK_SPACING_X ((TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE)
#define TERRAIN_GRID_BLOCK_SPACING_Y ((TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE)
// giving a total grid size of a disk grid_block of 32x28
#define TERRAIN_GRID_BLOCK_SIZE_X (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_X)
#define TERRAIN_GRID_BLOCK_SIZE_Y (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_Y)
// number of grid_blocks in the LRU memory cache // number of grid_blocks in the LRU memory cache
#define TERRAIN_GRID_BLOCK_CACHE_SIZE 10 #define TERRAIN_GRID_BLOCK_CACHE_SIZE 12
// format of grid on disk
#define TERRAIN_GRID_FORMAT_VERSION 1
/*
Data conventions in this library:
array[x][y]: x is increasing north, y is increasing east
array[x]: low order bits increase east first
bitmap: low order bits increase east first
*/
class AP_Terrain class AP_Terrain
{ {
@ -48,6 +77,12 @@ public:
// update terrain state. Should be called at 1Hz or more // update terrain state. Should be called at 1Hz or more
void update(void); void update(void);
// send any pending terrain request message
void send_request(mavlink_channel_t chan);
// handle terrain data from GCS
void handle_data(mavlink_message_t *msg);
// return terrain height in meters above sea level for a location // return terrain height in meters above sea level for a location
// return false if not available // return false if not available
bool height_amsl(const Location &loc, float &height); bool height_amsl(const Location &loc, float &height);
@ -62,21 +97,42 @@ private:
block oriented SD cards efficient block oriented SD cards efficient
*/ */
struct PACKED grid_block { struct PACKED grid_block {
// south west corner of block in degrees*10^7
int32_t lat;
int32_t lon;
// crc of whole block, taken with crc=0 // crc of whole block, taken with crc=0
uint16_t crc; uint16_t crc;
// format version number
uint16_t version;
// grid spacing in meters // grid spacing in meters
uint16_t spacing; uint16_t spacing;
// heights in meters over a 45x45 grid // heights in meters over a 32*28 grid
int16_t height[TERRAIN_GRID_BLOCK_SIZE][TERRAIN_GRID_BLOCK_SIZE]; int16_t height[TERRAIN_GRID_BLOCK_SIZE_X][TERRAIN_GRID_BLOCK_SIZE_Y];
// bitmap of 5x5 grids filled in from GCS // south west corner of block in degrees*10^7
uint8_t bitmap[(TERRAIN_GRID_BLOCK_MUL*TERRAIN_GRID_BLOCK_MUL+7)/8]; int32_t lat;
int32_t lon;
// bitmap of 4x4 grids filled in from GCS (56 bits are used)
uint64_t bitmap;
};
/*
grid_block for disk IO, aligned on 2048 byte boundaries
*/
union grid_io_block {
struct grid_block grid;
uint8_t buffer[2048];
};
/*
a grid_block plus some meta data used for requesting new blocks
*/
struct grid_cache {
struct grid_block grid;
// the last time access was requested to this block, used for LRU
uint32_t last_access_ms;
}; };
/* /*
@ -86,42 +142,56 @@ private:
struct grid_info { struct grid_info {
// rounded latitude/longitude in degrees. // rounded latitude/longitude in degrees.
int8_t lat_degrees; int8_t lat_degrees;
uint8_t lon_degrees; int16_t lon_degrees;
// lat and lon of SW corner of 45x45 grid // lat and lon of SW corner of this 32*28 grid, *10^7 degrees
int32_t grid_lat; int32_t grid_lat;
int32_t grid_lon; int32_t grid_lon;
// indexes into 45x45 grid. x is north, y is east // indexes into 32x28 grid
uint8_t idx_x; uint8_t idx_x; // north (0..27)
uint8_t idx_y; uint8_t idx_y; // east (0..31)
// fraction (0..1) within grid square. x is north, y is east // fraction within the grid square
float frac_x; float frac_x; // north (0..1)
float frac_y; float frac_y; // east (0..1)
// bit number within the 9x9 grid_block bitmap
uint8_t bitnum;
}; };
// given a location, fill a grid_info structure // given a location, fill a grid_info structure
void calculate_grid_info(const Location &loc, struct grid_info &info) const; void calculate_grid_info(const Location &loc, struct grid_info &info) const;
// find a grid /*
struct grid_block &find_grid(const Location &loc, uint16_t ofs_north, uint16_t ofs_east) const; find a grid structure given a grid_info
*/
struct grid_cache &find_grid(const struct grid_info &info);
/*
calculate bit number in grid_block bitmap. This corresponds to a
bit representing a 4x4 mavlink transmitted block
*/
uint8_t grid_bitnum(uint8_t idx_x, uint8_t idx_y);
/*
given a grid_info check that a given idx_x/idx_y is available (set
in the bitmap)
*/
bool check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y);
/*
request any missing 4x4 grids from a block
*/
bool request_missing(mavlink_channel_t chan, const struct grid_info &info);
// reference to AHRS, so we can ask for our position, // reference to AHRS, so we can ask for our position,
// heading and speed // heading and speed
AP_AHRS &ahrs; AP_AHRS &ahrs;
// cache of grids in memory, LRU // cache of grids in memory, LRU
struct { struct grid_cache cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
uint32_t last_access_ms;
struct grid_block block;
} grid_cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
// last time we asked for more grids // last time we asked for more grids
uint32_t last_request_time_ms; uint32_t last_request_time_ms;
}; };
#endif // HAVE_AP_TERRAIN
#endif // __AP_TERRAIN_H__ #endif // __AP_TERRAIN_H__