aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: add Object Avoidance Database, remove old Sector->Location converter

Browse Source
This commit is contained in:
Tom Pittenger 2019-06-28 22:57:09 -07:00 committed by Tom Pittenger
parent 9ad6d14c16
commit b8dcdca909
12 changed files with 71 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -379,26 +379,6 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
return get_boundary_points(primary_instance, num_points);
}
// copy location points around vehicle into a buffer owned by the caller
// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
// num_copied is updated with the number of locations copied into the buffer
// returns true on success, false on failure (should only happen if there is a semaphore conflict)
bool AP_Proximity::copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
{
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
num_copied = 0;
return false;
}
// call backend copy_locations
return drivers[instance]->copy_locations(buff, buff_size, num_copied);
}
bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
{
return copy_locations(primary_instance, buff, buff_size, num_copied);
}
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const

16
libraries/AP_Proximity/AP_Proximity.h
View File

@ -20,13 +20,11 @@
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Common/Location.h>
#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform
#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10
#define PROXIMITY_LOCATION_TIMEOUT_MS 3000 // locations (provided by copy_locations method) are valid for this many milliseconds
class AP_Proximity_Backend;
@ -65,13 +63,6 @@ public:
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
};
// structure holding locations of detected objects in earth frame
struct Proximity_Location {
float radius_m; // radius of object in meters
Location loc;
uint32_t last_update_ms;
};
// detect and initialise any available proximity sensors
void init(void);
@ -108,13 +99,6 @@ public:
const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
const Vector2f* get_boundary_points(uint16_t& num_points) const;
// copy location points around vehicle into a buffer owned by the caller
// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
// num_copied is updated with the number of locations copied into the buffer
// returns true on success, false on failure (should only happen if there is a semaphore conflict)
bool copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
bool copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;

90
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -16,6 +16,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AC_Avoidance/AP_OADatabase.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity.h"
#include "AP_Proximity_Backend.h"
@ -174,19 +175,22 @@ void AP_Proximity_Backend::init_boundary()
_sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
_boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
}
update_locations();
}
// update boundary points used for object avoidance based on a single sector's distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB)
{
// sanity check
if (sector >= _num_sectors) {
return;
}
if (push_to_OA_DB) {
database_push(_angle[sector], _distance[sector]);
}
// find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= _num_sectors) {
@ -229,27 +233,6 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
if (!_distance_valid[prev_sector_ccw]) {
_boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance;
}
update_locations();
}
// copy location points around vehicle into a buffer owned by the caller
// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
// num_copied is updated with the number of locations copied into the buffer
// returns true on success, false on failure which should only happen if buff is nullptr
bool AP_Proximity_Backend::copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
{
// sanity check buffer
if (buff == nullptr) {
num_copied = 0;
return false;
}
WITH_SEMAPHORE(_rsem);
// copy locations into caller's buffer
num_copied = MIN(_location_count, buff_size);
memcpy(buff, _locations, num_copied * sizeof(AP_Proximity::Proximity_Location));
return true;
}
// set status and update valid count
@ -351,40 +334,41 @@ bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, in
return found;
}
void AP_Proximity_Backend::update_locations()
// returns true if database is ready to be pushed to and all cached data is ready
bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, float &current_vehicle_bearing)
{
WITH_SEMAPHORE(_rsem);
// get number of objects, return early if none
const uint8_t num_objects = get_object_count();
if (num_objects == 0) {
_location_count = 0;
return;
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
return false;
}
// get vehicle location and heading in degrees
float veh_bearing;
Location my_loc;
{
WITH_SEMAPHORE(AP::ahrs().get_semaphore());
if (!AP::ahrs().get_position(my_loc)) {
return;
}
veh_bearing = AP::ahrs().yaw_sensor * 0.01f;
if (!AP::ahrs().get_position(current_loc)) {
return false;
}
// convert offset from vehicle position to earth frame location
const uint32_t now_ms = AP_HAL::millis();
_location_count = 0;
for (uint8_t i=0; i<num_objects; i++) {
float ang_deg, dist_m;
if (get_object_angle_and_distance(i, ang_deg, dist_m)) {
Location temp_loc = my_loc;
temp_loc.offset_bearing(wrap_180(veh_bearing + ang_deg), dist_m);
_locations[_location_count].loc = temp_loc;
_locations[_location_count].radius_m = 1;
_locations[_location_count].last_update_ms = now_ms;
_location_count++;
}
current_vehicle_bearing = AP::ahrs().yaw_sensor * 0.01f;
return true;
}
// update Object Avoidance database with Earth-frame point
void AP_Proximity_Backend::database_push(const float angle, const float distance)
{
Location current_loc;
float current_vehicle_bearing;
if (database_prepare_for_push(current_loc, current_vehicle_bearing) == true) {
database_push(angle, distance, AP_HAL::millis(), current_loc, current_vehicle_bearing);
}
}
// update Object Avoidance database with Earth-frame point
void AP_Proximity_Backend::database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location &current_loc, const float current_vehicle_bearing)
{
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
return;
}
Location temp_loc = current_loc;
temp_loc.offset_bearing(wrap_180(current_vehicle_bearing + angle), distance);
oaDb->queue_push(temp_loc, timestamp_ms, distance, angle);
}

