AP_Proximity: add Object Avoidance Database, remove old Sector->Location converter

libraries/AP_Proximity/AP_Proximity.cpp

@@ -379,26 +379,6 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
     return get_boundary_points(primary_instance, num_points);
 }
 
-// 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(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
-{
-    if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
-        num_copied = 0;
-        return false;
-    }
-
-    // call backend copy_locations
-    return drivers[instance]->copy_locations(buff, buff_size, num_copied);
-}
-
-bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
-{
-    return copy_locations(primary_instance, buff, buff_size, num_copied);
-}
-
 // get distance and angle to closest object (used for pre-arm check)
 //   returns true on success, false if no valid readings
 bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const

libraries/AP_Proximity/AP_Proximity.h

@@ -20,13 +20,11 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
-#include <AP_Common/Location.h>
 
 #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 copy_locations method) are valid for this many milliseconds
 
 class AP_Proximity_Backend;
 
@@ -65,13 +63,6 @@ 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);
 
@@ -108,13 +99,6 @@ public:
     const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
     const Vector2f* get_boundary_points(uint16_t& num_points) 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(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
-    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;

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -16,6 +16,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Common/Location.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AC_Avoidance/AP_OADatabase.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Proximity.h"
 #include "AP_Proximity_Backend.h"
@@ -174,19 +175,22 @@ 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;
     }
-    update_locations();
 }
 
 // update boundary points used for object avoidance based on a single sector's distance changing
 //   the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
 //   the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
-void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
+void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB)
 {
     // sanity check
     if (sector >= _num_sectors) {
         return;
     }
 
+    if (push_to_OA_DB) {
+        database_push(_angle[sector], _distance[sector]);
+    }
+
     // find adjacent sector (clockwise)
     uint8_t next_sector = sector + 1;
     if (next_sector >= _num_sectors) {
@@ -229,27 +233,6 @@ 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;
     }
-    update_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 which should only happen if buff is nullptr
-bool AP_Proximity_Backend::copy_locations(AP_Proximity::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);
-    memcpy(buff, _locations, num_copied * sizeof(AP_Proximity::Proximity_Location));
-    return true;
 }
 
 // set status and update valid count
@@ -351,40 +334,41 @@ bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, in
     return found;
 }
 
-void AP_Proximity_Backend::update_locations()
+// returns true if database is ready to be pushed to and all cached data is ready
+bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, float &current_vehicle_bearing)
 {
-    WITH_SEMAPHORE(_rsem);
+    AP_OADatabase *oaDb = AP::oadatabase();
+    if (oaDb == nullptr || !oaDb->healthy()) {
+        return false;
+    }
 
-    // get number of objects, return early if none
-    const uint8_t num_objects = get_object_count();
-    if (num_objects == 0) {
-        _location_count = 0;
-        return;
+    if (!AP::ahrs().get_position(current_loc)) {
+        return false;
     }
 
-    // get vehicle location and heading in degrees
-    float veh_bearing;
-    Location my_loc;
-    {
-        WITH_SEMAPHORE(AP::ahrs().get_semaphore());
-        if (!AP::ahrs().get_position(my_loc)) {
-            return;
-        }
-        veh_bearing = AP::ahrs().yaw_sensor * 0.01f;
-    }
-
-    // convert offset from vehicle position to earth frame location
-    const uint32_t now_ms = AP_HAL::millis();
-    _location_count = 0;
-    for (uint8_t i=0; i<num_objects; i++) {
-        float ang_deg, dist_m;
-        if (get_object_angle_and_distance(i, ang_deg, dist_m)) {
-            Location temp_loc = my_loc;
-            temp_loc.offset_bearing(wrap_180(veh_bearing + ang_deg), dist_m);
-            _locations[_location_count].loc = temp_loc;
-            _locations[_location_count].radius_m = 1;
-            _locations[_location_count].last_update_ms = now_ms;
-            _location_count++;
-        }
+    current_vehicle_bearing = AP::ahrs().yaw_sensor * 0.01f;
+    return true;
+}
+
+// update Object Avoidance database with Earth-frame point
+void AP_Proximity_Backend::database_push(const float angle, const float distance)
+{
+    Location current_loc;
+    float current_vehicle_bearing;
+    if (database_prepare_for_push(current_loc, current_vehicle_bearing) == true) {
+        database_push(angle, distance, AP_HAL::millis(), current_loc, current_vehicle_bearing);
     }
 }
+
+// update Object Avoidance database with Earth-frame point
+void AP_Proximity_Backend::database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location &current_loc, const float current_vehicle_bearing)
+{
+    AP_OADatabase *oaDb = AP::oadatabase();
+    if (oaDb == nullptr || !oaDb->healthy()) {
+        return;
+    }
+
+    Location temp_loc = current_loc;
+    temp_loc.offset_bearing(wrap_180(current_vehicle_bearing + angle), distance);
+    oaDb->queue_push(temp_loc, timestamp_ms, distance, angle);
+}

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -17,6 +17,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Proximity.h"
+#include <AP_Common/Location.h>
 
 #define PROXIMITY_SECTORS_MAX   12  // maximum number of sectors
 #define PROXIMITY_BOUNDARY_DIST_MIN 0.6f    // minimum distance for a boundary point.  This ensures the object avoidance code doesn't think we are outside the boundary.
@@ -64,12 +65,6 @@ public:
     // get distances in 8 directions. used for sending distances to ground station
     bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) 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 which should only happen if buff is nullptr
