From 0848d96354540b35f966add110593de2f787e547 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Tue, 30 May 2017 12:33:03 +0200
Subject: [PATCH] GCS_MAVLink: add send_distance_sensor_downward function

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 11 +++++++++++
 2 files changed, 12 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 29e21994f5..e9a86c1ec0 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -144,6 +144,7 @@ public:
     bool send_battery_status(const AP_BattMonitor &battery) const;
     void send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const;
     bool send_distance_sensor(const RangeFinder &rangefinder) const;
+    void send_distance_sensor_downward(const RangeFinder &rangefinder) const;
     void send_ahrs2(AP_AHRS &ahrs);
     bool send_gps_raw(AP_GPS &gps);
     void send_system_time(AP_GPS &gps);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 55c589919e..1e39c792e1 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -245,6 +245,17 @@ bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const
     return true;
 }
 
+void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder) const
+{
+    // exit immediately if rangefinder is disabled or not downward looking
+    if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
+        return;
+    }
+    uint8_t instance;
+    rangefinder.find_instance(ROTATION_PITCH_270, instance);
+    send_distance_sensor(rangefinder, instance);
+}
+
 // report AHRS2 state
 void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
 {