Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OADatabase: calculate object radius based on distance and beam width 

also all object database items are normal importance
This commit is contained in:
Randy Mackay 2019-11-14 17:09:16 +09:00
parent 1d79ff11b8
commit 674d631fb1
2 changed files with 37 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/AC_Avoidance/AP_OADatabase.cpp
View File

@ -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++;
}
}
}

23
libraries/AC_Avoidance/AP_OADatabase.h
View File

@ -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