-    bool copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
-
 protected:
 
     // set status and update valid_count
@@ -85,15 +80,17 @@ protected:
     // update boundary points used for object avoidance based on a single sector's distance changing
     //   the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
     //   the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
-    void update_boundary_for_sector(uint8_t sector);
+    void update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB);
 
     // get ignore area info
     uint8_t get_ignore_area_count() const;
     bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const;
     bool get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const;
 
-    // earth frame objects
-    void update_locations();
+    // database helpers
+    bool database_prepare_for_push(Location &current_loc, float &current_vehicle_bearing);
+    void database_push(const float angle, const float distance);
+    void database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location &current_loc, const float current_vehicle_bearing);
 
     AP_Proximity &frontend;
     AP_Proximity::Proximity_State &state;   // reference to this instances state
@@ -111,9 +108,4 @@ protected:
     // fence boundary
     Vector2f _sector_edge_vector[PROXIMITY_SECTORS_MAX];    // vector for right-edge of each sector, used to speed up calculation of boundary
     Vector2f _boundary_point[PROXIMITY_SECTORS_MAX];        // bounding polygon around the vehicle calculated conservatively for object avoidance
-
-    // earth frame locations (i.e. detected obstacles stored as lat/lon points)
-    uint16_t _location_count;                               // number of locations held in _locations buffer
-    AP_Proximity::Proximity_Location _locations[PROXIMITY_SECTORS_MAX];   // buffer of locations
-    HAL_Semaphore_Recursive _rsem;                          // semaphore for access to _locations and _location_count
 };

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -418,7 +418,7 @@ bool AP_Proximity_LightWareSF40C::process_reply()
                 _last_distance_received_ms = AP_HAL::millis();
                 success = true;
                 // update boundary used for avoidance
-                update_boundary_for_sector(sector);
+                update_boundary_for_sector(sector, true);
             }
             break;
         }

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -67,17 +67,17 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
         if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
             uint8_t sector = packet.orientation;
             _angle[sector] = sector * 45;
-            _distance[sector] = packet.current_distance / 100.0f;
-            _distance_min = packet.min_distance / 100.0f;
-            _distance_max = packet.max_distance / 100.0f;
+            _distance[sector] = packet.current_distance * 0.01f;
+            _distance_min = packet.min_distance * 0.01f;
+            _distance_max = packet.max_distance * 0.01f;
             _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
             _last_update_ms = AP_HAL::millis();
-            update_boundary_for_sector(sector);
+            update_boundary_for_sector(sector, true);
         }
 
         // store upward distance
         if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
-            _distance_upward = packet.current_distance / 100.0f;
+            _distance_upward = packet.current_distance * 0.01f;
             _last_upward_update_ms = AP_HAL::millis();
         }
         return;
