From e9685ba13e6e6eebe13b038645916ff3b6f88a31 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Tue, 30 May 2017 12:33:37 +0200
Subject: [PATCH] GCS_MAVLink: add rangefinder msg

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

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index e9a86c1ec0..d3458d84a6 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -145,6 +145,7 @@ public:
     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_rangefinder(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 1e39c792e1..1c374ca4c9 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -256,6 +256,19 @@ void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder)
     send_distance_sensor(rangefinder, instance);
 }
 
+void GCS_MAVLINK::send_rangefinder(const RangeFinder &rangefinder) const
+{
+    // exit immediately if rangefinder is disabled or not downward looking
+    if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
+        // no sonar to report
+        return;
+    }
+    mavlink_msg_rangefinder_send(
+            chan,
+            rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
+            rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
+}
+
 // report AHRS2 state
 void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
 {