/*
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 .
*/
/*
handle vehicle <-> GCS communications for terrain library
*/
#include "AP_Terrain.h"
#if AP_TERRAIN_AVAILABLE
#include
#include
#include
#include
#include
#include
#include
#include
extern const AP_HAL::HAL& hal;
#if HAL_GCS_ENABLED
/*
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= 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. Request any cached blocks.
// this allows for download of mission items when we have no GPS lock
send_cache_request(chan);
return;
}
// 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;
}
// check cache blocks that may have been setup by a TERRAIN_CHECK,
// mission items, rally items, squares surrounding our current
// location, favourite holiday destination, scripting, height
// reference location, ....
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;
}
}
#endif // HAL_GCS_ENABLED
/*
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; iprintf("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 // HAL_GCS_ENABLED
#endif // AP_TERRAIN_AVAILABLE