@@ -104,8 +104,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
         const float total_distances = MIN(360.0f / increment, MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
 
         // set distance min and max
-        _distance_min = packet.min_distance / 100.0f;
-        _distance_max = packet.max_distance / 100.0f;
+        _distance_min = packet.min_distance * 0.01f;
+        _distance_max = packet.max_distance * 0.01f;
         _last_update_ms = AP_HAL::millis();
 
         // get user configured yaw correction from front end
@@ -115,6 +115,10 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
             increment *= -1;
         }
 
+        Location current_loc;
+        float current_vehicle_bearing;
+        const bool database_ready = database_prepare_for_push(current_loc, current_vehicle_bearing);
+
         // initialise updated array and proximity sector angles (to closest object) and distances
         bool sector_updated[_num_sectors];
         float sector_width_half[_num_sectors];
@@ -140,13 +144,18 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
                     sector_updated[i] = true;
                 }
             }
+
+            // update Object Avoidance database with Earth-frame point
+            if (database_ready) {
+                database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_vehicle_bearing);
+            }
         }
 
         // update proximity sectors validity and boundary point
         for (uint8_t i = 0; i < _num_sectors; i++) {
             _distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
             if (sector_updated[i]) {
-                update_boundary_for_sector(i);
+                update_boundary_for_sector(i, false);
             }
         }
     }

libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp

@@ -69,7 +69,7 @@ void AP_Proximity_MorseSITL::update(void)
             _distance_valid[sector] = true;
             _distance[sector] = range;
             _angle[sector] = angle_deg;
-            update_boundary_for_sector(sector);
+            update_boundary_for_sector(sector, true);
         }
     }
 

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -416,7 +416,7 @@ void AP_Proximity_RPLidarA2::parse_response_data()
                             _distance[_last_sector] = _distance_m_last;
                             _distance_valid[_last_sector] = true;
                             // update boundary used for avoidance
-                            update_boundary_for_sector(_last_sector);
+                            update_boundary_for_sector(_last_sector, true);
                             // initialize the new sector
                             _last_sector     = sector;
                             _distance_m_last = distance_m;

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -52,12 +52,12 @@ void AP_Proximity_RangeFinder::update(void)
             if (sensor->orientation() <= ROTATION_YAW_315) {
                 uint8_t sector = (uint8_t)sensor->orientation();
                 _angle[sector] = sector * 45;
-                _distance[sector] = sensor->distance_cm() / 100.0f;
-                _distance_min = sensor->min_distance_cm() / 100.0f;
-                _distance_max = sensor->max_distance_cm() / 100.0f;
+                _distance[sector] = sensor->distance_cm() * 0.01f;
+                _distance_min = sensor->min_distance_cm() * 0.01f;
+                _distance_max = sensor->max_distance_cm() * 0.01f;
                 _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
                 _last_update_ms = now;
-                update_boundary_for_sector(sector);
+                update_boundary_for_sector(sector, true);
             }
             // check upward facing range finder
             if (sensor->orientation() == ROTATION_PITCH_90) {

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -57,7 +57,7 @@ void AP_Proximity_SITL::update(void)
             set_status(AP_Proximity::Proximity_Good);
             _distance_valid[last_sector] = true;
             _angle[last_sector] = _sector_middle_deg[last_sector];
-            update_boundary_for_sector(last_sector);
+            update_boundary_for_sector(last_sector, true);
         } else {
             _distance_valid[last_sector] = false;
         }

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -124,6 +124,6 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_
         _distance_valid[sector] = distance_cm != 0xffff;
         _last_distance_received_ms = AP_HAL::millis();
         // update boundary used for avoidance
-        update_boundary_for_sector(sector);
+        update_boundary_for_sector(sector, true);
     }
 }

libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp

@@ -178,6 +178,6 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
         _distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
         _last_distance_received_ms = AP_HAL::millis();
         // update boundary used for avoidance
-        update_boundary_for_sector(sector);
+        update_boundary_for_sector(sector, true);
     }
 }