/* 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 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_position(loc)) { // we don't know where we are 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 for (uint16_t i=0; i<cache_size; i++) { if (cache[i].state >= GRID_CACHE_VALID) { if (request_missing(chan, cache[i])) { 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_position(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; } 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