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

AP_Proximity: move copy_locations to backend 

also remove unused get_locations
also fix comment for copy_locations method
This commit is contained in:
Randy Mackay 2019-05-29 15:20:48 +09:00
parent b792fe4b26
commit 9c3812e384
4 changed files with 84 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -379,76 +379,24 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
return get_boundary_points(primary_instance, num_points);
}
const AP_Proximity::Proximity_Location* AP_Proximity::get_locations(uint16_t& location_count) const
{
if (_location_count == 0) {
return nullptr;
}
location_count = _location_count;
return _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 (should only happen if there is a semaphore conflict)
bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
bool AP_Proximity::copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
{
// sanity check buffer
if (buff == nullptr) {
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
num_copied = 0;
return false;
}
WITH_SEMAPHORE(_rsem);
// copy locations into caller's buffer
num_copied = MIN(_location_count, buff_size);
for (uint16_t i=0; i<num_copied; i++) {
buff[i] = _locations[i];
}
return true;
// call backend copy_locations
return drivers[instance]->copy_locations(buff, buff_size, num_copied);
}
void AP_Proximity::locations_update()
bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
{
// exit immediately if primary sensor is disabled
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
return;
}
WITH_SEMAPHORE(_rsem);
// get number of objects, return early if none
uint8_t num_objects = get_object_count();
if (num_objects == 0) {
_location_count = 0;
return;
}
// get vehicle location and heading in degrees
Location my_loc;
if (!AP::ahrs().get_position(my_loc)) {
return;
}
const float veh_bearing = AP::ahrs().yaw_sensor * 0.01f;
// 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++;
}
}
return copy_locations(primary_instance, buff, buff_size, num_copied);
}
// get distance and angle to closest object (used for pre-arm check)

13
libraries/AP_Proximity/AP_Proximity.h
View File

@ -26,7 +26,7 @@
#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 get_locations method) are valid for this many milliseconds
#define PROXIMITY_LOCATION_TIMEOUT_MS 3000 // locations (provided by copy_locations method) are valid for this many milliseconds
class AP_Proximity_Backend;
@ -108,14 +108,11 @@ public:
const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
const Vector2f* get_boundary_points(uint16_t& num_points) const;
// get Location points around vehicle for use by avoidance in earth-frame
// returns nullptr and sets num_points to zero if no boundary can be returned
const Proximity_Location* get_locations(uint16_t& location_count) 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)
@ -177,12 +174,6 @@ private:
AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored
void detect_instance(uint8_t instance);
// earth frame objects
void locations_update();
uint16_t _location_count; // number of locations held in _locations buffer
Proximity_Location _locations[PROXIMITY_MAX_DIRECTION]; // buffer of locations
HAL_Semaphore_Recursive _rsem; // semaphore for access to _locations and _location_count
};
namespace AP {

64
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -14,6 +14,8 @@
*/
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity.h"
#include "AP_Proximity_Backend.h"
@ -172,7 +174,7 @@ 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;
}
frontend.locations_update();
update_locations();
}
// update boundary points used for object avoidance based on a single sector's distance changing
@ -227,7 +229,27 @@ 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;
}
frontend.locations_update();
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
@ -328,3 +350,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()
{
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;
}
// 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;
}
// 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++;
}
}
}

14
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -64,6 +64,12 @@ 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
@ -86,6 +92,9 @@ protected:
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();
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state
@ -102,4 +111,9 @@ 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
};