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_Filesystem/AP_Filesystem.h>
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include <AP_Common/AP_Common.h>
      26             : #include <AP_Math/AP_Math.h>
      27             : #include <stdio.h>
      28             : #include <GCS_MAVLink/GCS.h>
      29             : 
      30             : extern const AP_HAL::HAL& hal;
      31             : 
      32             : /*
      33             :   check for blocks that need to be read from disk
      34             :  */
      35     2214109 : void AP_Terrain::check_disk_read(void)
      36             : {
      37    28708970 :     for (uint16_t i=0; i<cache_size; i++) {
      38    26503818 :         if (cache[i].state == GRID_CACHE_DISKWAIT) {
      39        8957 :             disk_block.block = cache[i].grid;
      40        8957 :             disk_io_state = DiskIoWaitRead;
      41        8957 :             return;
      42             :         }
      43             :     }    
      44             : }
      45             : 
      46             : /*
      47             :   check for blocks that need to be written to disk
      48             :  */
      49     2205152 : void AP_Terrain::check_disk_write(void)
      50             : {
      51    28666976 :     for (uint16_t i=0; i<cache_size; i++) {
      52    26461824 :         if (cache[i].state == GRID_CACHE_DIRTY) {
      53           0 :             disk_block.block = cache[i].grid;
      54           0 :             disk_io_state = DiskIoWaitWrite;
      55           0 :             return;
      56             :         }
      57             :     }    
      58             : }
      59             : 
      60             : /*
      61             :   Check if we need to do disk IO for grids. 
      62             :  */
      63     2223524 : void AP_Terrain::schedule_disk_io(void)
      64             : {
      65     2223524 :     if (enable == 0 || !allocate() || diskless()) {
      66           0 :         return;
      67             :     }
      68             : 
      69     2223524 :     if (!timer_setup) {
      70        1691 :         timer_setup = true;
      71        1691 :         hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Terrain::io_timer, void));
      72             :     }
      73             : 
      74     2223524 :     switch (disk_io_state) {
      75     2214109 :     case DiskIoIdle:
      76             :         // look for a block that needs reading or writing
      77     2214109 :         check_disk_read();
      78     2214109 :         if (disk_io_state == DiskIoIdle) {
      79             :             // still idle, check for writes
      80     2205152 :             check_disk_write();            
      81             :         }
      82     2214109 :         break;
      83             :         
      84        8943 :     case DiskIoDoneRead: {
      85             :         // a read has completed
      86        8943 :         int16_t cache_idx = find_io_idx(GRID_CACHE_DISKWAIT);
      87        8943 :         if (cache_idx != -1) {
      88        8942 :             if (disk_block.block.bitmap != 0) {
      89             :                 // when bitmap is zero we read an empty block
      90        8358 :                 cache[cache_idx].grid = disk_block.block;
      91             :             }
      92        8942 :             cache[cache_idx].state = GRID_CACHE_VALID;
      93        8942 :             cache[cache_idx].last_access_ms = AP_HAL::millis();
      94             :         }
      95        8943 :         disk_io_state = DiskIoIdle;
      96        8943 :         break;
      97             :     }
      98             : 
      99           0 :     case DiskIoDoneWrite: {
     100             :         // a write has completed
     101           0 :         int16_t cache_idx = find_io_idx(GRID_CACHE_DIRTY);
     102           0 :         if (cache_idx != -1) {
     103           0 :             if (cache[cache_idx].grid.bitmap == disk_block.block.bitmap) {
     104             :                 // only mark valid if more grids haven't been added
     105           0 :                 cache[cache_idx].state = GRID_CACHE_VALID;
     106             :             }
     107             :         }
     108           0 :         disk_io_state = DiskIoIdle;
     109           0 :         break;
     110             :     }
     111             :         
     112         472 :     case DiskIoWaitWrite:
     113             :     case DiskIoWaitRead:
     114             :         // waiting for io_timer()
     115         472 :         break;
     116             :     }
     117             : }
     118             : 
     119             : 
     120             : /********************************************************
     121             : All the functions below this point run in the IO timer context, which
     122             : is a separate thread. The code uses the state machine controlled by
     123             : disk_io_state to manage who has access to the structures and to
     124             : prevent race conditions.
     125             : 
     126             : The IO timer context owns the data when disk_io_state is
     127             : DiskIoWaitWrite or DiskIoWaitRead. The main thread owns the data when
     128             : disk_io_state is DiskIoIdle, DiskIoDoneWrite or DiskIoDoneRead
     129             : 
     130             : All file operations are done by the IO thread.
     131             : *********************************************************/
     132             : 
     133             : 
     134             : /*
     135             :   open the current degree file
     136             :  */
     137        8957 : void AP_Terrain::open_file(void)
     138             : {
     139        8957 :     if (diskless()) {
     140           0 :         return;
     141             :     }
     142        8957 :     struct grid_block &block = disk_block.block;
     143        8957 :     if (fd != -1 && 
     144        7267 :         block.lat_degrees == file_lat_degrees &&
     145        7232 :         block.lon_degrees == file_lon_degrees) {
     146             :         // already open on right file
     147        7223 :         return;
     148             :     }
     149        1734 :     if (file_path == nullptr) {
     150        1690 :         const char* terrain_dir = hal.util->get_custom_terrain_directory();
     151        1690 :         if (terrain_dir == nullptr) {
     152        1690 :             terrain_dir = HAL_BOARD_TERRAIN_DIRECTORY;
     153             :         }
     154        1690 :         if (asprintf(&file_path, "%s/NxxExxx.DAT", terrain_dir) <= 0) {
     155           0 :             io_failure = true;
     156           0 :             file_path = nullptr;
     157           0 :             return;
     158             :         }
     159             :     }
     160        1734 :     if (file_path == nullptr) {
     161           0 :         io_failure = true;
     162           0 :         return;
     163             :     }
     164        1734 :     char *p = &file_path[strlen(file_path)-12];
     165        1734 :     if (*p != '/') {
     166           0 :         io_failure = true;
     167           0 :         return;        
     168             :     }
     169             :     // our fancy templatified MIN macro get gcc 9.3.0 all confused; it
     170             :     // thinks there are more digits than there can be so says there's
     171             :     // a buffer overflow in the snprintf.  Constrain it long-form:
     172        1734 :     uint32_t lat_tmp = abs((int32_t)block.lat_degrees);
     173        1734 :     if (lat_tmp > 99U) {
     174           0 :         lat_tmp = 99U;
     175             :     }
     176        1734 :     uint32_t lon_tmp = abs((int32_t)block.lon_degrees);
     177        1734 :     if (lon_tmp > 999U) {
     178           0 :         lon_tmp = 999;
     179             :     }
     180        3468 :     hal.util->snprintf(p, 13, "/%c%02u%c%03u.DAT",
     181        1734 :              block.lat_degrees<0?'S':'N',
     182             :              (unsigned)lat_tmp,
     183        1734 :              block.lon_degrees<0?'W':'E',
     184             :              (unsigned)lon_tmp);
     185             : 
     186             :     // create directory if need be
     187        1734 :     if (!directory_created) {
     188        1690 :         *p = 0;
     189        1690 :         directory_created = !AP::FS().mkdir(file_path);
     190        1690 :         *p = '/';
     191             : 
     192        1690 :         if (!directory_created) {
     193        1690 :             if (errno == EEXIST) {
     194             :                 // directory already existed
     195        1690 :                 directory_created = true;
     196             :             } else {
     197             :                 // if we didn't succeed at making the directory, then IO failed
     198           0 :                 io_failure = true;
     199           0 :                 return;
     200             :             }
     201             :         }
     202             :     }
     203             : 
     204        1734 :     if (fd != -1) {
     205          44 :         AP::FS().close(fd);
     206             :     }
     207        1734 :     fd = AP::FS().open(file_path, O_RDWR|O_CREAT);
     208        1734 :     if (fd == -1) {
     209             : #if TERRAIN_DEBUG
     210             :         hal.console->printf("Open %s failed - %s\n",
     211             :                             file_path, strerror(errno));
     212             : #endif
     213           0 :         io_failure = true;
     214           0 :         return;
     215             :     }
     216             : 
     217        1734 :     file_lat_degrees = block.lat_degrees;
     218        1734 :     file_lon_degrees = block.lon_degrees;
     219             : }
     220             : 
     221             : /*
     222             :   work out how many blocks needed in a stride for a given location
     223             :  */
     224        8957 : uint32_t AP_Terrain::east_blocks(struct grid_block &block) const
     225             : {
     226        8957 :     Location loc1, loc2;
     227        8957 :     loc1.lat = block.lat_degrees*10*1000*1000L;
     228        8957 :     loc1.lng = block.lon_degrees*10*1000*1000L;
     229        8957 :     loc2.lat = loc1.lat;
     230        8957 :     loc2.lng = (block.lon_degrees+1)*10*1000*1000L;
     231             : 
     232             :     // shift another two blocks east to ensure room is available
     233        8957 :     loc2.offset(0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
     234        8957 :     const Vector2f offset = loc1.get_distance_NE(loc2);
     235        8957 :     return offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SPACING_Y);
     236             : }
     237             : 
     238             : /*
     239             :   seek to the right offset for disk_block
     240             :  */
     241        8957 : void AP_Terrain::seek_offset(void)
     242             : {
     243        8957 :     struct grid_block &block = disk_block.block;
     244             :     // work out how many longitude blocks there are at this latitude
     245        8957 :     uint32_t blocknum = east_blocks(block) * block.grid_idx_x + block.grid_idx_y;
     246        8957 :     uint32_t file_offset = blocknum * sizeof(union grid_io_block);
     247        8957 :     if (AP::FS().lseek(fd, file_offset, SEEK_SET) != (off_t)file_offset) {
     248             : #if TERRAIN_DEBUG
     249             :         hal.console->printf("Seek %lu failed - %s\n",
     250             :                             (unsigned long)file_offset, strerror(errno));
     251             : #endif
     252           0 :         AP::FS().close(fd);
     253           0 :         fd = -1;
     254           0 :         io_failure = true;
     255             :     }
     256        8957 : }
     257             : 
     258             : /*
     259             :   write out disk_block
     260             :  */
     261           0 : void AP_Terrain::write_block(void)
     262             : {
     263           0 :     seek_offset();
     264           0 :     if (io_failure || diskless()) {
     265           0 :         return;
     266             :     }
     267             : 
     268           0 :     disk_block.block.crc = get_block_crc(disk_block.block);
     269             : 
     270           0 :     ssize_t ret = AP::FS().write(fd, &disk_block, sizeof(disk_block));
     271           0 :     if (ret  != sizeof(disk_block)) {
     272             : #if TERRAIN_DEBUG
     273             :         hal.console->printf("write failed - %s\n", strerror(errno));
     274             : #endif
     275           0 :         AP::FS().close(fd);
     276           0 :         fd = -1;
     277           0 :         io_failure = true;
     278             :     } else {
     279           0 :         AP::FS().fsync(fd);
     280             : #if TERRAIN_DEBUG
     281             :         printf("wrote block at %ld %ld ret=%d mask=%07llx\n",
     282             :                (long)disk_block.block.lat,
     283             :                (long)disk_block.block.lon,
     284             :                (int)ret,
     285             :                (unsigned long long)disk_block.block.bitmap);
     286             : #endif
     287             :     }
     288           0 :     disk_io_state = DiskIoDoneWrite;
     289             : }
     290             : 
     291             : /*
     292             :   read in disk_block
     293             :  */
     294        8957 : void AP_Terrain::read_block(void)
     295             : {
     296        8957 :     seek_offset();
     297        8957 :     if (io_failure || diskless()) {
     298           0 :         return;
     299             :     }
     300        8957 :     int32_t lat = disk_block.block.lat;
     301        8957 :     int32_t lon = disk_block.block.lon;
     302             : 
     303        8957 :     ssize_t ret = AP::FS().read(fd, &disk_block, sizeof(disk_block));
     304       17358 :     if (ret != sizeof(disk_block) || 
     305        8401 :         !TERRAIN_LATLON_EQUAL(disk_block.block.lat,lat) ||
     306        8369 :         !TERRAIN_LATLON_EQUAL(disk_block.block.lon,lon) ||
     307        8369 :         disk_block.block.bitmap == 0 ||
     308        8369 :         disk_block.block.spacing != grid_spacing ||
     309       25727 :         disk_block.block.version != TERRAIN_GRID_FORMAT_VERSION ||
     310        8369 :         disk_block.block.crc != get_block_crc(disk_block.block)) {
     311             : #if TERRAIN_DEBUG
     312             :         printf("read empty block at %ld %ld ret=%d (%ld %ld %u 0x%08lx) 0x%04x:0x%04x\n",
     313             :                (long)lat,
     314             :                (long)lon,
     315             :                (int)ret,
     316             :                (long)disk_block.block.lat,
     317             :                (long)disk_block.block.lon,
     318             :                (unsigned)disk_block.block.spacing,
     319             :                (unsigned long)disk_block.block.bitmap,
     320             :                (unsigned)disk_block.block.crc,
     321             :                (unsigned)get_block_crc(disk_block.block));
     322             : #endif
     323             :         // a short read or bad data is not an IO failure, just a
     324             :         // missing block on disk
     325         588 :         memset(&disk_block, 0, sizeof(disk_block));
     326         588 :         disk_block.block.lat = lat;
     327         588 :         disk_block.block.lon = lon;
     328         588 :         disk_block.block.bitmap = 0;
     329             :     } else {
     330             : #if TERRAIN_DEBUG
     331             :         printf("read block at %ld %ld ret=%d mask=%07llx\n",
     332             :                (long)lat,
     333             :                (long)lon,
     334             :                (int)ret,
     335             :                (unsigned long long)disk_block.block.bitmap);
     336             : #endif
     337        8369 :         if (disk_block.block.version_minor < TERRAIN_VERSION_MINOR_MIN) {
     338             :             /*
     339             :               this triggers a pre-arm warning to prompt the user to
     340             :               download new terrain data
     341             :              */
     342             : #if HAL_GCS_ENABLED
     343           0 :             if (!found_old_data && hal.util->get_soft_armed()) {
     344             :                 // in-flight warning for GCS operator
     345           0 :                 gcs().send_text(MAV_SEVERITY_WARNING, "terrain data expired, possible errors");
     346             :             }
     347             : #endif
     348           0 :             found_old_data = true;
     349             :         }
     350             :     }
     351        8957 :     disk_io_state = DiskIoDoneRead;
     352             : }
     353             : 
     354             : /*
     355             :   timer called to do disk IO
     356             :  */
     357   207676987 : void AP_Terrain::io_timer(void)
     358             : {
     359   207676987 :     if (io_failure) {
     360             :         // retry the IO every 5s to allow for remount of sdcard
     361           0 :         uint32_t now = AP_HAL::millis();
     362           0 :         if (now - last_retry_ms > 5000) {
     363           0 :             io_failure = false;
     364           0 :             last_retry_ms = now;
     365             :         }
     366           0 :         return;
     367             :     }
     368             : 
     369   207676987 :     update_reference_offset();
     370             : 
     371   207676987 :     switch (disk_io_state) {
     372   207668030 :     case DiskIoIdle:
     373             :     case DiskIoDoneRead:
     374             :     case DiskIoDoneWrite:
     375             :         // nothing to do
     376   207668030 :         break;
     377             :         
     378           0 :     case DiskIoWaitWrite:
     379             :         // need to write out the block
     380           0 :         open_file();
     381           0 :         if (fd == -1) {
     382           0 :             return;
     383             :         }
     384           0 :         write_block();
     385           0 :         break;
     386             : 
     387        8957 :     case DiskIoWaitRead:
     388             :         // need to read in the block
     389        8957 :         open_file();
     390        8957 :         if (fd == -1) {
     391           0 :             return;
     392             :         }
     393        8957 :         read_block();
     394        8957 :         break;
     395             :     }
     396             : }
     397             : 
     398             : #endif // AP_TERRAIN_AVAILABLE