mirror of https://github.com/ArduPilot/ardupilot
AP_OADatabase: calculate object radius based on distance and beam width
also all object database items are normal importance
This commit is contained in:
parent
1d79ff11b8
commit
674d631fb1
|
@ -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
|
||||
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue