From 6b4a3ef98bf26e6b225ac38792b682fc64999fcd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 31 Oct 2013 09:54:45 +1100
Subject: [PATCH] Plane: added MAVLink sonar logging

---
 ArduPlane/GCS_Mavlink.pde | 18 ++++++++++++++++++
 ArduPlane/defines.h       |  1 +
 2 files changed, 19 insertions(+)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 5b25d5f1cb..6e4fef23b5 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -533,6 +533,18 @@ static void NOINLINE send_wind(mavlink_channel_t chan)
         wind.z);
 }
 
+static void NOINLINE send_rangefinder(mavlink_channel_t chan)
+{
+    if (!sonar.enabled()) {
+        // no sonar to report
+        return;
+    }
+    mavlink_msg_rangefinder_send(
+        chan,
+        sonar.distance_cm() * 0.01f,
+        sonar.voltage());
+}
+
 static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 {
     mavlink_msg_mission_current_send(
@@ -705,6 +717,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_hwstatus(chan);
         break;
 
+    case MSG_RANGEFINDER:
+        CHECK_PAYLOAD_SIZE(RANGEFINDER);
+        send_rangefinder(chan);
+        break;
+
     case MSG_WIND:
         CHECK_PAYLOAD_SIZE(WIND);
         send_wind(chan);
@@ -1099,6 +1116,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
         send_message(MSG_WIND);
+        send_message(MSG_RANGEFINDER);
         send_message(MSG_SYSTEM_TIME);
     }
 }
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index dc632ed3e5..e99627cff5 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -140,6 +140,7 @@ enum ap_message {
     MSG_SIMSTATE,
     MSG_HWSTATUS,
     MSG_WIND,
+    MSG_RANGEFINDER,
     MSG_RETRY_DEFERRED // this must be last
 };