diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 0a7c64ae9a..1a09de5214 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -99,9 +99,10 @@ float Aircraft::ground_height_difference() const sitl->terrain_enable && terrain && terrain->height_amsl(home, h1, false) && terrain->height_amsl(location, h2, false)) { + h2 += local_ground_level; return h2 - h1; } - return 0.0f; + return local_ground_level; } void Aircraft::set_precland(SIM_Precland *_precland) { @@ -510,6 +511,7 @@ void Aircraft::update_model(const struct sitl_input &input) loc.alt = sitl->opos.alt.get() * 1.0e2; set_start_location(loc, sitl->opos.hdg.get()); } + local_ground_level = 0.0f; update(input); } @@ -879,6 +881,9 @@ void Aircraft::update_external_payload(const struct sitl_input &input) if (precland && precland->is_enabled()) { precland->update(get_location(), get_position()); + if (precland->_over_precland_base) { + local_ground_level += precland->_origin_height; + } } // update RichenPower generator diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 610ec0c789..f099c0b057 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -172,6 +172,7 @@ protected: float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube float battery_voltage = -1.0f; float battery_current; + float local_ground_level; // ground level at local position // battery model Battery battery;