diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 42233a47d8..7fd010bd37 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -1,5 +1,10 @@ #include "AC_Avoid.h" +#include // AHRS library +#include // Failsafe fence library +#include +#include + #if APM_BUILD_TYPE(APM_BUILD_APMrover2) # define AP_AVOID_BEHAVE_DEFAULT AC_Avoid::BehaviourType::BEHAVIOR_STOP #else @@ -51,11 +56,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { }; /// Constructor -AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AC_Fence& fence, const AP_Proximity& proximity, const AP_Beacon* beacon) - : _ahrs(ahrs), - _fence(fence), - _proximity(proximity), - _beacon(beacon) +AC_Avoid::AC_Avoid() { _singleton = this; @@ -135,13 +136,16 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c bool limit_alt = false; float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) + const AP_AHRS &_ahrs = AP::ahrs(); + // calculate distance below fence - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + AC_Fence *fence = AP::fence(); + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); // _fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = _fence.get_safe_alt_max() + veh_alt; + alt_diff = fence->get_safe_alt_max() + veh_alt; limit_alt = true; } @@ -161,7 +165,8 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // get distance from proximity sensor float proximity_alt_diff; - if (_proximity.get_upward_distance(proximity_alt_diff)) { + AP_Proximity *proximity = AP::proximity(); + if (proximity && proximity->get_upward_distance(proximity_alt_diff)) { proximity_alt_diff -= _margin; if (!limit_alt || proximity_alt_diff < alt_diff) { alt_diff = proximity_alt_diff; @@ -267,6 +272,13 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, flo */ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) { + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return; + } + + AC_Fence &_fence = *fence; + // exit if circular fence is not enabled if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) { return; @@ -277,6 +289,8 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f return; } + const AP_AHRS &_ahrs = AP::ahrs(); + // get position as a 2D offset from ahrs home Vector2f position_xy; if (!_ahrs.get_relative_position_NE_home(position_xy)) { @@ -325,6 +339,13 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f */ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) { + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return; + } + + AC_Fence &_fence = *fence; + // exit if the polygon fence is not enabled if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) { return; @@ -354,6 +375,8 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2 */ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) { + AP_Beacon *_beacon = AP::beacon(); + // exit if the beacon is not present if (_beacon == nullptr) { return; @@ -372,7 +395,11 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f } // adjust velocity using beacon - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, _fence.get_margin(), dt); + float margin = 0; + if (AP::fence()) { + margin = AP::fence()->get_margin(); + } + adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, margin, dt); } /* @@ -381,6 +408,13 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) { // exit immediately if proximity sensor is not present + AP_Proximity *proximity = AP::proximity(); + if (!proximity) { + return; + } + + AP_Proximity &_proximity = *proximity; + if (_proximity.get_status() != AP_Proximity::Proximity_Good) { return; } @@ -406,6 +440,8 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des return; } + const AP_AHRS &_ahrs = AP::ahrs(); + // do not adjust velocity if vehicle is outside the polygon fence Vector2f position_xy; if (earth_frame) { @@ -417,6 +453,12 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des position_xy = position_xy * 100.0f; // m to cm } + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return; + } + AC_Fence &_fence = *fence; + if (_fence.boundary_breached(position_xy, num_points, boundary)) { return; } @@ -535,6 +577,12 @@ float AC_Avoid::distance_to_lean_pct(float dist_m) // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) { + AP_Proximity *proximity = AP::proximity(); + if (proximity == nullptr) { + return; + } + AP_Proximity &_proximity = *proximity; + // exit immediately if proximity sensor is not present if (_proximity.get_status() != AP_Proximity::Proximity_Good) { return; diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 1469ca67a9..be14190d3f 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -3,11 +3,7 @@ #include #include #include -#include // AHRS library #include // Attitude controller library for sqrt controller -#include // Failsafe fence library -#include -#include #define AC_AVOID_ACCEL_CMSS_MAX 100.0f // maximum acceleration/deceleration in cm/s/s used to avoid hitting fence @@ -28,7 +24,7 @@ */ class AC_Avoid { public: - AC_Avoid(const AP_AHRS& ahrs, const AC_Fence& fence, const AP_Proximity& proximity, const AP_Beacon* beacon = nullptr); + AC_Avoid(); /* Do not allow copies */ AC_Avoid(const AC_Avoid &other) = delete; @@ -131,12 +127,6 @@ private: // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative); - // external references - const AP_AHRS& _ahrs; - const AC_Fence& _fence; - const AP_Proximity& _proximity; - const AP_Beacon* _beacon; - // parameters AP_Int8 _enabled; AP_Int16 _angle_max; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes)