From 674d631fb1f529f5fd6b2f55a88027cfc62329b2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 Nov 2019 17:09:16 +0900 Subject: [PATCH] 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 | 110 +++++++---------------- libraries/AC_Avoidance/AP_OADatabase.h | 23 +---- 2 files changed, 37 insertions(+), 96 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OADatabase.cpp b/libraries/AC_Avoidance/AP_OADatabase.cpp index 2e7b17446d..a51a6808fc 100644 --- a/libraries/AC_Avoidance/AP_OADatabase.cpp +++ b/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; - - if (distance <= 0 || angle < 0 || angle > 360) { - // 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; + // ignore objects that are far away + if ((_dist_max > 0.0f) && (distance > _dist_max)) { + return; } - 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 @@ -353,13 +315,7 @@ void AP_OADatabase::database_items_remove_all_expired() 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++; - } + index++; } } } diff --git a/libraries/AC_Avoidance/AP_OADatabase.h b/libraries/AC_Avoidance/AP_OADatabase.h index 03507a1789..7fc9ee52d2 100644 --- a/libraries/AC_Avoidance/AP_OADatabase.h +++ b/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 *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