From f2163fbc24fc10811d61c371f50c650ea8503802 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 Jun 2019 10:09:40 +1000 Subject: [PATCH] AC_WPNav: use singleton for getting AC_Avoid instance --- libraries/AC_WPNav/AC_Loiter.cpp | 1 + libraries/AC_WPNav/AC_Loiter.h | 4 ---- libraries/AC_WPNav/AC_WPNav.h | 4 ---- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index d9f6d9bae5..38647ac45f 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -305,6 +305,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt) // Limit the velocity to prevent fence violations // TODO: We need to also limit the _desired_accel + AC_Avoid *_avoid = AP::ac_avoid(); if (_avoid != nullptr) { _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt); } diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index c3b57fc81b..694d01845b 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -16,9 +16,6 @@ public: /// Constructor AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); - /// provide pointer to avoidance library - void set_avoidance(AC_Avoid* avoid_ptr) { _avoid = avoid_ptr; } - /// init_target to a position in cm from ekf origin void init_target(const Vector3f& position); @@ -73,7 +70,6 @@ protected: const AP_AHRS_View& _ahrs; AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; - AC_Avoid *_avoid = nullptr; // parameters AP_Float _angle_max; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index a76a10be54..76b285d364 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -51,9 +51,6 @@ public: /// provide pointer to terrain database void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_ptr; } - /// provide pointer to avoidance library - void set_avoidance(AC_Avoid* avoid_ptr) { _avoid = avoid_ptr; } - /// provide rangefinder altitude void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } @@ -285,7 +282,6 @@ protected: AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; AP_Terrain *_terrain; - AC_Avoid *_avoid; // parameters AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions