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

AP_Proximity: always use 8 sectors and fix ignore areas

This commit is contained in:
Randy Mackay 2019-12-04 11:46:03 +09:00 committed by Andrew Tridgell
parent 6523bae938
commit 43cd419dcf
14 changed files with 78 additions and 361 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
View File

@ -38,10 +38,6 @@ void AP_Proximity_AirSimSITL::update(void)
memset(_angle, 0, sizeof(_angle));
memset(_distance, 0, sizeof(_distance));
// only use 8 sectors to match RPLidar
const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX);
const uint16_t degrees_per_sector = 360 / nsectors;
for (uint16_t i=0; i<points.length; i++) {
Vector3f &point = points.data[i];
if (point.is_zero()) {
@ -49,7 +45,7 @@ void AP_Proximity_AirSimSITL::update(void)
}
float angle_deg = wrap_360(degrees(atan2f(-point.y, point.x)));
uint16_t angle_rounded = uint16_t(angle_deg+0.5);
uint8_t sector = wrap_360(angle_rounded + 22.5f) / degrees_per_sector;
const uint8_t sector = convert_angle_to_sector(angle_rounded);
if (!_distance_valid[sector] || PROXIMITY_MAX_RANGE < _distance[sector]) {
_distance_valid[sector] = true;
const Vector2f v = Vector2f(point.x, point.y);
@ -61,7 +57,7 @@ void AP_Proximity_AirSimSITL::update(void)
#if 0
printf("npoints=%u\n", points.length);
for (uint16_t i=0; i<nsectors; i++) {
for (uint16_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]);
}
#endif

126
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -36,12 +36,10 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity
// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool AP_Proximity_Backend::get_horizontal_distance(float angle_deg, float &distance) const
{
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
if (_distance_valid[sector]) {
distance = _distance[sector];
return true;
}
const uint8_t sector = convert_angle_to_sector(angle_deg);
if (_distance_valid[sector]) {
distance = _distance[sector];
return true;
}
return false;
}
@ -54,7 +52,7 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
uint8_t sector = 0;
// check all sectors for shorter distance
for (uint8_t i=0; i<_num_sectors; i++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
if (!sector_found || (_distance[i] < _distance[sector])) {
sector = i;
@ -73,14 +71,14 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity_Backend::get_object_count() const
{
return _num_sectors;
return PROXIMITY_NUM_SECTORS;
}
// get an object's angle and distance, used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
bool AP_Proximity_Backend::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if (object_number < _num_sectors && _distance_valid[object_number]) {
if (object_number < PROXIMITY_NUM_SECTORS && _distance_valid[object_number]) {
angle_deg = _angle[object_number];
distance = _distance[object_number];
return true;
@ -93,7 +91,7 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
{
// exit immediately if we have no good ranges
bool valid_distances = false;
for (uint8_t i=0; i<_num_sectors; i++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
valid_distances = true;
break;
@ -113,7 +111,7 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
}
// cycle through all sectors filling in distances
for (uint8_t i=0; i<_num_sectors; i++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
// convert angle to orientation
int16_t orientation = static_cast<int16_t>(_angle[i] * (PROXIMITY_MAX_DIRECTION / 360.0f));
@ -149,7 +147,7 @@ const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points)
// check at least one sector has valid data, if not, exit
bool some_valid = false;
for (uint8_t i=0; i<_num_sectors; i++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
some_valid = true;
break;
@ -161,7 +159,7 @@ const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points)
}
// return boundary points
num_points = _num_sectors;
num_points = PROXIMITY_NUM_SECTORS;
return _boundary_point;
}
@ -169,8 +167,8 @@ const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points)
// should be called if the sector_middle_deg or _setor_width_deg arrays are changed
void AP_Proximity_Backend::init_boundary()
{
for (uint8_t sector=0; sector < _num_sectors; sector++) {
float angle_rad = radians((float)_sector_middle_deg[sector]+(float)_sector_width_deg[sector]/2.0f);
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
float angle_rad = radians((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
_sector_edge_vector[sector].x = cosf(angle_rad) * 100.0f;
_sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
_boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
@ -183,7 +181,7 @@ void AP_Proximity_Backend::init_boundary()
void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB)
{
// sanity check
if (sector >= _num_sectors) {
if (sector >= PROXIMITY_NUM_SECTORS) {
return;
}
@ -193,7 +191,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons
// find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= _num_sectors) {
if (next_sector >= PROXIMITY_NUM_SECTORS) {
next_sector = 0;
}
@ -217,7 +215,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons
}
// repeat for edge between sector and previous sector
uint8_t prev_sector = (sector == 0) ? _num_sectors-1 : sector-1;
uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector] && _distance_valid[sector]) {
shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
@ -229,7 +227,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons
_boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance;
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary
uint8_t prev_sector_ccw = (prev_sector == 0) ? _num_sectors-1 : prev_sector-1;
uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1;
if (!_distance_valid[prev_sector_ccw]) {
_boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance;
}
@ -241,97 +239,23 @@ void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
state.status = status;
}
bool AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees, uint8_t &sector) const
uint8_t AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees) const
{
// sanity check angle
if (angle_degrees > 360.0f || angle_degrees < -180.0f) {
return false;
}
// convert to 0 ~ 360
if (angle_degrees < 0.0f) {
angle_degrees += 360.0f;
}
bool closest_found = false;
uint8_t closest_sector;
float closest_angle;
// search for which sector angle_degrees falls into
for (uint8_t i = 0; i < _num_sectors; i++) {
float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - angle_degrees));
// record if closest
if (!closest_found || angle_diff < closest_angle) {
closest_found = true;
closest_sector = i;
closest_angle = angle_diff;
}
if (fabsf(angle_diff) <= _sector_width_deg[i] / 2.0f) {
sector = i;
return true;
}
}
// angle_degrees might have been within a gap between sectors
if (closest_found) {
sector = closest_sector;
return true;
}
return false;
return wrap_360(angle_degrees + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;
}
// get ignore area info
uint8_t AP_Proximity_Backend::get_ignore_area_count() const
// check if a reading should be ignored because it falls into an ignore area
bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
{
// count number of ignore sectors
uint8_t count = 0;
// check angle vs each ignore area
for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
if (frontend._ignore_width_deg[i] != 0) {
count++;
}
}
return count;
}
// get next ignore angle
bool AP_Proximity_Backend::get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const
{
if (index >= PROXIMITY_MAX_IGNORE) {
return false;
}
angle_deg = frontend._ignore_angle_deg[index];
width_deg = frontend._ignore_width_deg[index];
return true;
}
// retrieve start or end angle of next ignore area (i.e. closest ignore area higher than the start_angle)
// start_or_end = 0 to get start, 1 to retrieve end
bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const
{
bool found = false;
int16_t smallest_angle_diff = 0;
int16_t smallest_angle_start = 0;
for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
if (frontend._ignore_width_deg[i] != 0) {
int16_t offset = start_or_end == 0 ? -frontend._ignore_width_deg[i] : +frontend._ignore_width_deg[i];
int16_t ignore_start_angle = wrap_360(frontend._ignore_angle_deg[i] + offset/2.0f);
int16_t ang_diff = wrap_360(ignore_start_angle - start_angle);
if (!found || ang_diff < smallest_angle_diff) {
smallest_angle_diff = ang_diff;
smallest_angle_start = ignore_start_angle;
found = true;
if (abs(angle_deg - frontend._ignore_width_deg[i]) <= (frontend._ignore_width_deg[i]/2)) {
return true;
}
}
}
if (found) {
ignore_start = smallest_angle_start;
}
return found;
return false;
}
// returns true if database is ready to be pushed to and all cached data is ready

26
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -19,7 +19,8 @@
#include "AP_Proximity.h"
#include <AP_Common/Location.h>
#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors
#define PROXIMITY_NUM_SECTORS 8 // number of sectors
#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // width of sectors in degrees
#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.
#define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out
@ -71,7 +72,7 @@ protected:
void set_status(AP_Proximity::Status status);
// find which sector a given angle falls into
bool convert_angle_to_sector(float angle_degrees, uint8_t &sector) const;
uint8_t convert_angle_to_sector(float angle_degrees) const;
// initialise the boundary and sector_edge_vector array used for object avoidance
// should be called if the sector_middle_deg or _setor_width_deg arrays are changed
@ -82,10 +83,9 @@ protected:
// 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(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;
// check if a reading should be ignored because it falls into an ignore area
// angles should be in degrees and in the range of 0 to 360
bool ignore_reading(uint16_t angle_deg) const;
// database helpers. all angles are in degrees
bool database_prepare_for_push(Location &current_loc, float &current_heading);
@ -96,16 +96,14 @@ protected:
AP_Proximity::Proximity_State &state; // reference to this instances state
// sectors
uint8_t _num_sectors = PROXIMITY_MAX_DIRECTION;
uint16_t _sector_middle_deg[PROXIMITY_SECTORS_MAX] = {0, 45, 90, 135, 180, 225, 270, 315, 0, 0, 0, 0}; // middle angle of each sector
uint8_t _sector_width_deg[PROXIMITY_SECTORS_MAX] = {45, 45, 45, 45, 45, 45, 45, 45, 0, 0, 0, 0}; // width (in degrees) of each sector
const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] = {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
// sensor data
float _angle[PROXIMITY_SECTORS_MAX]; // angle to closest object within each sector
float _distance[PROXIMITY_SECTORS_MAX]; // distance to closest object within each sector
bool _distance_valid[PROXIMITY_SECTORS_MAX]; // true if a valid distance received for each sector
float _angle[PROXIMITY_NUM_SECTORS]; // angle to closest object within each sector
float _distance[PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector
bool _distance_valid[PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector
// 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
Vector2f _sector_edge_vector[PROXIMITY_NUM_SECTORS]; // vector for right-edge of each sector, used to speed up calculation of boundary
Vector2f _boundary_point[PROXIMITY_NUM_SECTORS]; // bounding polygon around the vehicle calculated conservatively for object avoidance
};

70
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -73,10 +73,8 @@ void AP_Proximity_LightWareSF40C::update(void)
// initialise sensor
void AP_Proximity_LightWareSF40C::initialise()
{
// initialise sectors
if (!_sector_initialised) {
init_sectors();
}
// initialise boundary
init_boundary();
// exit immediately if we've sent initialisation requests in the last second
uint32_t now_ms = AP_HAL::millis();
@ -137,66 +135,6 @@ void AP_Proximity_LightWareSF40C::restart_sensor()
_sensor_state.output_rate = 0;
}
// initialise sector angles using user defined ignore areas
void AP_Proximity_LightWareSF40C::init_sectors()
{
// use defaults if no ignore areas defined
uint8_t ignore_area_count = get_ignore_area_count();
if (ignore_area_count == 0) {
_sector_initialised = true;
return;
}
uint8_t sector = 0;
for (uint8_t i=0; i<ignore_area_count; i++) {
// get ignore area info
uint16_t ign_area_angle;
uint8_t ign_area_width;
if (get_ignore_area(i, ign_area_angle, ign_area_width)) {
// calculate how many degrees of space we have between this end of this ignore area and the start of the end
int16_t start_angle, end_angle;
get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
get_next_ignore_start_or_end(0, start_angle, end_angle);
int16_t degrees_to_fill = wrap_360(end_angle - start_angle);
// divide up the area into sectors
while ((degrees_to_fill > 0) && (sector < PROXIMITY_SECTORS_MAX)) {
uint16_t sector_size;
if (degrees_to_fill >= 90) {
// set sector to maximum of 45 degrees
sector_size = 45;
} else if (degrees_to_fill > 45) {
// use half the remaining area to optimise size of this sector and the next
sector_size = degrees_to_fill / 2.0f;
} else {
// 45 degrees or less are left so put it all into the next sector
sector_size = degrees_to_fill;
}
// record the sector middle and width
_sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f);
_sector_width_deg[sector] = sector_size;
// move onto next sector
start_angle += sector_size;
sector++;
degrees_to_fill -= sector_size;
}
}
}
// set num sectors
_num_sectors = sector;
// re-initialise boundary because sector locations have changed
init_boundary();
// record success
_sector_initialised = true;
}
// send message to sensor
void AP_Proximity_LightWareSF40C::send_message(MessageID msgid, bool write, const uint8_t *payload, uint16_t payload_len)
{
@ -404,8 +342,8 @@ void AP_Proximity_LightWareSF40C::process_message()
const uint16_t idx = 14 + (i * 2);
const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
if (!ignore_reading(angle_deg)) {
const uint8_t sector = convert_angle_to_sector(angle_deg);
if (sector != _last_sector) {
// update boundary used for avoidance
if (_last_sector != UINT8_MAX) {

2
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
View File

@ -28,7 +28,6 @@ private:
// initialise sensor
void initialise();
void init_sectors();
// restart sensor and re-init our state
void restart_sensor();
@ -106,7 +105,6 @@ private:
// internal variables
AP_HAL::UARTDriver *_uart; // uart for communicating with sensor
bool _sector_initialised; // true if sectors have been initialised
uint32_t _last_request_ms; // system time of last request
uint32_t _last_reply_ms; // system time of last valid reply
uint32_t _last_restart_ms; // system time we restarted the sensor

75
libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp
View File

@ -101,72 +101,11 @@ bool AP_Proximity_LightWareSF40C_v09::initialise()
}
return false;
}
// initialise sectors
if (!_sector_initialised) {
init_sectors();
return false;
}
return true;
}
// initialise sector angles using user defined ignore areas
void AP_Proximity_LightWareSF40C_v09::init_sectors()
{
// use defaults if no ignore areas defined
uint8_t ignore_area_count = get_ignore_area_count();
if (ignore_area_count == 0) {
_sector_initialised = true;
return;
}
uint8_t sector = 0;
for (uint8_t i=0; i<ignore_area_count; i++) {
// get ignore area info
uint16_t ign_area_angle;
uint8_t ign_area_width;
if (get_ignore_area(i, ign_area_angle, ign_area_width)) {
// calculate how many degrees of space we have between this end of this ignore area and the start of the end
int16_t start_angle, end_angle;
get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
get_next_ignore_start_or_end(0, start_angle, end_angle);
int16_t degrees_to_fill = wrap_360(end_angle - start_angle);
// divide up the area into sectors
while ((degrees_to_fill > 0) && (sector < PROXIMITY_SECTORS_MAX)) {
uint16_t sector_size;
if (degrees_to_fill >= 90) {
// set sector to maximum of 45 degrees
sector_size = 45;
} else if (degrees_to_fill > 45) {
// use half the remaining area to optimise size of this sector and the next
sector_size = degrees_to_fill / 2.0f;
} else {
// 45 degrees or less are left so put it all into the next sector
sector_size = degrees_to_fill;
}
// record the sector middle and width
_sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f);
_sector_width_deg[sector] = sector_size;
// move onto next sector
start_angle += sector_size;
sector++;
degrees_to_fill -= sector_size;
}
}
}
// set num sectors
_num_sectors = sector;
// re-initialise boundary because sector locations have changed
init_boundary();
// record success
_sector_initialised = true;
return true;
}
// set speed of rotating motor
@ -281,15 +220,15 @@ bool AP_Proximity_LightWareSF40C_v09::send_request_for_distance()
// increment sector
_last_sector++;
if (_last_sector >= _num_sectors) {
if (_last_sector >= PROXIMITY_NUM_SECTORS) {
_last_sector = 0;
}
// prepare request
char request_str[16];
snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n",
MIN(_sector_width_deg[_last_sector], 999),
MIN(_sector_middle_deg[_last_sector], 999));
(unsigned int)PROXIMITY_SECTOR_WIDTH_DEG,
_sector_middle_deg[_last_sector]);
uart->write(request_str);
@ -410,8 +349,8 @@ bool AP_Proximity_LightWareSF40C_v09::process_reply()
{
float angle_deg = (float)atof(element_buf[0]);
float distance_m = (float)atof(element_buf[1]);
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
if (!ignore_reading(angle_deg)) {
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = distance_m;
_distance_valid[sector] = is_positive(distance_m);

2
libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h
View File

@ -36,7 +36,6 @@ private:
// initialise sensor (returns true if sensor is successfully initialised)
bool initialise();
void init_sectors();
void set_motor_speed(bool on_off);
void set_motor_direction();
void set_forward_direction();
@ -93,5 +92,4 @@ private:
uint8_t _motor_speed; // motor speed as reported by lidar
uint8_t _motor_direction = 99; // motor direction as reported by lidar
int16_t _forward_direction = 999; // forward direction as reported by lidar
bool _sector_initialised = false;
};

12
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -109,11 +109,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const bool database_ready = database_prepare_for_push(current_loc, current_heading);
// initialise updated array and proximity sector angles (to closest object) and distances
bool sector_updated[_num_sectors];
float sector_width_half[_num_sectors];
for (uint8_t i = 0; i < _num_sectors; i++) {
bool sector_updated[PROXIMITY_NUM_SECTORS];
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
sector_updated[i] = false;
sector_width_half[i] = _sector_width_deg[i] * 0.5f;
_angle[i] = _sector_middle_deg[i];
_distance[i] = MAX_DISTANCE;
}
@ -134,10 +132,10 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const float mid_angle = wrap_360((float)j * increment + yaw_correction);
// iterate over proximity sectors
for (uint8_t i = 0; i < _num_sectors; i++) {
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - mid_angle));
// update distance array sector with shortest distance from message
if ((angle_diff <= sector_width_half[i]) && (packet_distance_m < _distance[i])) {
if ((angle_diff <= (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) && (packet_distance_m < _distance[i])) {
_distance[i] = packet_distance_m;
_angle[i] = mid_angle;
sector_updated[i] = true;
@ -151,7 +149,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
}
// update proximity sectors validity and boundary point
for (uint8_t i = 0; i < _num_sectors; i++) {
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
_distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
if (sector_updated[i]) {
update_boundary_for_sector(i, false);

8
libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp
View File

@ -41,10 +41,6 @@ void AP_Proximity_MorseSITL::update(void)
memset(_angle, 0, sizeof(_angle));
memset(_distance, 0, sizeof(_distance));
// only use 8 sectors to match RPLidar
const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX);
const uint16_t degrees_per_sector = 360 / nsectors;
for (uint16_t i=0; i<points.length; i++) {
Vector3f &point = points.data[i];
float &range = ranges.data[i];
@ -54,7 +50,7 @@ void AP_Proximity_MorseSITL::update(void)
}
float angle_deg = wrap_360(degrees(atan2f(-point.y, point.x)));
uint16_t angle_rounded = uint16_t(angle_deg+0.5);
uint8_t sector = wrap_360(angle_rounded + 22.5f) / degrees_per_sector;
const uint8_t sector = convert_angle_to_sector(angle_rounded);
if (!_distance_valid[sector] || range < _distance[sector]) {
_distance_valid[sector] = true;
_distance[sector] = range;
@ -65,7 +61,7 @@ void AP_Proximity_MorseSITL::update(void)
#if 0
printf("npoints=%u\n", points.length);
for (uint16_t i=0; i<nsectors; i++) {
for (uint16_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]);
}
#endif

72
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -130,15 +130,12 @@ float AP_Proximity_RPLidarA2::distance_min() const
bool AP_Proximity_RPLidarA2::initialise()
{
// initialise sectors
if (!_sector_initialised) {
init_sectors();
return false;
}
// initialise boundary
init_boundary();
if (!_initialised) {
reset_rplidar(); // set to a known state
Debug(1, "LIDAR initialised");
return true;
}
return true;
@ -159,65 +156,6 @@ void AP_Proximity_RPLidarA2::reset_rplidar()
}
// initialise sector angles using user defined ignore areas, left same as SF40C
void AP_Proximity_RPLidarA2::init_sectors()
{
// use defaults if no ignore areas defined
const uint8_t ignore_area_count = get_ignore_area_count();
if (ignore_area_count == 0) {
_sector_initialised = true;
return;
}
uint8_t sector = 0;
for (uint8_t i=0; i<ignore_area_count; i++) {
// get ignore area info
uint16_t ign_area_angle;
uint8_t ign_area_width;
if (get_ignore_area(i, ign_area_angle, ign_area_width)) {
// calculate how many degrees of space we have between this end of this ignore area and the start of the end
int16_t start_angle, end_angle;
get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
get_next_ignore_start_or_end(0, start_angle, end_angle);
int16_t degrees_to_fill = wrap_360(end_angle - start_angle);
// divide up the area into sectors
while ((degrees_to_fill > 0) && (sector < PROXIMITY_SECTORS_MAX)) {
uint16_t sector_size;
if (degrees_to_fill >= 90) {
// set sector to maximum of 45 degrees
sector_size = 45;
} else if (degrees_to_fill > 45) {
// use half the remaining area to optimise size of this sector and the next
sector_size = degrees_to_fill / 2.0f;
} else {
// 45 degrees or less are left so put it all into the next sector
sector_size = degrees_to_fill;
}
// record the sector middle and width
_sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f);
_sector_width_deg[sector] = sector_size;
// move onto next sector
start_angle += sector_size;
sector++;
degrees_to_fill -= sector_size;
}
}
}
// set num sectors
_num_sectors = sector;
// re-initialise boundary because sector locations have changed
init_boundary();
// record success
_sector_initialised = true;
}
// set Lidar into SCAN mode
void AP_Proximity_RPLidarA2::set_scan_mode()
{
@ -403,8 +341,8 @@ void AP_Proximity_RPLidarA2::parse_response_data()
Debug(2, " D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality);
#endif
_last_distance_received_ms = AP_HAL::millis();
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
if (!ignore_reading(angle_deg)) {
const uint8_t sector = convert_angle_to_sector(angle_deg);
if (distance_m > distance_min()) {
if (_last_sector == sector) {
if (_distance_m_last > distance_m) {

2
libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
View File

@ -69,7 +69,6 @@ private:
// initialise sensor (returns true if sensor is successfully initialised)
bool initialise();
void init_sectors();
void set_scan_mode();
// send request for something from sensor
@ -87,7 +86,6 @@ private:
bool _information_data;
bool _resetted;
bool _initialised;
bool _sector_initialised;
uint8_t _payload_length;
uint8_t _cnt;

2
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -62,7 +62,7 @@ void AP_Proximity_SITL::update(void)
_distance_valid[last_sector] = false;
}
last_sector++;
if (last_sector >= _num_sectors) {
if (last_sector >= PROXIMITY_NUM_SECTORS) {
last_sector = 0;
}
} else {

16
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

@ -119,13 +119,11 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
// process reply
void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
_distance_valid[sector] = distance_cm != 0xffff;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
}
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
_distance_valid[sector] = distance_cm != 0xffff;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
}

18
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -168,15 +168,13 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
// process reply
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
//check for target too far, target too close and sensor not connected
_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, true);
}
//check for target too far, target too close and sensor not connected
_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, true);
}