From e24f3f941978a9d6a5ef1ec649fd0bf9167b3d6b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 23 Nov 2016 11:09:33 +1100
Subject: [PATCH] SITL: correct on-ground correction

Using the cached ground_height_difference may mean
you end up at the wrong height if you were near
the edge of an SRTM square and had high horizontal
velocity
---
 libraries/SITL/SIM_Aircraft.cpp | 22 +++++++++++++++-------
 libraries/SITL/SIM_Aircraft.h   |  6 +++---
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index d2e8acabff..2c7b1e6c15 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -119,17 +119,25 @@ bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degree
 }
     
 /*
-   return true if we are on the ground
+   return difference in altitude between home position and current loc
 */
-bool Aircraft::on_ground(const Vector3f &pos)
+float Aircraft::ground_height_difference() const
 {
     float h1, h2;
     if (sitl->terrain_enable && terrain &&
         terrain->height_amsl(home, h1, false) &&
         terrain->height_amsl(location, h2, false)) {
-        ground_height_difference = h2 - h1;
+        return h2 - h1;
     }
-    return (-pos.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference;
+    return 0.0f;
+}
+
+/*
+   return true if we are on the ground
+*/
+bool Aircraft::on_ground(const Vector3f &pos) const
+{
+    return (-pos.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference();
 }
 
 /*
@@ -438,8 +446,8 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     // new velocity vector
     velocity_ef += accel_earth * delta_time;
 
+    const bool was_on_ground = on_ground(position);
     // new position vector
-    Vector3f old_position = position;
     position += velocity_ef * delta_time;
 
     // velocity relative to air mass, in earth frame
@@ -456,11 +464,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     
     // constrain height to the ground
     if (on_ground(position)) {
-        if (!on_ground(old_position) && AP_HAL::millis() - last_ground_contact_ms > 1000) {
+        if (!was_on_ground && AP_HAL::millis() - last_ground_contact_ms > 1000) {
             printf("Hit ground at %f m/s\n", velocity_ef.z);
             last_ground_contact_ms = AP_HAL::millis();
         }
-        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
+        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference());
 
         switch (ground_behavior) {
         case GROUND_BEHAVIOR_NONE:
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 80ebf6bfbf..660e51a052 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -166,12 +166,12 @@ protected:
     bool use_smoothing;
     
     AP_Terrain *terrain;
-    float ground_height_difference;
+    float ground_height_difference() const;
 
     const float FEET_TO_METERS = 0.3048f;
     const float KNOTS_TO_METERS_PER_SECOND = 0.51444f;
-    
-    bool on_ground(const Vector3f &pos);
+
+    bool on_ground(const Vector3f &pos) const;
 
     /* update location from position */
     void update_position(void);