Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_Terrain: allocate cache as block linked list instead of array

Browse Source 
It is difficult to find enough contiguous free heap space for a large
terrain cache when it is allocated as one large array. This is
especially problematic on STM32 with the variety of heaps across
different RAM regions.

The cache is only accessed by iterating through its blocks in order.
Therefore, using a singly-linked list of blocks is a natural fit as
there is no random access to penalize. The blocks are then allocated
independently so they can be allocated in different areas or regions as
heap availability dictates. There is an order-1% space overhead for the
`next` pointer and extra heap block headers.

This also fixes memory corruption if the user gives a zero or negative
cache size parameter then enables terrain.
This commit is contained in:
Thomas Watson
2026-09-22 12:03:24 +10:00
committed by Andrew Tridgell
parent f271ebddde
commit 002e8da59a
5 changed files with 79 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Terrain/AP_Terrain.cpp
+22 -7
View File
@@ -81,7 +81,7 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
// @Param: CACHE_SZ
// @DisplayName: Terrain cache size
// @Description: The number of 32x28 cache blocks to keep in memory. Each block uses about 1800 bytes of memory
// @Range: 0 128
// @Range: 1 128
// @User: Advanced
AP_GROUPINFO("CACHE_SZ", 5, AP_Terrain, config_cache_size, TERRAIN_GRID_BLOCK_CACHE_SIZE),
@@ -509,13 +509,28 @@ void AP_Terrain::log_terrain_data()
*/
void AP_Terrain::allocate(void)
{
cache = (struct grid_cache *)calloc(config_cache_size, sizeof(cache[0]));
if (cache == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terrain: Allocation failed");
memory_alloc_failed = true;
return;
struct grid_cache *next = nullptr;
int cache_size = config_cache_size;
for (auto i=0; i<cache_size; i++) {
struct grid_cache *head = (struct grid_cache *)calloc(1, sizeof(struct grid_cache));
if (head == nullptr) {
memory_alloc_failed = true;
break;
}
head->next = next;
next = head;
}
if (memory_alloc_failed || !next) { // fail if error or we never tried to allocate anything
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terrain: Allocation failed");
while (next != nullptr) { // don't leak what we already allocated
struct grid_cache *head = next;
next = head->next;
free(head);
}
memory_alloc_failed = true;
} else { // success! make available for use
cache = next;
}
cache_size = config_cache_size;
}
/*
libraries/AP_Terrain/AP_Terrain.h
+3 -2
View File
@@ -289,6 +289,8 @@ private:
// the last time access was requested to this block, used for LRU
uint32_t last_access_ms;
struct grid_cache *next; // must be last member for safe clearing
};
/*
@@ -361,7 +363,7 @@ private:
/*
disk IO functions
*/
int16_t find_io_idx(enum GridCacheState state);
AP_Terrain::grid_cache *find_io_cache(enum GridCacheState state);
uint16_t get_block_crc(struct grid_block &block);
void check_disk_read(void);
void check_disk_write(void);
@@ -414,7 +416,6 @@ private:
}
// cache of grids in memory, LRU
uint8_t cache_size = 0;
struct grid_cache *cache = nullptr;
// a grid_cache block waiting for disk IO
libraries/AP_Terrain/TerrainGCS.cpp
+16 -16
View File
@@ -95,9 +95,9 @@ bool AP_Terrain::request_missing(GCS_MAVLINK &link, const struct grid_info &info
*/
bool AP_Terrain::send_cache_request(GCS_MAVLINK &link)
{
for (uint16_t i=0; i<cache_size; i++) {
if (cache[i].state >= GRID_CACHE_VALID) {
if (request_missing(link, cache[i])) {
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (curr->state >= GRID_CACHE_VALID) {
if (request_missing(link, *curr)) {
return true;
}
}
@@ -170,24 +170,24 @@ 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) {
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (curr->grid.spacing != grid_spacing) {
continue;
}
if (cache[i].state == GRID_CACHE_INVALID) {
if (curr->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) {
if (curr->state == GRID_CACHE_DISKWAIT) {
pending += maskbits;
continue;
}
if (cache[i].state == GRID_CACHE_DIRTY) {
if (curr->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);
uint8_t bitcount = bitcount64(curr->grid.bitmap);
pending += maskbits - bitcount;
loaded += bitcount;
}
@@ -283,21 +283,21 @@ 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 &&
struct grid_cache *curr;
for (curr = cache; curr != nullptr; curr = curr->next) {
if (TERRAIN_LATLON_EQUAL(curr->grid.lat,packet.lat) &&
TERRAIN_LATLON_EQUAL(curr->grid.lon,packet.lon) &&
curr->grid.spacing == packet.grid_spacing &&
grid_spacing == packet.grid_spacing &&
packet.gridbit < 56) {
break;
}
}
if (i == cache_size) {
if (curr == nullptr) {
// we don't have that grid, ignore data
return;
}
struct grid_cache &gcache = cache[i];
struct grid_cache &gcache = *curr;
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;
libraries/AP_Terrain/TerrainIO.cpp
+15 -15
View File
@@ -34,9 +34,9 @@ extern const AP_HAL::HAL& hal;
*/
void AP_Terrain::check_disk_read(void)
{
for (uint16_t i=0; i<cache_size; i++) {
if (cache[i].state == GRID_CACHE_DISKWAIT) {
disk_block.block = cache[i].grid;
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (curr->state == GRID_CACHE_DISKWAIT) {
disk_block.block = curr->grid;
disk_io_state = DiskIoWaitRead;
return;
}
@@ -48,9 +48,9 @@ void AP_Terrain::check_disk_read(void)
*/
void AP_Terrain::check_disk_write(void)
{
for (uint16_t i=0; i<cache_size; i++) {
if (cache[i].state == GRID_CACHE_DIRTY) {
disk_block.block = cache[i].grid;
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (curr->state == GRID_CACHE_DIRTY) {
disk_block.block = curr->grid;
disk_io_state = DiskIoWaitWrite;
return;
}
@@ -83,14 +83,14 @@ void AP_Terrain::schedule_disk_io(void)
case DiskIoDoneRead: {
// a read has completed
int16_t cache_idx = find_io_idx(GRID_CACHE_DISKWAIT);
if (cache_idx != -1) {
struct grid_cache *entry = find_io_cache(GRID_CACHE_DISKWAIT);
if (entry != nullptr) {
if (disk_block.block.bitmap != 0) {
// when bitmap is zero we read an empty block
cache[cache_idx].grid = disk_block.block;
entry->grid = disk_block.block;
}
cache[cache_idx].state = GRID_CACHE_VALID;
cache[cache_idx].last_access_ms = AP_HAL::millis();
entry->state = GRID_CACHE_VALID;
entry->last_access_ms = AP_HAL::millis();
}
disk_io_state = DiskIoIdle;
break;
@@ -98,11 +98,11 @@ void AP_Terrain::schedule_disk_io(void)
case DiskIoDoneWrite: {
// a write has completed
int16_t cache_idx = find_io_idx(GRID_CACHE_DIRTY);
if (cache_idx != -1) {
if (cache[cache_idx].grid.bitmap == disk_block.block.bitmap) {
struct grid_cache *entry = find_io_cache(GRID_CACHE_DIRTY);
if (entry != nullptr) {
if (entry->grid.bitmap == disk_block.block.bitmap) {
// only mark valid if more grids haven't been added
cache[cache_idx].state = GRID_CACHE_VALID;
entry->state = GRID_CACHE_VALID;
}
}
disk_io_state = DiskIoIdle;
libraries/AP_Terrain/TerrainUtil.cpp
+23 -23
View File
@@ -106,26 +106,26 @@ void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info
*/
AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info)
{
uint16_t oldest_i = 0;
struct grid_cache *oldest = cache;
// see if we have that grid
const auto now_ms = AP_HAL::millis();
for (uint16_t i=0; i<cache_size; i++) {
if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,info.grid_lat) &&
TERRAIN_LATLON_EQUAL(cache[i].grid.lon,info.grid_lon) &&
cache[i].grid.spacing == grid_spacing) {
cache[i].last_access_ms = now_ms;
return cache[i];
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (TERRAIN_LATLON_EQUAL(curr->grid.lat,info.grid_lat) &&
TERRAIN_LATLON_EQUAL(curr->grid.lon,info.grid_lon) &&
curr->grid.spacing == grid_spacing) {
curr->last_access_ms = now_ms;
return *curr;
}
if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
oldest_i = i;
if (curr->last_access_ms < oldest->last_access_ms) {
oldest = curr;
}
}
// Not found. Use the oldest grid and make it this grid,
// initially unpopulated
struct grid_cache &grid = cache[oldest_i];
memset(&grid, 0, sizeof(grid));
struct grid_cache &grid = *oldest;
memset(&grid, 0, offsetof(struct grid_cache, next));
grid.grid.lat = info.grid_lat;
grid.grid.lon = info.grid_lon;
@@ -145,26 +145,26 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info
}
/*
find cache index of disk_block
find cache entry of disk_block
*/
int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
AP_Terrain::grid_cache *AP_Terrain::find_io_cache(enum GridCacheState state)
{
// try first with given state
for (uint16_t i=0; i<cache_size; i++) {
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon) &&
cache[i].state == state) {
return i;
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,curr->grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,curr->grid.lon) &&
curr->state == state) {
return curr;
}
}
// then any state
for (uint16_t i=0; i<cache_size; i++) {
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon)) {
return i;
for (struct grid_cache *curr = cache; curr != nullptr; curr = curr->next) {
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,curr->grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,curr->grid.lon)) {
return curr;
}
}
return -1;
return nullptr;
}
/*