zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Terrain: added mavlink transport of terrain data

mission-4.1.18
Andrew Tridgell 11 years ago
parent
e23ee34d95
commit
09518d2d91
2 changed files with 292 additions and 113 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 271
      libraries/AP_Terrain/AP_Terrain.cpp
  2. 134
      libraries/AP_Terrain/AP_Terrain.h

271
libraries/AP_Terrain/AP_Terrain.cpp
Unescape View File

@ -18,7 +18,12 @@ @@ -18,7 +18,12 @@
#include <AP_Common.h>
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#include <GCS.h>
#include "AP_Terrain.h"
#include <assert.h>
#include <stdio.h>
#if HAVE_AP_TERRAIN
extern const AP_HAL::HAL& hal;
@ -43,106 +48,121 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = { @@ -43,106 +48,121 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = {
// constructor
AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) :
ahrs(_ahrs),
grid_cache(NULL),
last_request_time_ms(0)
{
AP_Param::setup_object_defaults(this, var_info);
}
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
/*
calculate bit number in grid_block bitmap. This corresponds to a
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 45x45 grid SW corner, plus the
grid index and grid square fraction
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
{
// 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;
info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
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;
ref.lat = info.lat_degrees;
ref.lng = info.lon_degrees;
ref.lat = info.lat_degrees*10*1000*1000L;
ref.lng = info.lon_degrees*10*1000*1000L;
// find offset from reference
Vector2f offset = location_diff(ref, loc);
// work out how many 45x45 grid squares we are in. x is north, y is east
info.idx_x = ((uint16_t)(offset.x / grid_spacing))/TERRAIN_GRID_BLOCK_SIZE;
info.idx_y = ((uint16_t)(offset.y / grid_spacing))/TERRAIN_GRID_BLOCK_SIZE;
// get indices in terms of grid_spacing elements
uint32_t idx_x = offset.x / grid_spacing;
uint32_t idx_y = offset.y / grid_spacing;
// find indexes into 32*28 grids for this degree reference. Note
// the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
// 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;
// work out fractional (0 to 1) position within grid square.
info.frac_x = (offset.x - (info.idx_x * (float)TERRAIN_GRID_BLOCK_SIZE * grid_spacing)) / grid_spacing;
info.frac_y = (offset.y - (info.idx_y * (float)TERRAIN_GRID_BLOCK_SIZE * grid_spacing)) / grid_spacing;
// find the indices within the 32*28 grid
info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X;
info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;
// calculate lat/lon of SW corner of 45x45 grid_block
location_offset(ref, info.idx_x*grid_spacing, info.idx_y*grid_spacing);
// 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_lon = ref.lng;
// calculate bit number in grid_block bitmap
info.bitnum = (info.idx_y/TERRAIN_GRID_MAVLINK_SIZE)*TERRAIN_GRID_BLOCK_MUL + info.idx_x/TERRAIN_GRID_MAVLINK_SIZE;
ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
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
*/
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;
// see if we have that grid
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (grid_cache[i].lat == info.grid_lat &&
grid_cache[i].lon == info.grid_lon) {
grid_cache[i].last_access_ms = hal.scheduler->millis();
return &grid_cache[i];
if (cache[i].grid.lat == info.grid_lat &&
cache[i].grid.lon == info.grid_lon) {
cache[i].last_access_ms = hal.scheduler->millis();
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;
}
}
// Not found. Use the oldest grid and make it this grid,
// unpopulated
struct grid_block &grid = grid_cache[oldest_i];
// initially unpopulated
struct grid_cache &grid = cache[oldest_i];
memset(&grid, 0, sizeof(grid));
grid.lat = info.grid_lat;
grid.lon = info.grid_lon;
grid.spacing = grid_spacing;
grid.grid.lat = info.grid_lat;
grid.grid.lon = info.grid_lon;
grid.grid.spacing = grid_spacing;
grid.last_access_ms = hal.scheduler->millis();
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
a given position
@ -153,64 +173,153 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height) @@ -153,64 +173,153 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
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
// scaling between grids
uint16_t spacing2 = spacing*1.2f;
calculate_grid_info(loc, info00);
calculate_grid_info(loc, info01, 0, spacing2);
calculate_grid_info(loc, info10, spacing2, 0);
calculate_grid_info(loc, info11, spacing2, spacing2);
calculate_grid_info(loc, info);
// find the grid
const struct grid_block &grid = find_grid(info).grid;
/*
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);
// hXY are the heights of the 4 surrounding grid points
int16_t h00, h01, h10, h11;
if (!height_sw_corner(info00, h00) ||
!height_sw_corner(info01, h01) ||
!height_sw_corner(info10, h10) ||
!height_sw_corner(info11, h11)) {
// we don't have the data on all sided
// 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;
}
// TODO: cope with crossing degree boundaries
// hXY are the heights of the 4 surrounding grid points
int16_t h00, h01, h10, h11;
float avg1 = (1.0f-info00.frac_x) * h00 + info00.frac_x * h10;
float avg2 = (1.0f-info00.frac_x) * h01 + info00.frac_x * h11;
float avg = (1.0f-info00.frac_y) * avg1 + info00.frac_y * avg2;
h00 = grid.height[info.idx_x+0][info.idx_y+0];
h01 = grid.height[info.idx_x+0][info.idx_y+1];
h10 = grid.height[info.idx_x+1][info.idx_y+0];
h11 = grid.height[info.idx_x+1][info.idx_y+1];
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;
}
/*
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;
}
/*
update terrain data. Check if we need to request more grids. This
should be called at 1Hz
send any pending terrain request to the GCS
*/
void AP_Terrain::update(void)
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;
}
// find any missing 5x5 blocks in the current grid
// request any missing 4x4 blocks in the current grid
struct grid_info info;
calculate_grid_info(loc, info);
if (request_missing(info)) {
if (request_missing(chan, info)) {
return;
}
}
void AP_Terrain::request_missing(struct grid_info &info)
/*
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
should be called at 1Hz
*/
void AP_Terrain::update(void)
{
float height;
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return;
}
if (height_amsl(loc, height)) {
printf("height %.2f\n", height);
}
}
#endif // HAVE_AP_TERRAIN

