diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 48e9f6b0b2..58088444ad 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -306,3 +306,31 @@ bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const // get closest object from backend 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(); +} diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 32390a00fe..6c4c5f2517 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -80,6 +80,19 @@ public: // returns true on success, false if no valid readings 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 struct Proximity_State { uint8_t instance; // the instance number of this proximity sensor diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index f3bb390c85..e0fb9e795f 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -65,6 +65,55 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance) 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 // 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 diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index c8ce801c14..2dc05f3c08 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -33,6 +33,10 @@ public: // update the state structure 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) // returns true on successful read and places distance in distance bool get_horizontal_distance(float angle_deg, float &distance) const; @@ -45,6 +49,9 @@ public: // returns true on success, false if no valid readings 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: // set status and update valid_count diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index daa6a650cb..ade74e8ce0 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -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) bool AP_Proximity_LightWareSF40C::initialise() { diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index 1cfb6ba958..930f366f3e 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h @@ -18,6 +18,10 @@ public: // update state void update(void); + // get maximum and minimum distances (in meters) of sensor + float distance_max() const; + float distance_min() const; + private: enum RequestType { diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index cd364ba9a5..7289b4f8c0 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -22,8 +22,8 @@ extern const AP_HAL::HAL& hal; -#define PROXIMITY_MAX_RANGE 200 -#define PROXIMITY_ACCURACY 0.1 +#define PROXIMITY_MAX_RANGE 200.0f +#define PROXIMITY_ACCURACY 0.1f /* 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; } +// 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 diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index 2d829853b8..81da426627 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -16,6 +16,10 @@ public: // update state void update(void) override; + // get maximum and minimum distances (in meters) of sensor + float distance_max() const; + float distance_min() const; + private: SITL::SITL *sitl; Vector2l *fence;