20
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -17,6 +17,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity.h"
#include <AP_Common/Location.h>
#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors
#define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary.
@ -64,12 +65,6 @@ public:
// get distances in 8 directions. used for sending distances to ground station
bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;
// copy location points around vehicle into a buffer owned by the caller
// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
// num_copied is updated with the number of locations copied into the buffer
// returns true on success, false on failure which should only happen if buff is nullptr
bool copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
protected:
// set status and update valid_count
@ -85,15 +80,17 @@ protected:
// update boundary points used for object avoidance based on a single sector's distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void update_boundary_for_sector(uint8_t sector);
void update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB);
// get ignore area info
uint8_t get_ignore_area_count() const;
bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const;
bool get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const;
// earth frame objects
void update_locations();
// database helpers
bool database_prepare_for_push(Location &current_loc, float &current_vehicle_bearing);
void database_push(const float angle, const float distance);
void database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location &current_loc, const float current_vehicle_bearing);
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state
@ -111,9 +108,4 @@ protected:
// fence boundary
Vector2f _sector_edge_vector[PROXIMITY_SECTORS_MAX]; // vector for right-edge of each sector, used to speed up calculation of boundary
Vector2f _boundary_point[PROXIMITY_SECTORS_MAX]; // bounding polygon around the vehicle calculated conservatively for object avoidance
// earth frame locations (i.e. detected obstacles stored as lat/lon points)
uint16_t _location_count; // number of locations held in _locations buffer
AP_Proximity::Proximity_Location _locations[PROXIMITY_SECTORS_MAX]; // buffer of locations
HAL_Semaphore_Recursive _rsem; // semaphore for access to _locations and _location_count
};

2
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -418,7 +418,7 @@ bool AP_Proximity_LightWareSF40C::process_reply()
_last_distance_received_ms = AP_HAL::millis();
success = true;
// update boundary used for avoidance
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
break;
}

25
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -67,17 +67,17 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
uint8_t sector = packet.orientation;
_angle[sector] = sector * 45;
_distance[sector] = packet.current_distance / 100.0f;
_distance_min = packet.min_distance / 100.0f;
_distance_max = packet.max_distance / 100.0f;
_distance[sector] = packet.current_distance * 0.01f;
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
_last_update_ms = AP_HAL::millis();
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
// store upward distance
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
_distance_upward = packet.current_distance / 100.0f;
_distance_upward = packet.current_distance * 0.01f;
_last_upward_update_ms = AP_HAL::millis();
}
return;
@ -104,8 +104,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const float total_distances = MIN(360.0f / increment, MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
// set distance min and max
_distance_min = packet.min_distance / 100.0f;
_distance_max = packet.max_distance / 100.0f;
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
_last_update_ms = AP_HAL::millis();
// get user configured yaw correction from front end
@ -115,6 +115,10 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
increment *= -1;
}
Location current_loc;
float current_vehicle_bearing;
const bool database_ready = database_prepare_for_push(current_loc, current_vehicle_bearing);
// initialise updated array and proximity sector angles (to closest object) and distances
bool sector_updated[_num_sectors];
float sector_width_half[_num_sectors];
@ -140,13 +144,18 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
sector_updated[i] = true;
}
}
// update Object Avoidance database with Earth-frame point
if (database_ready) {
database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_vehicle_bearing);
}
}
// update proximity sectors validity and boundary point
for (uint8_t i = 0; i < _num_sectors; i++) {
_distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
if (sector_updated[i]) {
update_boundary_for_sector(i);
update_boundary_for_sector(i, false);
}
}
}

2
libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp
View File

@ -69,7 +69,7 @@ void AP_Proximity_MorseSITL::update(void)
_distance_valid[sector] = true;
_distance[sector] = range;
_angle[sector] = angle_deg;
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
}

2
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -416,7 +416,7 @@ void AP_Proximity_RPLidarA2::parse_response_data()
_distance[_last_sector] = _distance_m_last;
_distance_valid[_last_sector] = true;
// update boundary used for avoidance
update_boundary_for_sector(_last_sector);
update_boundary_for_sector(_last_sector, true);
// initialize the new sector
_last_sector = sector;
_distance_m_last = distance_m;

8
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -52,12 +52,12 @@ void AP_Proximity_RangeFinder::update(void)
if (sensor->orientation() <= ROTATION_YAW_315) {
uint8_t sector = (uint8_t)sensor->orientation();
_angle[sector] = sector * 45;
_distance[sector] = sensor->distance_cm() / 100.0f;
_distance_min = sensor->min_distance_cm() / 100.0f;
_distance_max = sensor->max_distance_cm() / 100.0f;
_distance[sector] = sensor->distance_cm() * 0.01f;
_distance_min = sensor->min_distance_cm() * 0.01f;
_distance_max = sensor->max_distance_cm() * 0.01f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
_last_update_ms = now;
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
// check upward facing range finder
if (sensor->orientation() == ROTATION_PITCH_90) {

2
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -57,7 +57,7 @@ void AP_Proximity_SITL::update(void)
set_status(AP_Proximity::Proximity_Good);
_distance_valid[last_sector] = true;
_angle[last_sector] = _sector_middle_deg[last_sector];
update_boundary_for_sector(last_sector);
update_boundary_for_sector(last_sector, true);
} else {
_distance_valid[last_sector] = false;
}

2
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

@ -124,6 +124,6 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_
_distance_valid[sector] = distance_cm != 0xffff;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
}

2
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -178,6 +178,6 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
_distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector);
update_boundary_for_sector(sector, true);
}
}