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

AP_Proximity: add get_object methods for use with non-GPS avoidance

This commit is contained in:
Randy Mackay 2016-12-17 12:42:13 +09:00
parent 802206baed
commit fbbd13db75
4 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -307,6 +307,27 @@ bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
}
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity::get_object_count() const
{
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
return 0;
}
// get count from backend
return drivers[primary_instance]->get_object_count();
}
// 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::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_object_angle_and_distance(object_number, angle_deg, distance);
}
// get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity::get_distances(Proximity_Distance_Array &prx_dist_array) const
{

4
libraries/AP_Proximity/AP_Proximity.h
View File

@ -80,6 +80,10 @@ public:
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;
// get number of objects, angle and distance - used for non-GPS avoidance
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// stucture holding distances in 8 directions
struct Proximity_Distance_Array {
uint8_t orientation[8]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)

18
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -65,6 +65,24 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
return sector_found;
}
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity_Backend::get_object_count() const
{
return _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]) {
angle_deg = _angle[object_number];
distance = _distance[object_number];
return true;
}
return false;
}
// get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity_Backend::get_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
{

4
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -50,6 +50,10 @@ public:
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;
// get number of objects, angle and distance - used for non-GPS avoidance
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// get distances in 8 directions. used for sending distances to ground station
bool get_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;