From 32bfeed169ca8b3f1a1580249b0a6d1731abdeb0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 22 Jul 2014 08:24:00 +1000
Subject: [PATCH] Plane: incorporate terrain mavlink calls

---
 ArduPlane/ArduPlane.pde   |  6 ++++++
 ArduPlane/GCS_Mavlink.pde | 16 ++++++++++++++++
 ArduPlane/Parameters.h    |  1 +
 ArduPlane/Parameters.pde  |  4 ++++
 4 files changed, 27 insertions(+)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index e7182c212f..5a5a139a27 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -467,7 +467,9 @@ AP_Airspeed airspeed(aparm);
 
 ////////////////////////////////////////////////////////////////////////////////
 // terrain handling
+#if HAVE_AP_TERRAIN
 AP_Terrain terrain(ahrs);
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////
 // ACRO controller state
@@ -977,6 +979,10 @@ static void one_second_loop()
     // update notify flags
     AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
     AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
+
+#if HAVE_AP_TERRAIN
+    terrain.update();
+#endif
 }
 
 static void log_perf_info()
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index d8d5363047..e5ac80e423 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -614,6 +614,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         send_rangefinder(chan);
         break;
 
+    case MSG_TERRAIN:
+#if HAVE_AP_TERRAIN
+        CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
+        terrain.send_request(chan);
+#endif
+        break;
+
     case MSG_WIND:
         CHECK_PAYLOAD_SIZE(WIND);
         send_wind(chan);
@@ -847,6 +854,9 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_WIND);
         send_message(MSG_RANGEFINDER);
         send_message(MSG_SYSTEM_TIME);
+#if HAVE_AP_TERRAIN
+        send_message(MSG_TERRAIN);
+#endif
     }
 }
 
@@ -1462,6 +1472,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
 #endif
 
+    case MAVLINK_MSG_ID_TERRAIN_DATA:
+#if HAVE_AP_TERRAIN
+        terrain.handle_data(msg);
+#endif
+        break;
+        
     default:
         // forward unknown messages to the other link if there is one
         for (uint8_t i=0; i<num_gcs; i++) {
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 976ac3e09f..d726d1161e 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -116,6 +116,7 @@ public:
         k_param_takeoff_throttle_slewrate,
         k_param_takeoff_throttle_max,
         k_param_sonar,
+        k_param_terrain,
 
         // 100: Arming parameters
         k_param_arming = 100,
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 70b7164d49..3b08f3b403 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -867,6 +867,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
     GOBJECT(sonar,                  "RNGFND", RangeFinder),
 
+    // @Group: TERRAIN
+    // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp
+    GOBJECT(terrain,                "TERRAIN_", AP_Terrain),
+
     // RC channel
     //-----------
     // @Group: RC1_