AP_Proximity: add get_upward_distance

This allow support for upward facing proximity sensor received through a DISTANCE_SENSOR message.
Also added SITL test

libraries/AP_Proximity/AP_Proximity.cpp

@@ -385,3 +385,18 @@ float AP_Proximity::distance_min() const
     // get minimum distance from backend
     return drivers[primary_instance]->distance_min();
 }
+
+// get distance in meters upwards, returns true on success
+bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const
+{
+    if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
+        return false;
+    }
+    // get upward distance from backend
+    return drivers[instance]->get_upward_distance(distance);
+}
+
+bool AP_Proximity::get_upward_distance(float &distance) const
+{
+    return get_upward_distance(primary_instance, distance);
+}

libraries/AP_Proximity/AP_Proximity.h

@@ -107,6 +107,14 @@ public:
         enum Proximity_Status   status;     // sensor status
     };
 
+    //
+    // support for upwardward facing sensors
+    //
+
+    // get distance upwards in meters. returns true on success
+    bool get_upward_distance(uint8_t instance, float &distance) const;
+    bool get_upward_distance(float &distance) const;
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -39,6 +39,9 @@ public:
     virtual float distance_max() const = 0;
     virtual float distance_min() const = 0;
 
+    // get distance upwards in meters. returns true on success
+    virtual bool get_upward_distance(float &distance) const { return false; }
+
     // handle mavlink DISTANCE_SENSOR messages
     virtual void handle_msg(mavlink_message_t *msg) {}
 

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -43,6 +43,16 @@ void AP_Proximity_MAV::update(void)
     }
 }
 
+// get distance upwards in meters. returns true on success
+bool AP_Proximity_MAV::get_upward_distance(float &distance) const
+{
+    if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_MAV_TIMEOUT_MS)) {
+        distance = _distance_upward;
+        return true;
+    }
+    return false;
+}
+
 // handle mavlink DISTANCE_SENSOR messages
 void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
 {
@@ -60,4 +70,10 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
         _last_update_ms = AP_HAL::millis();
         update_boundary_for_sector(sector);
     }
+
+    // store upward distance
+    if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
+        _distance_upward = packet.current_distance / 100.0f;
+        _last_upward_update_ms = AP_HAL::millis();
+    }
 }

libraries/AP_Proximity/AP_Proximity_MAV.h

@@ -19,6 +19,9 @@ public:
     float distance_max() const { return _distance_max; }
     float distance_min() const { return _distance_min; };
 
+    // get distance upwards in meters. returns true on success
+    bool get_upward_distance(float &distance) const;
+
     // handle mavlink DISTANCE_SENSOR messages
     void handle_msg(mavlink_message_t *msg) override;
 
@@ -27,7 +30,12 @@ private:
     // initialise sensor (returns true if sensor is succesfully initialised)
     bool initialise();
 
+    // horizontal distance support
     uint32_t _last_update_ms;   // system time of last DISTANCE_SENSOR message received
     float _distance_max;        // max range of sensor in meters
     float _distance_min;        // min range of sensor in meters
+
+    // upward distance support
+    uint32_t _last_upward_update_ms;    // system time of last update distance
+    float _distance_upward;             // upward distance in meters
 };

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -38,6 +38,10 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
     if (fence_count == nullptr || ptype != AP_PARAM_INT8) {
         AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL");
     }
+    fence_alt_max = (AP_Float *)AP_Param::find("FENCE_ALT_MAX", &ptype);
+    if (fence_alt_max == nullptr || ptype != AP_PARAM_FLOAT) {
+        AP_HAL::panic("Proximity_SITL: Failed to find FENCE_ALT_MAX");
+    }
 }
 
 // update the state of the sensor
@@ -125,4 +129,12 @@ float AP_Proximity_SITL::distance_min() const
     return 0.0f;
 }
 
+// get distance upwards in meters. returns true on success
+bool AP_Proximity_SITL::get_upward_distance(float &distance) const
+{
+    // return distance to fence altitude
+    distance = MAX(0.0f, fence_alt_max->get() - sitl->height_agl);
+    return true;
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_Proximity/AP_Proximity_SITL.h

@@ -20,10 +20,14 @@ public:
     float distance_max() const;
     float distance_min() const;
 
+    // get distance upwards in meters. returns true on success
+    bool get_upward_distance(float &distance) const;
+
 private:
     SITL::SITL *sitl;
     Vector2l *fence;
     AP_Int8 *fence_count;
+    AP_Float *fence_alt_max;
     uint32_t last_load_ms;
     AC_PolyFence_loader fence_loader;
     Location current_loc;