From 0d26252bdbe15ae1670fc81dd2f1885bf36b91d0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 23 Sep 2015 08:31:17 +1000
Subject: [PATCH] AP_Terrain: don't allocate cache array when terrain not
 enabled

this makes it easy to save 22k of ram when running other experiments
---
 libraries/AP_Terrain/AP_Terrain.cpp  | 26 ++++++++++++++++++++++++--
 libraries/AP_Terrain/AP_Terrain.h    |  5 +++--
 libraries/AP_Terrain/TerrainGCS.cpp  | 10 +++++-----
 libraries/AP_Terrain/TerrainIO.cpp   |  6 +++---
 libraries/AP_Terrain/TerrainUtil.cpp |  6 +++---
 5 files changed, 38 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp
index 7159bda9ca..421f8ec5b6 100644
--- a/libraries/AP_Terrain/AP_Terrain.cpp
+++ b/libraries/AP_Terrain/AP_Terrain.cpp
@@ -85,7 +85,7 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission, const AP_Rall
  */
 bool AP_Terrain::height_amsl(const Location &loc, float &height)
 {
-    if (!enable) {
+    if (!enable || !allocate()) {
         return false;
     }
 
@@ -250,7 +250,7 @@ bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude,
 */
 float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
 {
-    if (!enable || grid_spacing <= 0) {
+    if (!enable || !allocate() || grid_spacing <= 0) {
         return 0;
     }
 
@@ -378,4 +378,26 @@ void AP_Terrain::log_terrain_data(DataFlash_Class &dataflash)
     dataflash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+/*
+  allocate terrain cache. Making this dynamically allocated allows
+  memory to be saved when terrain functionality is disabled
+ */
+bool AP_Terrain::allocate(void)
+{
+    if (enable == 0) {
+        return false;
+    }
+    if (cache != nullptr) {
+        return true;
+    }
+    cache = (struct grid_cache *)calloc(TERRAIN_GRID_BLOCK_CACHE_SIZE, sizeof(cache[0]));
+    if (cache == nullptr) {
+        enable.set(0);
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("Terrain: allocation failed"));
+        return false;
+    }
+    cache_size = TERRAIN_GRID_BLOCK_CACHE_SIZE;
+    return true;
+}
+
 #endif // AP_TERRAIN_AVAILABLE
diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index 91e5a48f0a..fdd15c5a9e 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -167,7 +167,7 @@ public:
 
 private:
     // allocate the terrain subsystem data
-    void allocate(void);
+    bool allocate(void);
 
     /*
       a grid block is a structure in a local file containing height
@@ -338,7 +338,8 @@ private:
     const AP_Rally &rally;
 
     // cache of grids in memory, LRU
-    struct grid_cache cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
+    uint8_t cache_size = 0;
+    struct grid_cache *cache = nullptr;
 
     // a grid_cache block waiting for disk IO
     enum DiskIoState {
diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp
index c3361d5c8d..5f4e5bb73b 100644
--- a/libraries/AP_Terrain/TerrainGCS.cpp
+++ b/libraries/AP_Terrain/TerrainGCS.cpp
@@ -84,7 +84,7 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info
  */
 void AP_Terrain::send_request(mavlink_channel_t chan)
 {
-    if (enable == 0) {
+    if (enable == 0 || !allocate()) {
         // not enabled
         return;
     }
@@ -131,7 +131,7 @@ void AP_Terrain::send_request(mavlink_channel_t chan)
     }
 
     // check cache blocks that may have been setup by a TERRAIN_CHECK
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (cache[i].state >= GRID_CACHE_VALID) {
             if (request_missing(chan, cache[i])) {
                 return;
@@ -161,7 +161,7 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
 {
     pending = 0;
     loaded = 0;
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (cache[i].grid.spacing != grid_spacing) {
             continue;
         }
@@ -261,7 +261,7 @@ void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
     mavlink_msg_terrain_data_decode(msg, &packet);
 
     uint16_t i;
-    for (i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (i=0; i<cache_size; i++) {
         if (cache[i].grid.lat == packet.lat && 
             cache[i].grid.lon == packet.lon && 
             cache[i].grid.spacing == packet.grid_spacing &&
@@ -270,7 +270,7 @@ void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
             break;
         }
     }
-    if (i == TERRAIN_GRID_BLOCK_CACHE_SIZE) {
+    if (i == cache_size) {
         // we don't have that grid, ignore data
         return;
     }
diff --git a/libraries/AP_Terrain/TerrainIO.cpp b/libraries/AP_Terrain/TerrainIO.cpp
index 5b98f36606..b6a72da34e 100644
--- a/libraries/AP_Terrain/TerrainIO.cpp
+++ b/libraries/AP_Terrain/TerrainIO.cpp
@@ -41,7 +41,7 @@ extern const AP_HAL::HAL& hal;
  */
 void AP_Terrain::check_disk_read(void)
 {
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (cache[i].state == GRID_CACHE_DISKWAIT) {
             disk_block.block = cache[i].grid;
             disk_io_state = DiskIoWaitRead;
@@ -55,7 +55,7 @@ void AP_Terrain::check_disk_read(void)
  */
 void AP_Terrain::check_disk_write(void)
 {
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (cache[i].state == GRID_CACHE_DIRTY) {
             disk_block.block = cache[i].grid;
             disk_io_state = DiskIoWaitWrite;
@@ -69,7 +69,7 @@ void AP_Terrain::check_disk_write(void)
  */
 void AP_Terrain::schedule_disk_io(void)
 {
-    if (enable == 0) {
+    if (enable == 0 || !allocate()) {
         return;
     }
 
diff --git a/libraries/AP_Terrain/TerrainUtil.cpp b/libraries/AP_Terrain/TerrainUtil.cpp
index 96d94751c1..0de487179e 100644
--- a/libraries/AP_Terrain/TerrainUtil.cpp
+++ b/libraries/AP_Terrain/TerrainUtil.cpp
@@ -120,7 +120,7 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info
     uint16_t oldest_i = 0;
 
     // see if we have that grid
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (cache[i].grid.lat == info.grid_lat && 
             cache[i].grid.lon == info.grid_lon &&
             cache[i].grid.spacing == grid_spacing) {
@@ -159,7 +159,7 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info
 int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
 {
     // try first with given state
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (disk_block.block.lat == cache[i].grid.lat &&
             disk_block.block.lon == cache[i].grid.lon && 
             cache[i].state == state) {
@@ -167,7 +167,7 @@ int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
         }
     }    
     // then any state
-    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+    for (uint16_t i=0; i<cache_size; i++) {
         if (disk_block.block.lat == cache[i].grid.lat &&
             disk_block.block.lon == cache[i].grid.lon) {
             return i;