diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b87c285457..1b57dc52ca 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -789,6 +789,9 @@ bool QuadPlane::setup(void) AP_BoardConfig::config_error("Unable to allocate %s", "wp_nav"); } AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); +#if AP_TERRAIN_AVAILABLE + wp_nav->set_terrain(&plane.terrain); +#endif loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); if (!loiter_nav) { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index fe33242917..c9436a89af 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -162,6 +162,11 @@ void Plane::init_ardupilot() #if AC_FENCE == ENABLED fence.init(); #endif + +#if AP_TERRAIN_AVAILABLE + Location::set_terrain(&terrain); +#endif + } //********************************************************************************