/* 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 #include #include #include #include #include "AP_Terrain.h" #if AP_TERRAIN_AVAILABLE #include #include 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 (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; location_offset(loc2, 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= 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) { 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) { pending = 0; loaded = 0; for (uint16_t i=0; imsgid == 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.flags.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, 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(mavlink_message_t *msg) { mavlink_terrain_data_t packet; mavlink_msg_terrain_data_decode(msg, &packet); uint16_t i; for (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; location_offset(loc2, 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