diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 29ec611128..bf8035b435 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -22,6 +22,7 @@ #include "AP_Proximity_MAV.h" #include "AP_Proximity_SITL.h" #include "AP_Proximity_MorseSITL.h" +#include extern const AP_HAL::HAL &hal; @@ -378,6 +379,78 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const return get_boundary_points(primary_instance, num_points); } +const AP_Proximity::Proximity_Location* AP_Proximity::get_locations(uint16_t& location_count) const +{ + if (_location_count == 0) { + return nullptr; + } + + location_count = _location_count; + return _locations; +} + +// copy location points around vehicle into a buffer owned by the caller +// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) +// num_copied is updated with the number of locations copied into the buffer +// returns true on success, false on failure (should only happen if there is a semaphore conflict) +bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied) +{ + // sanity check buffer + if (buff == nullptr) { + num_copied = 0; + return false; + } + + WITH_SEMAPHORE(_rsem); + + // copy locations into caller's buffer + num_copied = MIN(_location_count, buff_size); + for (uint16_t i=0; i #include #include +#include #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored #define PROXIMITY_MAX_DIRECTION 8 #define PROXIMITY_SENSOR_ID_START 10 +#define PROXIMITY_LOCATION_TIMEOUT_MS 3000 // locations (provided by get_locations method) are valid for this many milliseconds class AP_Proximity_Backend; @@ -63,6 +65,13 @@ public: float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters }; + // structure holding locations of detected objects in earth frame + struct Proximity_Location { + float radius_m; // radius of object in meters + Location loc; + uint32_t last_update_ms; + }; + // detect and initialise any available proximity sensors void init(void); @@ -99,6 +108,16 @@ public: const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const; const Vector2f* get_boundary_points(uint16_t& num_points) const; + // get Location points around vehicle for use by avoidance in earth-frame + // returns nullptr and sets num_points to zero if no boundary can be returned + const Proximity_Location* get_locations(uint16_t& location_count) const; + + // copy location points around vehicle into a buffer owned by the caller + // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) + // num_copied is updated with the number of locations copied into the buffer + // returns true on success, false on failure (should only happen if there is a semaphore conflict) + bool copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied); + // get distance and angle to closest object (used for pre-arm check) // returns true on success, false if no valid readings bool get_closest_object(float& angle_deg, float &distance) const; @@ -158,6 +177,12 @@ private: AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored void detect_instance(uint8_t instance); + + // earth frame objects + void locations_update(); + uint16_t _location_count; // number of locations held in _locations buffer + Proximity_Location _locations[PROXIMITY_MAX_DIRECTION]; // buffer of locations + HAL_Semaphore_Recursive _rsem; // semaphore for access to _locations and _location_count }; namespace AP { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 648d89824a..768b8c0e3d 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -172,6 +172,7 @@ void AP_Proximity_Backend::init_boundary() _sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f; _boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT; } + frontend.locations_update(); } // update boundary points used for object avoidance based on a single sector's distance changing @@ -226,6 +227,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector) if (!_distance_valid[prev_sector_ccw]) { _boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance; } + frontend.locations_update(); } // set status and update valid count