134
libraries/AP_Terrain/AP_Terrain.h
Unescape View File

@ -19,20 +19,49 @@ @@ -19,20 +19,49 @@
#include <AP_Common.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_AHRS.h>
// MAVLink sends 5x5 grids
#define TERRAIN_GRID_MAVLINK_SIZE 5
// MAVLink sends 4x4 grids
#define TERRAIN_GRID_MAVLINK_SIZE 4
// a 2k grid_block contains 9x9 of the mavlink grids
#define TERRAIN_GRID_BLOCK_MUL 9
// a 2k grid_block on disk contains 8x7 of the mavlink grids. Each
// 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
#define TERRAIN_GRID_BLOCK_SIZE (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL)
// this is the spacing between 32x28 grid blocks, in grid_spacing units
#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
#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
{
@ -48,6 +77,12 @@ public: @@ -48,6 +77,12 @@ public:
// update terrain state. Should be called at 1Hz or more
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 false if not available
bool height_amsl(const Location &loc, float &height);
@ -62,21 +97,42 @@ private: @@ -62,21 +97,42 @@ private:
block oriented SD cards efficient
*/
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
uint16_t crc;
// format version number
uint16_t version;
// grid spacing in meters
uint16_t spacing;
// heights in meters over a 45x45 grid
int16_t height[TERRAIN_GRID_BLOCK_SIZE][TERRAIN_GRID_BLOCK_SIZE];
// heights in meters over a 32*28 grid
int16_t height[TERRAIN_GRID_BLOCK_SIZE_X][TERRAIN_GRID_BLOCK_SIZE_Y];
// south west corner of block in degrees*10^7
int32_t lat;
int32_t lon;
// bitmap of 5x5 grids filled in from GCS
uint8_t bitmap[(TERRAIN_GRID_BLOCK_MUL*TERRAIN_GRID_BLOCK_MUL+7)/8];
// 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: @@ -86,42 +142,56 @@ private:
struct grid_info {
// rounded latitude/longitude in 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_lon;
// indexes into 45x45 grid. x is north, y is east
uint8_t idx_x;
uint8_t idx_y;
// indexes into 32x28 grid
uint8_t idx_x; // north (0..27)
uint8_t idx_y; // east (0..31)
// fraction (0..1) within grid square. x is north, y is east
float frac_x;
float frac_y;
// bit number within the 9x9 grid_block bitmap
uint8_t bitnum;
// fraction within the grid square
float frac_x; // north (0..1)
float frac_y; // east (0..1)
};
// given a location, fill a grid_info structure
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,
// heading and speed
AP_AHRS &ahrs;
// cache of grids in memory, LRU
struct {
uint32_t last_access_ms;
struct grid_block block;
} grid_cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
struct grid_cache cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
// last time we asked for more grids
uint32_t last_request_time_ms;
};
#endif // HAVE_AP_TERRAIN
#endif // __AP_TERRAIN_H__

Write Preview
Loading…
Cancel
Save