Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   handle disk IO for terrain code
      17             :  */
      18             : 
      19             : #include "AP_Terrain.h"
      20             : 
      21             : #if AP_TERRAIN_AVAILABLE
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : #include <AP_Common/AP_Common.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : /*
      30             :   calculate bit number in grid_block bitmap. This corresponds to a
      31             :   bit representing a 4x4 mavlink transmitted block
      32             : */
      33  3785770030 : uint8_t AP_Terrain::grid_bitnum(uint8_t idx_x, uint8_t idx_y)
      34             : {    
      35             :     ASSERT_RANGE(idx_x,0,27);
      36             :     ASSERT_RANGE(idx_y,0,31);
      37  3785770030 :     uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE;
      38  3785770030 :     uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE;
      39             :     ASSERT_RANGE(subgrid_x,0,TERRAIN_GRID_BLOCK_MUL_X-1);
      40             :     ASSERT_RANGE(subgrid_y,0,TERRAIN_GRID_BLOCK_MUL_Y-1);
      41  3785770030 :     return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x;
      42             : }
      43             : 
      44             : /*
      45             :   given a grid_info check that a given idx_x/idx_y is available (set
      46             :   in the bitmap)
      47             :  */
      48  3785770092 : bool AP_Terrain::check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y)
      49             : {
      50  3785770092 :     uint8_t bitnum = grid_bitnum(idx_x, idx_y);
      51  3785770405 :     return (grid.bitmap & (((uint64_t)1U)<<bitnum)) != 0;
      52             : }
      53             : 
      54             : /*
      55             :   given a location, calculate the 32x28 grid SW corner, plus the
      56             :   grid indices
      57             : */
      58  1008129292 : void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const
      59             : {
      60             :     // grids start on integer degrees. This makes storing terrain data
      61             :     // on the SD card a bit easier
      62  1008129292 :     info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
      63  1008129292 :     info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L);
      64             : 
      65             :     // create reference position for this rounded degree position
      66  1008129292 :     Location ref;
      67  1008129219 :     ref.lat = info.lat_degrees*10*1000*1000L;
      68  1008129219 :     ref.lng = info.lon_degrees*10*1000*1000L;
      69             : 
      70             :     // find offset from reference
      71  1008129219 :     const Vector2f offset = ref.get_distance_NE(loc);
      72             : 
      73             :     // get indices in terms of grid_spacing elements
      74  1008128765 :     uint32_t idx_x = offset.x / grid_spacing;
      75  1008128559 :     uint32_t idx_y = offset.y / grid_spacing;
      76             : 
      77             :     // find indexes into 32*28 grids for this degree reference. Note
      78             :     // the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
      79             :     // overlap between grids
      80  1008128469 :     info.grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X;
      81  1008128469 :     info.grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y;
      82             : 
      83             :     // find the indices within the 32*28 grid
      84  1008128469 :     info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X;
      85  1008128469 :     info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;
      86             : 
      87             :     // find the fraction (0..1) within the square
      88  1008128469 :     info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing;
      89  1008128409 :     info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing;
      90             : 
      91             :     // calculate lat/lon of SW corner of 32*28 grid_block
      92  1008128333 :     ref.offset(info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing,
      93  1008128433 :                info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing);
      94  1008128257 :     info.grid_lat = ref.lat;
      95  1008128257 :     info.grid_lon = ref.lng;
      96             : 
      97             :     ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
      98             :     ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1);
      99             :     ASSERT_RANGE(info.frac_x,0,1);
     100             :     ASSERT_RANGE(info.frac_y,0,1);
     101  1008128257 : }
     102             : 
     103             : 
     104             : /*
     105             :   find a grid structure given a grid_info
     106             :  */
     107  1008926189 : AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info)
     108             : {
     109  1008926189 :     uint16_t oldest_i = 0;
     110             : 
     111             :     // see if we have that grid
     112  1008926189 :     const auto now_ms = AP_HAL::millis();
     113  1427802942 :     for (uint16_t i=0; i<cache_size; i++) {
     114  1427791413 :         if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,info.grid_lat) &&
     115  2436708562 :             TERRAIN_LATLON_EQUAL(cache[i].grid.lon,info.grid_lon) &&
     116  1008914189 :             cache[i].grid.spacing == grid_spacing) {
     117  1008917410 :             cache[i].last_access_ms = now_ms;
     118  1008917410 :             return cache[i];
     119             :         }
     120   418877098 :         if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
     121    77785620 :             oldest_i = i;
     122             :         }
     123             :     }
     124             : 
     125             :     // Not found. Use the oldest grid and make it this grid,
     126             :     // initially unpopulated
     127       11529 :     struct grid_cache &grid = cache[oldest_i];
     128       11529 :     memset(&grid, 0, sizeof(grid));
     129             : 
     130       11529 :     grid.grid.lat = info.grid_lat;
     131       11529 :     grid.grid.lon = info.grid_lon;
     132       11529 :     grid.grid.spacing = grid_spacing;
     133        9039 :     grid.grid.grid_idx_x = info.grid_idx_x;
     134        9039 :     grid.grid.grid_idx_y = info.grid_idx_y;
     135        9039 :     grid.grid.lat_degrees = info.lat_degrees;
     136        9039 :     grid.grid.lon_degrees = info.lon_degrees;
     137        9039 :     grid.grid.version = TERRAIN_GRID_FORMAT_VERSION;
     138        9039 :     grid.grid.version_minor = TERRAIN_VERSION_MINOR_MIN;
     139        9039 :     grid.last_access_ms = now_ms;
     140             : 
     141             :     // mark as waiting for disk read
     142        9039 :     grid.state = GRID_CACHE_DISKWAIT;
     143             : 
     144        9039 :     return grid;
     145             : }
     146             : 
     147             : /*
     148             :   find cache index of disk_block
     149             :  */
     150        8943 : int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
     151             : {
     152             :     // try first with given state
     153       41940 :     for (uint16_t i=0; i<cache_size; i++) {
     154       41939 :         if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
     155       50881 :             TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon) &&
     156        8942 :             cache[i].state == state) {
     157        8942 :             return i;
     158             :         }
     159             :     }    
     160             :     // then any state
     161          13 :     for (uint16_t i=0; i<cache_size; i++) {
     162          12 :         if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
     163           0 :             TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon)) {
     164           0 :             return i;
     165             :         }
     166             :     }    
     167           1 :     return -1;
     168             : }
     169             : 
     170             : /*
     171             :   get CRC for a block
     172             :  */
     173        8369 : uint16_t AP_Terrain::get_block_crc(struct grid_block &block)
     174             : {
     175        8369 :     uint16_t saved_crc = block.crc;
     176        8369 :     block.crc = 0;
     177             :     /*
     178             :       note that we exclude version_minor and any later bytes from the
     179             :       CRC to maintain backwards compatibility so old versions of
     180             :       ArduPilot accept new terrain blocks
     181             :      */
     182        8369 :     uint16_t ret = crc16_ccitt((const uint8_t *)&block, offsetof(struct grid_block, version_minor), 0);
     183        8369 :     block.crc = saved_crc;
     184        8369 :     return ret;
     185             : }
     186             : 
     187             : #endif // AP_TERRAIN_AVAILABLE