From f8918d15b150ce71ae2e2c93c065245a975c5ecd Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 23 Nov 2016 11:19:02 +1100
Subject: [PATCH] SITL: add a hagl method to aircraft

---
 libraries/SITL/SIM_Aircraft.cpp | 9 ++++++++-
 libraries/SITL/SIM_Aircraft.h   | 3 +++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index eb6f327eb0..4b295dc62e 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -132,12 +132,19 @@ float Aircraft::ground_height_difference() const
     return 0.0f;
 }
 
+/*
+   return current height above ground level (metres)
+*/
+float Aircraft::hagl() const
+{
+    return (-position.z) + home.alt*0.01f - ground_level - frame_height - ground_height_difference();
+}
 /*
    return true if we are on the ground
 */
 bool Aircraft::on_ground() const
 {
-    return (-position.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference();
+    return hagl() <= 0;
 }
 
 /*
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 24436387fc..96a9060233 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -173,6 +173,9 @@ protected:
 
     bool on_ground() const;
 
+    // returns height above ground level in metres
+    float hagl() const; // metres
+
     /* update location from position */
     void update_position(void);