AP_OADatabase: calculate object radius based on distance and beam width

also all object database items are normal importance

libraries/AC_Avoidance/AP_OADatabase.cpp

@@ -68,6 +68,31 @@ const AP_Param::GroupInfo AP_OADatabase::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("OUTPUT", 4, AP_OADatabase, _output_level, (float)OA_DbOutputLevel::OUTPUT_LEVEL_SEND_HIGH),
 
+    // @Param: BEAM_WIDTH
+    // @DisplayName: OADatabase beam width
+    // @Description: Beam width of incoming lidar data
+    // @Units: deg
+    // @Range: 1 10
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("BEAM_WIDTH", 5, AP_OADatabase, _beam_width, 5.0f),
+
+    // @Param: RADIUS_MIN
+    // @DisplayName: OADatabase Minimum  radius
+    // @Description: Minimum radius of objects held in database
+    // @Units: m
+    // @Range: 0 10
+    // @User: Advanced
+    AP_GROUPINFO("RADIUS_MIN", 6, AP_OADatabase, _radius_min, 0.01f),
+
+    // @Param: DIST_MAX
+    // @DisplayName: OADatabase Distance Maximum
+    // @Description: Maximum distance of objects held in database.  Set to zero to disable the limits
+    // @Units: m
+    // @Range: 0 10
+    // @User: Advanced
+    AP_GROUPINFO("DIST_MAX", 7, AP_OADatabase, _dist_max, 0.0f),
+
     AP_GROUPEND
 };
 
@@ -86,6 +111,9 @@ void AP_OADatabase::init()
     init_database();
     init_queue();
 
+    // initialise scalar using beam width of at least 1deg
+    dist_to_radius_scalar = tanf(radians(MAX(_beam_width, 1.0f)));
+
     if (!healthy()) {
         gcs().send_text(MAV_SEVERITY_INFO, "DB init failed . Sizes queue:%u, db:%u", (unsigned int)_queue.size, (unsigned int)_database.size);
         delete _queue.items;
@@ -101,7 +129,6 @@ void AP_OADatabase::update()
     }
 
     process_queue();
-    optimize_db_filter();
     database_items_remove_all_expired();
 }
 
@@ -112,34 +139,12 @@ void AP_OADatabase::queue_push(const Location &loc, uint32_t timestamp_ms, float
         return;
     }
 
