From e7a54c83d1e2964828e0e833a230778551fedb98 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Jul 2016 14:42:31 +1000 Subject: [PATCH] SITL: allow SITL to use terrain data for ground height --- libraries/SITL/SIM_Aircraft.cpp | 44 +++++++++++++++++++++++++++-- libraries/SITL/SIM_Aircraft.h | 12 +++++++- libraries/SITL/SIM_Helicopter.cpp | 4 ++- libraries/SITL/SIM_Multicopter.cpp | 3 +- libraries/SITL/SIM_Plane.cpp | 2 ++ libraries/SITL/SIM_QuadPlane.cpp | 1 + libraries/SITL/SIM_SingleCopter.cpp | 2 +- 7 files changed, 61 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index bfaabe22c3..c33e2b0bc5 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -30,6 +30,7 @@ #endif #include +#include 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; + } + } } } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index bd1a4402e8..d77269afab 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -22,6 +22,7 @@ #include #include "SITL.h" +#include 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); diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index c6ca076d77..5de365b2b3 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/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(); } diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index f6b1257d4d..33990b8874 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/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 diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 4da8564b7b..c1b505fdbb 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/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; } /* diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index b9819ab54e..d25af72fa6 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/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; } /* diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp index 865174f1c9..1a565adf91 100644 --- a/libraries/SITL/SIM_SingleCopter.cpp +++ b/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(); }