mirror of https://github.com/ArduPilot/ardupilot
AP_Proximity: add get distances, distance_max, distance_min
These are used to send distance-sensor messages to the ground station
This commit is contained in:
parent
218c8b2d16
commit
57ae14ab4c
|
@ -306,3 +306,31 @@ bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
|
||||||
// get closest object from backend
|
// get closest object from backend
|
||||||
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
|
return drivers[primary_instance]->get_closest_object(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
|
||||||
|
{
|
||||||
|
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
// get maximum distance from backend
|
||||||
|
return drivers[primary_instance]->get_distances(prx_dist_array);
|
||||||
|
}
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of primary sensor
|
||||||
|
float AP_Proximity::distance_max() const
|
||||||
|
{
|
||||||
|
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
// get maximum distance from backend
|
||||||
|
return drivers[primary_instance]->distance_max();
|
||||||
|
}
|
||||||
|
float AP_Proximity::distance_min() const
|
||||||
|
{
|
||||||
|
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
// get minimum distance from backend
|
||||||
|
return drivers[primary_instance]->distance_min();
|
||||||
|
}
|
||||||
|
|
|
@ -80,6 +80,19 @@ public:
|
||||||
// returns true on success, false if no valid readings
|
// returns true on success, false if no valid readings
|
||||||
bool get_closest_object(float& angle_deg, float &distance) const;
|
bool get_closest_object(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)
|
||||||
|
float distance[8]; // distance in meters
|
||||||
|
};
|
||||||
|
|
||||||
|
// get distances in 8 directions. used for sending distances to ground station
|
||||||
|
bool get_distances(Proximity_Distance_Array &prx_dist_array) const;
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of primary sensor
|
||||||
|
float distance_max() const;
|
||||||
|
float distance_min() const;
|
||||||
|
|
||||||
// The Proximity_State structure is filled in by the backend driver
|
// The Proximity_State structure is filled in by the backend driver
|
||||||
struct Proximity_State {
|
struct Proximity_State {
|
||||||
uint8_t instance; // the instance number of this proximity sensor
|
uint8_t instance; // the instance number of this proximity sensor
|
||||||
|
|
|
@ -65,6 +65,55 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
|
||||||
return sector_found;
|
return sector_found;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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
|
||||||
|
{
|
||||||
|
// exit immediately if we have no good ranges
|
||||||
|
bool valid_distances = false;
|
||||||
|
for (uint8_t i=0; i<_num_sectors; i++) {
|
||||||
|
if (_distance_valid[i]) {
|
||||||
|
valid_distances = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!valid_distances) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// initialise orientations and directions
|
||||||
|
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
|
||||||
|
// distances initialised to maximum distances
|
||||||
|
bool dist_set[8];
|
||||||
|
for (uint8_t i=0; i<8; i++) {
|
||||||
|
prx_dist_array.orientation[i] = i;
|
||||||
|
prx_dist_array.distance[i] = distance_max();
|
||||||
|
dist_set[i] = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// cycle through all sectors filling in distances
|
||||||
|
for (uint8_t i=0; i<_num_sectors; i++) {
|
||||||
|
if (_distance_valid[i]) {
|
||||||
|
// convert angle to orientation
|
||||||
|
int16_t orientation = _angle[i] / 45;
|
||||||
|
if ((orientation >= 0) && (orientation < 8) && (_distance[i] < prx_dist_array.distance[orientation])) {
|
||||||
|
prx_dist_array.distance[orientation] = _distance[i];
|
||||||
|
dist_set[orientation] = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// fill in missing orientations with average of adjacent orientations if necessary and possible
|
||||||
|
for (uint8_t i=0; i<8; i++) {
|
||||||
|
if (!dist_set[i]) {
|
||||||
|
uint8_t orient_before = (i==0) ? 7 : (i-1);
|
||||||
|
uint8_t orient_after = (i==7) ? 0 : (i+1);
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
// get boundary points around vehicle for use by avoidance
|
// get boundary points around vehicle for use by avoidance
|
||||||
// returns nullptr and sets num_points to zero if no boundary can be returned
|
// returns nullptr and sets num_points to zero if no boundary can be returned
|
||||||
const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) const
|
const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) const
|
||||||
|
|
|
@ -33,6 +33,10 @@ public:
|
||||||
// update the state structure
|
// update the state structure
|
||||||
virtual void update() = 0;
|
virtual void update() = 0;
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of sensor
|
||||||
|
virtual float distance_max() const = 0;
|
||||||
|
virtual float distance_min() const = 0;
|
||||||
|
|
||||||
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
|
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
|
||||||
// returns true on successful read and places distance in distance
|
// returns true on successful read and places distance in distance
|
||||||
bool get_horizontal_distance(float angle_deg, float &distance) const;
|
bool get_horizontal_distance(float angle_deg, float &distance) const;
|
||||||
|
@ -45,6 +49,9 @@ public:
|
||||||
// returns true on success, false if no valid readings
|
// returns true on success, false if no valid readings
|
||||||
bool get_closest_object(float& angle_deg, float &distance) const;
|
bool get_closest_object(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;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
// set status and update valid_count
|
// set status and update valid_count
|
||||||
|
|
|
@ -69,6 +69,16 @@ void AP_Proximity_LightWareSF40C::update(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of primary sensor
|
||||||
|
float AP_Proximity_LightWareSF40C::distance_max() const
|
||||||
|
{
|
||||||
|
return 100.0f;
|
||||||
|
}
|
||||||
|
float AP_Proximity_LightWareSF40C::distance_min() const
|
||||||
|
{
|
||||||
|
return 0.20f;
|
||||||
|
}
|
||||||
|
|
||||||
// initialise sensor (returns true if sensor is succesfully initialised)
|
// initialise sensor (returns true if sensor is succesfully initialised)
|
||||||
bool AP_Proximity_LightWareSF40C::initialise()
|
bool AP_Proximity_LightWareSF40C::initialise()
|
||||||
{
|
{
|
||||||
|
|
|
@ -18,6 +18,10 @@ public:
|
||||||
// update state
|
// update state
|
||||||
void update(void);
|
void update(void);
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of sensor
|
||||||
|
float distance_max() const;
|
||||||
|
float distance_min() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
enum RequestType {
|
enum RequestType {
|
||||||
|
|
|
@ -22,8 +22,8 @@
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
#define PROXIMITY_MAX_RANGE 200
|
#define PROXIMITY_MAX_RANGE 200.0f
|
||||||
#define PROXIMITY_ACCURACY 0.1
|
#define PROXIMITY_ACCURACY 0.1f
|
||||||
|
|
||||||
/*
|
/*
|
||||||
The constructor also initialises the proximity sensor.
|
The constructor also initialises the proximity sensor.
|
||||||
|
@ -115,4 +115,14 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of primary sensor
|
||||||
|
float AP_Proximity_SITL::distance_max() const
|
||||||
|
{
|
||||||
|
return PROXIMITY_MAX_RANGE;
|
||||||
|
}
|
||||||
|
float AP_Proximity_SITL::distance_min() const
|
||||||
|
{
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
#endif // CONFIG_HAL_BOARD
|
||||||
|
|
|
@ -16,6 +16,10 @@ public:
|
||||||
// update state
|
// update state
|
||||||
void update(void) override;
|
void update(void) override;
|
||||||
|
|
||||||
|
// get maximum and minimum distances (in meters) of sensor
|
||||||
|
float distance_max() const;
|
||||||
|
float distance_min() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
SITL::SITL *sitl;
|
SITL::SITL *sitl;
|
||||||
Vector2l *fence;
|
Vector2l *fence;
|
||||||
|
|
Loading…
Reference in New Issue