SITL: allow SITL to use terrain data for ground height

libraries/SITL/SIM_Aircraft.cpp

@@ -30,6 +30,7 @@
 #endif
 
 #include <DataFlash/DataFlash.h>
+#include <AP_Param/AP_Param.h>
 
 namespace SITL {
 
@@ -69,6 +70,8 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
 
     last_wall_time_us = get_wall_time_us();
     frame_counter = 0;
+
+    terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
 }
 
 
@@ -117,9 +120,15 @@ bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degree
 /*
    return true if we are on the ground
 */
-bool Aircraft::on_ground(const Vector3f &pos) const
+bool Aircraft::on_ground(const Vector3f &pos)
 {
-    return (-pos.z) + home.alt*0.01f <= ground_level + frame_height;
+    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 (-pos.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference;
 }
 
 /*
@@ -419,7 +428,36 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
             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);
+        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
+
+        switch (ground_behavior) {
+        case GROUND_BEHAVIOR_NONE:
+            break;
+        case GROUND_BEHAVIOR_NO_MOVEMENT: {
+            // zero roll/pitch, but keep yaw
+            float r, p, y;
+            dcm.to_euler(&r, &p, &y);
+            dcm.from_euler(0, 0, y);
+            // no X or Y movement
+            velocity_ef.x = 0;
+            velocity_ef.y = 0;
+            break;
+        }
+        case GROUND_BEHAVIOR_FWD_ONLY: {
+            // zero roll/pitch, but keep yaw
+            float r, p, y;
+            dcm.to_euler(&r, &p, &y);
+            dcm.from_euler(0, 0, y);
+            // only fwd movement
+            Vector3f v_bf = dcm.transposed() * velocity_ef;
+            v_bf.y = 0;
+            if (v_bf.x < 0) {
+                v_bf.x = 0;
+            }
+            velocity_ef = dcm * v_bf;
+            break;
+        }
+        }
     }
 }
 

libraries/SITL/SIM_Aircraft.h

@@ -22,6 +22,7 @@
 #include <AP_Math/AP_Math.h>
 
 #include "SITL.h"
+#include <AP_Terrain/AP_Terrain.h>
 
 namespace SITL {
 
@@ -144,10 +145,19 @@ protected:
     const char *frame;
     bool use_time_sync = true;
 
+    enum {
+        GROUND_BEHAVIOR_NONE=0,
+        GROUND_BEHAVIOR_NO_MOVEMENT,
+        GROUND_BEHAVIOR_FWD_ONLY,
+    } ground_behavior;
+    
+    AP_Terrain *terrain;
+    float ground_height_difference;
+
     const float FEET_TO_METERS = 0.3048f;
     const float KNOTS_TO_METERS_PER_SECOND = 0.51444f;
     
-    bool on_ground(const Vector3f &pos) const;
+    bool on_ground(const Vector3f &pos);
 
     /* update location from position */
     void update_position(void);

libraries/SITL/SIM_Helicopter.cpp

@@ -47,6 +47,8 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
         frame_type = HELI_FRAME_CONVENTIONAL;
     }
     gas_heli = (strstr(frame_str, "-gas") != NULL);
+
+    ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 
 /*
@@ -167,7 +169,7 @@ void Helicopter::update(const struct sitl_input &input)
         dcm.to_euler(&r, &p, &y);
         dcm.from_euler(0, 0, y);
         
-        position.z = -(ground_level + frame_height - home.alt*0.01f);
+        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
         velocity_ef.zero();
     }
 

libraries/SITL/SIM_Multicopter.cpp

@@ -39,6 +39,7 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
         frame->init(1.5, 0.51, 15, 4*radians(360));
     }
     frame_height = 0.1;
+    ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 
 // calculate rotational and linear accelerations
@@ -67,7 +68,7 @@ void MultiCopter::update(const struct sitl_input &input)
         dcm.to_euler(&r, &p, &y);
         dcm.from_euler(0, 0, y);
 
-        position.z = -(ground_level + frame_height - home.alt*0.01f);
+        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
     }
     
     // update lat/lon/altitude

libraries/SITL/SIM_Plane.cpp

@@ -47,6 +47,8 @@ Plane::Plane(const char *home_str, const char *frame_str) :
     } else if (strstr(frame_str, "-vtail")) {
         vtail = true;
     }
+
+    ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
 }
 
 /*

libraries/SITL/SIM_QuadPlane.cpp

@@ -82,6 +82,7 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
     // we use zero terminal velocity to let the plane model handle the drag
     frame->init(mass, 0.51, 0, 0);
 
+    ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 
 /*

libraries/SITL/SIM_SingleCopter.cpp

@@ -109,7 +109,7 @@ void SingleCopter::update(const struct sitl_input &input)
         dcm.to_euler(&r, &p, &y);
         dcm.from_euler(0, 0, y);
         
-        position.z = -(ground_level + frame_height - home.alt*0.01f);
+        position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
         velocity_ef.zero();
     }