Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: add PROXIMITY_MAX_DIRECTION and PROXIMITY_SENSOR_ID_START

Browse Source
This commit is contained in:
Pierre Kancir 2017-05-30 11:55:20 +02:00 committed by Francisco Ferreira
parent 8b38bd2e33
commit 38540b4dcc
3 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Proximity/AP_Proximity.h
View File

@ -24,6 +24,8 @@
#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform
#define PROXIMITY_YAW_CORRECTION_DEFAULT 22 // default correction for sensor error in yaw #define PROXIMITY_YAW_CORRECTION_DEFAULT 22 // default correction for sensor error in yaw
#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored #define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10
class AP_Proximity_Backend; class AP_Proximity_Backend;
@ -50,10 +52,10 @@ public:
Proximity_Good Proximity_Good
}; };
// structure holding distances in 8 directions. used for sending distances to ground station // structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
struct Proximity_Distance_Array { struct Proximity_Distance_Array {
uint8_t orientation[8]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION) uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[8]; // distance in meters float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
}; };
// detect and initialise any available proximity sensors // detect and initialise any available proximity sensors
@ -84,7 +86,7 @@ public:
bool get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const; bool get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const;
bool get_horizontal_distance(float angle_deg, float &distance) const; bool get_horizontal_distance(float angle_deg, float &distance) const;
// get distances in 8 directions. used for sending distances to ground station // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const; bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get boundary points around vehicle for use by avoidance // get boundary points around vehicle for use by avoidance

12
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -85,7 +85,7 @@ bool AP_Proximity_Backend::get_object_angle_and_distance(uint8_t object_number,
return false; return false;
} }
// get distances in 8 directions. used for sending distances to ground station // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
{ {
// exit immediately if we have no good ranges // exit immediately if we have no good ranges
@ -102,8 +102,8 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
// initialise orientations and directions // initialise orientations and directions
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc) // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
// distances initialised to maximum distances // distances initialised to maximum distances
bool dist_set[8]; bool dist_set[PROXIMITY_MAX_DIRECTION];
for (uint8_t i=0; i<8; i++) { for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
prx_dist_array.orientation[i] = i; prx_dist_array.orientation[i] = i;
prx_dist_array.distance[i] = distance_max(); prx_dist_array.distance[i] = distance_max();
dist_set[i] = false; dist_set[i] = false;
@ -122,10 +122,10 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
} }
// fill in missing orientations with average of adjacent orientations if necessary and possible // fill in missing orientations with average of adjacent orientations if necessary and possible
for (uint8_t i=0; i<8; i++) { for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
if (!dist_set[i]) { if (!dist_set[i]) {
uint8_t orient_before = (i==0) ? 7 : (i-1); uint8_t orient_before = (i==0) ? (PROXIMITY_MAX_DIRECTION - 1) : (i-1);
uint8_t orient_after = (i==7) ? 0 : (i+1); uint8_t orient_after = (i==(PROXIMITY_MAX_DIRECTION - 1)) ? 0 : (i+1);
if (dist_set[orient_before] && dist_set[orient_after]) { if (dist_set[orient_before] && dist_set[orient_after]) {
prx_dist_array.distance[i] = (prx_dist_array.distance[orient_before] + prx_dist_array.distance[orient_after]) / 2.0f; prx_dist_array.distance[i] = (prx_dist_array.distance[orient_before] + prx_dist_array.distance[orient_after]) / 2.0f;
} }

2
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -90,7 +90,7 @@ protected:
AP_Proximity::Proximity_State &state; // reference to this instances state AP_Proximity::Proximity_State &state; // reference to this instances state
// sectors // sectors
uint8_t _num_sectors = 8; 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 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 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