Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Avoidance/AP_Proximity: simplification and efficiency improvements #13024

Merged
merged 15 commits into from
Jan 1, 2020
Merged

AP_Avoidance/AP_Proximity: simplification and efficiency improvements #13024

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
71e6c91
AP_OADatabase: remove unnecessary const on arguments
rmackay9 Nov 27, 2019
c279dc8
AP_Proximity: remove unnecessary const from database_push
rmackay9 Nov 27, 2019
399aa60
AP_Proximity: integrate oadb::queue_push argument swap
rmackay9 Nov 27, 2019
69bd4b2
AP_OADatabase: calculate object radius based on distance and beam width
rmackay9 Nov 14, 2019
6684aa7
AP_OABendyRuler: integrate static object radius
rmackay9 Nov 14, 2019
8133989
AP_Proximity: always use 8 sectors and fix ignore areas
rmackay9 Dec 4, 2019
0ef9b6b
AP_Proximity: sf40c combines seven readings for oadb
rmackay9 Dec 4, 2019
11e2e95
AP_Proximity: remove unused get_horizontal_distance
rmackay9 Dec 4, 2019
54629bb
AP_OADatabase: replace Location with offset from origin
rmackay9 Dec 4, 2019
3a629c4
AP_Proximity: integrate oadb ekf-offset change
rmackay9 Dec 4, 2019
6ec7ac7
AP_Math: add vector2f::offset_bearing
rmackay9 Dec 4, 2019
0daec49
AP_OABendyRuler: integrate oadb ekf-offset change
rmackay9 Dec 4, 2019
ed28ba3
AP_OAPathPlanner: slow updates to 1hz, timeout to 3sec
rmackay9 Dec 9, 2019
398c010
AP_OADatabase: available regardless of hal-minimize-features
rmackay9 Dec 10, 2019
233823e
AP_OADatabase: send_adsb uses vehicle's current altitude
rmackay9 Dec 30, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 3 additions & 10 deletions libraries/AC_Avoidance/AP_OABendyRuler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -355,7 +355,6 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin)
{
#if !HAL_MINIMIZE_FEATURES
// exit immediately if db is empty
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
Expand All @@ -371,15 +370,10 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
// check each obstacle's distance from segment
float smallest_margin = FLT_MAX;
for (uint16_t i=0; i<oaDb->database_count(); i++) {

// convert obstacle's location to offset (in cm) from EKF origin
Vector2f point;
if (!oaDb->get_item(i).loc.get_vector_xy_from_origin_NE(point)) {
continue;
}

const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i);
const Vector2f point_cm = item.pos * 100.0f;
// margin is distance between line segment and obstacle minus obstacle's radius
const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point) * 0.01f - oaDb->get_accuracy();
const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point_cm) * 0.01f - item.radius;
if (m < smallest_margin) {
smallest_margin = m;
}
Expand All @@ -391,6 +385,5 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
return true;
}

#endif
return false;
}
133 changes: 48 additions & 85 deletions libraries/AC_Avoidance/AP_OADatabase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,6 @@

#include "AP_OADatabase.h"

#if !HAL_MINIMIZE_FEATURES

#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Math/AP_Math.h>
Expand Down Expand Up @@ -68,6 +66,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
};

Expand All @@ -86,6 +109,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;
Expand All @@ -101,45 +127,22 @@ void AP_OADatabase::update()
}

process_queue();
optimize_db_filter();
database_items_remove_all_expired();
}

