From c5a3781507231fc5cdd72f1453795d9c962476bc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 23 Apr 2016 16:27:02 +0900 Subject: [PATCH] AC_WPNav: accept terrain library reference --- libraries/AC_WPNav/AC_WPNav.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index d7173ed9b4..179e02ef1a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -8,6 +8,7 @@ #include // Inertial Navigation library #include // Position control library #include // Attitude control library +#include // loiter maximum velocities and accelerations #define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller @@ -53,6 +54,9 @@ public: /// Constructor AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); + /// provide pointer to terrain database + void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_ptr; } + /// /// loiter controller /// @@ -300,11 +304,12 @@ protected: // returns false if conversion failed (likely because terrain data was not available) bool get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &terrain_alt); - // references to inertial nav and ahrs libraries + // references and pointers to external libraries const AP_InertialNav& _inav; const AP_AHRS& _ahrs; AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; + AP_Terrain *_terrain = NULL; // parameters AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter