diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index 890549a73d..7b3fcb9dc6 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -130,6 +130,10 @@ AP_Avoidance::AP_Avoidance(AP_ADSB &adsb) : _adsb(adsb) { AP_Param::setup_object_defaults(this, var_info); + if (_singleton != nullptr) { + AP_HAL::panic("AP_Avoidance must be singleton"); + } + _singleton = this; } /* @@ -665,3 +669,16 @@ Vector2f AP_Avoidance::perpendicular_xy(const Location &p1, const Vector3f &v1, Vector2f ret_xy = Vector2f::perpendicular(delta_p_n, v1n); return ret_xy; } + + +// singleton instance +AP_Avoidance *AP_Avoidance::_singleton; + +namespace AP { + +AP_Avoidance *ap_avoidance() +{ + return AP_Avoidance::get_singleton(); +} + +} diff --git a/libraries/AP_Avoidance/AP_Avoidance.h b/libraries/AP_Avoidance/AP_Avoidance.h index df535d0d3f..4c8638c29e 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.h +++ b/libraries/AP_Avoidance/AP_Avoidance.h @@ -43,6 +43,15 @@ public: // constructor AP_Avoidance(class AP_ADSB &adsb); + /* Do not allow copies */ + AP_Avoidance(const AP_Avoidance &other) = delete; + AP_Avoidance &operator=(const AP_Avoidance&) = delete; + + // get singleton instance + static AP_Avoidance *get_singleton() { + return _singleton; + } + // obstacle class to hold latest information for a known obstacles class Obstacle { public: @@ -198,6 +207,8 @@ private: // multi-thread support for avoidance HAL_Semaphore_Recursive _rsem; + + static AP_Avoidance *_singleton; }; float closest_approach_xy(const Location &my_loc, @@ -211,3 +222,8 @@ float closest_approach_z(const Location &my_loc, const Location &obstacle_loc, const Vector3f &obstacle_vel, uint8_t time_horizon); + + +namespace AP { + AP_Avoidance *ap_avoidance(); +};