AP_Proximity: add get distances, distance_max, distance_min

These are used to send distance-sensor messages to the ground station

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();
+}

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

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

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

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()
 {

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 {

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

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;