From 73c52346336b1518fb5512d65432ab8d988b88ce Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Thu, 15 Oct 2015 10:23:31 -0700
Subject: [PATCH] AP_Terrain: cache status

instead of computing the terrain status on-demand, assign it in update() and cache the result. Then external tasks that check the status won't be doing terrain intensive calculations in their thread. All the calculations needed for the status were being performed in update already so this is an optimization.
---
 libraries/AP_Terrain/AP_Terrain.cpp | 36 +++++++++++------------------
 libraries/AP_Terrain/AP_Terrain.h   |  5 +++-
 2 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp
index b7eb63d31f..bf01d86aa4 100644
--- a/libraries/AP_Terrain/AP_Terrain.cpp
+++ b/libraries/AP_Terrain/AP_Terrain.cpp
@@ -302,7 +302,9 @@ void AP_Terrain::update(void)
 
     // update the cached current location height
     Location loc;
-    if (ahrs.get_position(loc) && height_amsl(loc, height)) {
+    bool pos_valid = ahrs.get_position(loc);
+    bool terrain_valid = height_amsl(loc, height);
+    if (pos_valid && terrain_valid) {
         last_current_loc_height = height;
         have_current_loc_height = true;
     }
@@ -313,33 +315,23 @@ void AP_Terrain::update(void)
     // check for pending rally data
     update_rally_data();
 
-    // update capabilities
+    // update capabilities and status
     if (enable) {
         hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
+        if (!pos_valid) {
+            // we don't know where we are
+            system_status = TerrainStatusUnhealthy;
+        } else if (!terrain_valid) {
+            // we don't have terrain data at current location
+            system_status = TerrainStatusUnhealthy;
+        } else {
+            system_status = TerrainStatusOK;
+        }
     } else {
         hal.util->clear_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
+        system_status = TerrainStatusDisabled;
     }
-}
 
-/*
-  return status enum for health reporting
-*/
-enum AP_Terrain::TerrainStatus AP_Terrain::status(void)
-{
-    if (!enable) {
-        return TerrainStatusDisabled;
-    }
-    Location loc;
-    if (!ahrs.get_position(loc)) {
-        // we don't know where we are
-        return TerrainStatusUnhealthy;
-    }
-    float height;
-    if (!height_amsl(loc, height)) {
-        // we don't have terrain data at current location
-        return TerrainStatusUnhealthy;        
-    }
-    return TerrainStatusOK; 
 }
 
 /*
diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index fdd15c5a9e..d54e14b062 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -94,7 +94,7 @@ public:
     void update(void);
 
     // return status enum for health reporting
-    enum TerrainStatus status(void);
+    enum TerrainStatus status(void) const { return system_status; }
 
     // send any pending terrain request message
     void send_request(mavlink_channel_t chan);
@@ -405,6 +405,9 @@ private:
     uint16_t last_rally_spacing;
 
     char *file_path = NULL;    
+
+    // status
+    enum TerrainStatus system_status = TerrainStatusDisabled;
 };
 #endif // AP_TERRAIN_AVAILABLE
 #endif // __AP_TERRAIN_H__