mirror of https://github.com/ArduPilot/ardupilot
341 lines
10 KiB
C++
341 lines
10 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
handle vehicle <-> GCS communications for terrain library
|
|
*/
|
|
|
|
#include "AP_Terrain.h"
|
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/*
|
|
request any missing 4x4 grids from a block, given a grid_cache
|
|
*/
|
|
bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcache)
|
|
{
|
|
struct grid_block &grid = gcache.grid;
|
|
|
|
if (options.get() & uint16_t(Options::DisableDownload)) {
|
|
return false;
|
|
}
|
|
|
|
if (grid.spacing != grid_spacing) {
|
|
// an invalid grid
|
|
return false;
|
|
}
|
|
|
|
// see if we are waiting for disk read
|
|
if (gcache.state == GRID_CACHE_DISKWAIT) {
|
|
// don't request data from the GCS till we know it's not on disk
|
|
return false;
|
|
}
|
|
|
|
// see if it is fully populated
|
|
if ((grid.bitmap & bitmap_mask) == bitmap_mask) {
|
|
// it is fully populated, nothing to do
|
|
return false;
|
|
}
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, TERRAIN_REQUEST)) {
|
|
// not enough buffer space
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
ask the GCS to send a set of 4x4 grids
|
|
*/
|
|
mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap);
|
|
last_request_time_ms[chan] = AP_HAL::millis();
|
|
|
|
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_cache &gcache = find_grid_cache(info);
|
|
return request_missing(chan, gcache);
|
|
}
|
|
|
|
/*
|
|
send any pending cache requests
|
|
*/
|
|
bool AP_Terrain::send_cache_request(mavlink_channel_t chan)
|
|
{
|
|
for (uint16_t i=0; i<cache_size; i++) {
|
|
if (cache[i].state >= GRID_CACHE_VALID) {
|
|
if (request_missing(chan, cache[i])) {
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
send any pending terrain request to the GCS
|
|
*/
|
|
void AP_Terrain::send_request(mavlink_channel_t chan)
|
|
{
|
|
if (!allocate()) {
|
|
// not enabled
|
|
return;
|
|
}
|
|
|
|
// see if we need to schedule some disk IO
|
|
schedule_disk_io();
|
|
|
|
Location loc;
|
|
if (!AP::ahrs().get_location(loc)) {
|
|
// we don't know where we are. Send a report and request any cached blocks.
|
|
// this allows for download of mission items when we have no GPS lock
|
|
loc = {};
|
|
send_terrain_report(chan, loc, true);
|
|
send_cache_request(chan);
|
|
return;
|
|
}
|
|
|
|
// always send a terrain report
|
|
send_terrain_report(chan, loc, true);
|
|
|
|
// did we request recently?
|
|
if (AP_HAL::millis() - last_request_time_ms[chan] < 2000) {
|
|
// too soon to request again
|
|
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;
|
|
}
|
|
|
|
// also request a larger set of up to 9 grids
|
|
for (int8_t x=-1; x<=1; x++) {
|
|
for (int8_t y=-1; y<=1; y++) {
|
|
Location loc2 = loc;
|
|
loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing,
|
|
y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing);
|
|
struct grid_info info2;
|
|
calculate_grid_info(loc2, info2);
|
|
if (request_missing(chan, info2)) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
// check cache blocks that may have been setup by a TERRAIN_CHECK
|
|
if (send_cache_request(chan)) {
|
|
return;
|
|
}
|
|
|
|
// request the current loc last to ensure it has highest last
|
|
// access time
|
|
if (request_missing(chan, info)) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
/*
|
|
count bits in a uint64_t
|
|
*/
|
|
uint8_t AP_Terrain::bitcount64(uint64_t b) const
|
|
{
|
|
return __builtin_popcount((unsigned)(b&0xFFFFFFFF)) + __builtin_popcount((unsigned)(b>>32));
|
|
}
|
|
|
|
/*
|
|
get some statistics for TERRAIN_REPORT
|
|
*/
|
|
void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded) const
|
|
{
|
|
pending = 0;
|
|
loaded = 0;
|
|
for (uint16_t i=0; i<cache_size; i++) {
|
|
if (cache[i].grid.spacing != grid_spacing) {
|
|
continue;
|
|
}
|
|
if (cache[i].state == GRID_CACHE_INVALID) {
|
|
continue;
|
|
}
|
|
uint8_t maskbits = TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y;
|
|
if (cache[i].state == GRID_CACHE_DISKWAIT) {
|
|
pending += maskbits;
|
|
continue;
|
|
}
|
|
if (cache[i].state == GRID_CACHE_DIRTY) {
|
|
// count dirty grids as a pending, so we know where there
|
|
// are disk writes pending
|
|
pending++;
|
|
}
|
|
uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
|
|
pending += maskbits - bitcount;
|
|
loaded += bitcount;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
handle terrain messages from GCS
|
|
*/
|
|
void AP_Terrain::handle_data(mavlink_channel_t chan, const mavlink_message_t &msg)
|
|
{
|
|
if (msg.msgid == MAVLINK_MSG_ID_TERRAIN_DATA) {
|
|
handle_terrain_data(msg);
|
|
} else if (msg.msgid == MAVLINK_MSG_ID_TERRAIN_CHECK) {
|
|
handle_terrain_check(chan, msg);
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
send a TERRAIN_REPORT for a location
|
|
*/
|
|
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate)
|
|
{
|
|
float terrain_height = 0;
|
|
float home_terrain_height = 0;
|
|
uint16_t spacing = 0;
|
|
Location current_loc;
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
|
if (ahrs.get_location(current_loc) &&
|
|
height_amsl(ahrs.get_home(), home_terrain_height, false) &&
|
|
height_amsl(loc, terrain_height, false)) {
|
|
// non-zero spacing indicates we have data
|
|
spacing = grid_spacing;
|
|
} else if (extrapolate && have_current_loc_height) {
|
|
// show the extrapolated height, so logs show what height is
|
|
// being used for navigation
|
|
terrain_height = last_current_loc_height;
|
|
} else {
|
|
// report terrain height if we can, but can't give current_height
|
|
height_amsl(loc, terrain_height, false);
|
|
}
|
|
uint16_t pending, loaded;
|
|
get_statistics(pending, loaded);
|
|
|
|
float current_height;
|
|
if (spacing == 0 && !(extrapolate && have_current_loc_height)) {
|
|
current_height = 0;
|
|
} else {
|
|
if (current_loc.relative_alt) {
|
|
current_height = current_loc.alt*0.01f;
|
|
} else {
|
|
current_height = (current_loc.alt - ahrs.get_home().alt)*0.01f;
|
|
}
|
|
}
|
|
current_height += home_terrain_height - terrain_height;
|
|
|
|
if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) {
|
|
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing,
|
|
terrain_height, current_height,
|
|
pending, loaded);
|
|
}
|
|
}
|
|
|
|
/*
|
|
handle TERRAIN_CHECK messages from GCS
|
|
*/
|
|
void AP_Terrain::handle_terrain_check(mavlink_channel_t chan, const mavlink_message_t &msg)
|
|
{
|
|
mavlink_terrain_check_t packet;
|
|
mavlink_msg_terrain_check_decode(&msg, &packet);
|
|
Location loc;
|
|
loc.lat = packet.lat;
|
|
loc.lng = packet.lon;
|
|
send_terrain_report(chan, loc, false);
|
|
}
|
|
|
|
/*
|
|
handle TERRAIN_DATA messages from GCS
|
|
*/
|
|
void AP_Terrain::handle_terrain_data(const mavlink_message_t &msg)
|
|
{
|
|
mavlink_terrain_data_t packet;
|
|
mavlink_msg_terrain_data_decode(&msg, &packet);
|
|
|
|
uint16_t i;
|
|
for (i=0; i<cache_size; i++) {
|
|
if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,packet.lat) &&
|
|
TERRAIN_LATLON_EQUAL(cache[i].grid.lon,packet.lon) &&
|
|
cache[i].grid.spacing == packet.grid_spacing &&
|
|
grid_spacing == packet.grid_spacing &&
|
|
packet.gridbit < 56) {
|
|
break;
|
|
}
|
|
}
|
|
if (i == 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];
|
|
}
|
|
}
|
|
gcache.grid.bitmap |= ((uint64_t)1) << packet.gridbit;
|
|
|
|
// mark dirty for disk IO
|
|
gcache.state = GRID_CACHE_DIRTY;
|
|
|
|
#if TERRAIN_DEBUG
|
|
hal.console->printf("Filled bit %u idx_x=%u idx_y=%u\n",
|
|
(unsigned)packet.gridbit, (unsigned)idx_x, (unsigned)idx_y);
|
|
if (gcache.grid.bitmap == bitmap_mask) {
|
|
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
|
grid.lat*1.0e-7f,
|
|
grid.lon*1.0e-7f,
|
|
grid.height[0][0]);
|
|
Location loc2;
|
|
loc2.lat = grid.lat;
|
|
loc2.lng = grid.lon;
|
|
loc2.offset(28*grid_spacing, 32*grid_spacing);
|
|
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
|
loc2.lat*1.0e-7f,
|
|
loc2.lng*1.0e-7f,
|
|
grid.height[27][31]);
|
|
}
|
|
#endif
|
|
|
|
// see if we need to schedule some disk IO
|
|
update();
|
|
}
|
|
|
|
|
|
#endif // AP_TERRAIN_AVAILABLE
|