-    AP_OADatabase::OA_DbItemImportance importance = AP_OADatabase::OA_DbItemImportance::Normal;
+    // ignore objects that are far away
-
+    if ((_dist_max > 0.0f) && (distance > _dist_max)) {
-    if (distance <= 0 || angle < 0 || angle > 360) {
+        return;
-        // sanity check
-        importance = AP_OADatabase::OA_DbItemImportance::Normal;
-
-    } else if (distance < 10 && (angle > (360-10) || angle < 10)) {
-        // far and directly in front +/- 10deg
-        importance = AP_OADatabase::OA_DbItemImportance::High;
-    } else if (distance < 5 && (angle > (360-30) || angle < 30)) {
-        // kinda far and forward of us +/- 30deg
-        importance = AP_OADatabase::OA_DbItemImportance::High;
-    } else if (distance < 3 && (angle > (360-90) || angle < 90)) {
-        // near and ahead +/- 90deg
-        importance = AP_OADatabase::OA_DbItemImportance::High;
-    } else if (distance < 1.5) {
-        // very close anywhere
-        importance = AP_OADatabase::OA_DbItemImportance::High;
-
-    } else if  (distance >= 10) {
-        // really far away
-        importance = AP_OADatabase::OA_DbItemImportance::Low;
-    } else if (distance < 5 && (angle <= (360-90) || angle >= 90)) {
-        // kinda far and behind us
-        importance = AP_OADatabase::OA_DbItemImportance::Low;
     }
 
-    const OA_DbItem item = {loc, timestamp_ms, 0, 0, importance};
+    const OA_DbItem item = {loc, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
     {
         WITH_SEMAPHORE(_queue.sem);
         _queue.items->push(item);
@@ -166,33 +171,6 @@ void AP_OADatabase::init_database()
     _database.items = new OA_DbItem[_database.size];
 }
 
-void AP_OADatabase::optimize_db_filter()
-{
-    // TODO: check database size and if we're getting full
-    // we should grow the database size and/or increase
-    // _database_filter_m so less objects go into it and let
-    // the existing ones timeout naturally
-
-    const float filter_m_backup = _database.filter_m;
-
-    if (_database.count > (_database.size * 0.90f)) {
-        // we're almost full, lets increase the filter size by requiring more
-        // spacing between points so less things get put into the database
-        _database.filter_m = MIN(_database.filter_m*_database.filter_grow_rate, _database.filter_max_m);
-
-    } else if (_database.count < (_database.size * 0.85f)) {
-        // we have some room, lets loosen the filter requirement (smaller object points) to allow more samples
-        _database.filter_m = MAX(_database.filter_m*_database.filter_shrink_rate,_database.filter_min_m);
-    }
-
-    // recompute the the radius filters
-    if (!is_equal(filter_m_backup,_database.filter_m)) {
-        _radius_importance_low = MIN(_database.filter_m*4,_database.filter_max_m);
-        _radius_importance_normal = _database.filter_m;
-        _radius_importance_high = MAX(_database.filter_m*0.25,_database.filter_min_m);
-    }
-}
-
 // get bitmask of gcs channels item should be sent to based on its importance
 // returns 0xFF (send to all channels) if should be sent, 0 if it should not be sent
 uint8_t AP_OADatabase::get_send_to_gcs_flags(const OA_DbItemImportance importance)
@@ -219,21 +197,6 @@ uint8_t AP_OADatabase::get_send_to_gcs_flags(const OA_DbItemImportance importanc
     return 0x0;
 }
 
-float AP_OADatabase::get_radius(const OA_DbItemImportance importance)
-{
-    switch (importance) {
-    case OA_DbItemImportance::Low:
-        return _radius_importance_low;
-
-    default:
-    case OA_DbItemImportance::Normal:
-        return _radius_importance_normal;
-
-    case OA_DbItemImportance::High:
-        return _radius_importance_high;
-    }
-}
-
 // returns true when there's more work inthe queue to do
 bool AP_OADatabase::process_queue()
 {
@@ -263,7 +226,6 @@ bool AP_OADatabase::process_queue()
             return false;
         }
 
-        item.radius = get_radius(item.importance);
         item.send_to_gcs = get_send_to_gcs_flags(item.importance);
 
         // compare item to all items in database. If found a similar item, update the existing, else add it as a new one
@@ -352,16 +314,10 @@ void AP_OADatabase::database_items_remove_all_expired()
     while (index < _database.count) {
         if (now_ms - _database.items[index].timestamp_ms > expiry_ms) {
             database_item_remove(index);
-        } else {
-            // overtime, the item radius will grow in size. If too big, remove it before the timer
-            _database.items[index].radius *= _database.radius_grow_rate;
-            if (_database.items[index].radius >= _database.filter_max_m) {
-                database_item_remove(index);
         } else {
             index++;
         }
     }
-    }
 }
 
 // returns true if a similar object already exists in database. When true, the object timer is also reset

libraries/AC_Avoidance/AP_OADatabase.h

@@ -45,9 +45,6 @@ public:
     // fetch an item in database. Undefined result when i >= _database.count.
     const OA_DbItem& get_item(uint32_t i) const { return _database.items[i]; }
 
-    // get radius (in meters) of objects in database
-    float get_accuracy() const { return _database.filter_m; }
-
     // get number of items in the database
     uint16_t database_count() const { return _database.count; }
 
@@ -65,9 +62,6 @@ private:
     void init_queue();
     void init_database();
 
-    // check queue and database sizes and adjust filter criteria to optimize use
-    void optimize_db_filter();
-
     // database item management
     void database_item_add(const OA_DbItem &item);
     void database_item_refresh(const uint16_t index, const uint32_t timestamp_ms, const float radius);
@@ -78,9 +72,6 @@ private:
     // returns 0xFF (send to all channels) if should be sent or 0 if it should not be sent
     uint8_t get_send_to_gcs_flags(const OA_DbItemImportance importance);
 
-    // used to determine the filter radius
-    float get_radius(const OA_DbItemImportance importance);
-
     // returns true if database item "index" is close to "item"
     bool is_close_to_item_in_database(const uint16_t index, const OA_DbItem &item) const;
 
@@ -97,21 +88,19 @@ private:
     AP_Int16        _database_size_param;                   // db size
     AP_Int8         _database_expiry_seconds;               // objects expire after this timeout
     AP_Int8         _output_level;                          // controls which items should be sent to GCS
+    AP_Float        _beam_width;                            // beam width used when converting lidar readings to object radius
+    AP_Float        _radius_min;                            // objects minimum radius (in meters)
+    AP_Float        _dist_max;                              // objects maximum distance (in meters)
 
     struct {
         ObjectBuffer<OA_DbItem> *items;                     // thread safe incoming queue of points from proximity sensor to be put into database
         uint16_t        size;                               // cached value of _queue_size_param.
         HAL_Semaphore   sem;                                // semaphore for multi-thread use of queue
     } _queue;
+    float dist_to_radius_scalar;                            // scalar to convert the distance and beam width to an object radius
 
     struct {
         OA_DbItem       *items;                             // array of objects in the database
-        float           filter_m                = 0.2f;     // object avoidance database optimization level radius. Min distance between each fence point. Larger means lower resolution
-        const float     filter_max_m            = 10.0f;    // filter value max size allowed to grow to
-        const float     filter_min_m            = 0.011f;   // worst case resolution of int32 lat/lng value at equator is 1.1cm;
-        const float     filter_grow_rate        = 1.03f;    // db filter how fast you grow to reduce items getting into dB
-        const float     filter_shrink_rate      = 0.99f;    // db filter how fast you shrink to increase items getting into dB
-        const float     radius_grow_rate        = 1.10f;    // db item radius growth over time. Resets if refreshed, otherwise decaying items grow
         uint16_t        count;                              // number of objects in the items array
         uint16_t        size;                               // cached value of _database_size_param that sticks after initialized
     } _database;
@@ -120,10 +109,6 @@ private:
     uint16_t _highest_index_sent[MAVLINK_COMM_NUM_BUFFERS]; // highest index in _database sent to GCS
     uint32_t _last_send_to_gcs_ms[MAVLINK_COMM_NUM_BUFFERS];// system time that send_adsb_vehicle was last called
 
-    float _radius_importance_low = _database.filter_m;
-    float _radius_importance_normal = _database.filter_m;
-    float _radius_importance_high = _database.filter_m;
-
     static AP_OADatabase *_singleton;
 };
 #else