// push a location into the database
void AP_OADatabase::queue_push(const Location &loc, const uint32_t timestamp_ms, const float distance, const float angle)
void AP_OADatabase::queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance)
{
if (!healthy()) {
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 = {pos, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

forcing everything to importance::Normal will defeat the whole GCS throttling scheme. Setting the Imporantance is still needed, maybe it's just the algorithm that assigns it that needs to change?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@magicrub,

Yes, I think if we want to try and reduce the number of objects we send to the GCS then improving the algorithm is a good idea and I guess that's what I was trying to do. While trying to get Bendy Ruler working for Copter I found the display of the objects on the GCS was so unintelligible (see pic in this issue) that I figured it was best to just simplify it down to a point where it worked and then we can re-build from there.

I didn't think that using the vehicle' heading (especially for Copter) was a good idea so what I've done is add the OA_DB_DIST_MAX parameter so the user can specify the maximum distance from the vehicle for any new objects entering the database. This reduces the number of objects overall both for sending to the GCS but also for the path planning done in BendyRuler.

{
WITH_SEMAPHORE(_queue.sem);
_queue.items->push(item);
Expand All @@ -166,33 +169,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)
Expand All @@ -219,21 +195,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()
{
Expand Down Expand Up @@ -263,7 +224,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
Expand Down Expand Up @@ -353,13 +313,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++;
}
}
}
Expand All @@ -372,7 +326,8 @@ bool AP_OADatabase::is_close_to_item_in_database(const uint16_t index, const OA_
return false;
}

return (_database.items[index].loc.get_distance(item.loc) < item.radius);
const float distance_sq = (_database.items[index].pos - item.pos).length_squared();
return ((distance_sq < sq(item.radius)) || (distance_sq < sq(_database.items[index].radius)));
}

// send ADSB_VEHICLE mavlink messages
Expand All @@ -386,6 +341,12 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_
return;
}

// use vehicle's current altitude
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
current_loc.alt = 0;
}

const uint8_t chan_as_bitmask = 1 << chan;
const char callsign[9] = "OA_DB";

Expand Down Expand Up @@ -418,12 +379,15 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_
continue;
}

// convert object's position as an offset from EKF origin to Location
const Location item_loc(Vector3f(_database.items[idx].pos.x * 100.0f, _database.items[idx].pos.y * 100.0f, 0));

mavlink_msg_adsb_vehicle_send(chan,
idx,
_database.items[idx].loc.lat,
_database.items[idx].loc.lng,
item_loc.lat,
item_loc.lng,
0, // altitude_type
_database.items[idx].loc.alt,
current_loc.alt, // use vehicle's current altitude
0, // heading
0, // hor_velocity
0, // ver_velocity
Expand Down Expand Up @@ -479,7 +443,6 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_

// singleton instance
AP_OADatabase *AP_OADatabase::_singleton;
#endif //!HAL_MINIMIZE_FEATURES

namespace AP {
AP_OADatabase *oadatabase()
Expand Down
43 changes: 8 additions & 35 deletions libraries/AC_Avoidance/AP_OADatabase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,8 @@
#pragma once

#include <AP_HAL/AP_HAL.h>
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>

#if !HAL_MINIMIZE_FEATURES
#include <AP_Param/AP_Param.h>

class AP_OADatabase {
Expand All @@ -26,7 +24,7 @@ class AP_OADatabase {
};

struct OA_DbItem {
Location loc; // location of object. TODO: turn this into Vector2Int to save memory
Vector2f pos; // position of the object as an offset in meters from the EKF origin
uint32_t timestamp_ms; // system time that object was last updated
float radius; // objects radius in meters
uint8_t send_to_gcs; // bitmask of mavlink comports to which details of this object should be sent
Expand All @@ -36,18 +34,15 @@ class AP_OADatabase {
void init();
void update();

// push a location into the database
void queue_push(const Location &loc, const uint32_t timestamp_ms, const float distance, const float angle);
// push an object into the database. Pos is the offset in meters from the EKF origin, angle is in degrees, distance in meters
void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance);

// returns true if database is healthy
bool healthy() const { return (_queue.items != nullptr) && (_database.items != nullptr); }

// 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; }

Expand All @@ -65,9 +60,6 @@ class AP_OADatabase {
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);
Expand All @@ -78,9 +70,6 @@ class AP_OADatabase {
// 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;

Expand All @@ -97,21 +86,19 @@ class AP_OADatabase {
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;
Expand All @@ -120,22 +107,8 @@ class AP_OADatabase {
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
class AP_OADatabase {
public:
static AP_OADatabase *get_singleton() { return nullptr; }
void init() {};
void queue_push(const Location &loc, const uint32_t timestamp_ms, const float distance, const float angle) {};
bool healthy() const { return false; }
void send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms) {};
};
#endif // #if !HAL_MINIMIZE_FEATURES

namespace AP {
AP_OADatabase *oadatabase();
Expand Down
Loading