From 1b05a18b84a027cbbdc9df88b238ca3c808963ff Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 14 May 2018 22:20:30 -0700
Subject: [PATCH] GCS_MAVLink: Wrap sending named float values

---
 libraries/GCS_MAVLink/GCS.cpp        |  5 +++++
 libraries/GCS_MAVLink/GCS.h          |  2 ++
 libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++++++-
 3 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index 905e993556..ea90a881ec 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -29,6 +29,11 @@ void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
         }                                            \
     } while (0)
 
+void GCS::send_named_float(const char *name, float value) const
+{
+    FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value));
+}
+
 void GCS::send_home(const Location &home) const
 {
     FOR_EACH_ACTIVE_CHANNEL(send_home(home));
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index fdb49cbe5a..9eac5ded79 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -183,6 +183,7 @@ public:
     void send_autopilot_version() const;
     void send_local_position() const;
     void send_vibration() const;
+    void send_named_float(const char *name, float value) const;
     void send_home(const Location &home) const;
     void send_ekf_origin(const Location &ekf_origin) const;
     void send_servo_output_raw(bool hil);
@@ -553,6 +554,7 @@ public:
     virtual uint8_t num_gcs() const = 0;
     void send_message(enum ap_message id);
     void send_mission_item_reached_message(uint16_t mission_index);
+    void send_named_float(const char *name, float value) const;
     void send_home(const Location &home) const;
     void send_ekf_origin(const Location &ekf_origin) const;
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 3d12f0d1ed..5c2aae7a7c 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -600,11 +600,12 @@ void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg)
 
 void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...)
 {
-    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
+    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
     va_list arg_list;
     va_start(arg_list, fmt);
     hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list);
     va_end(arg_list);
+    text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0;
     gcs().send_statustext(severity, (1<<chan), text);
 }
 
@@ -1540,6 +1541,13 @@ void GCS_MAVLINK::send_vibration() const
         ins.get_accel_clip_count(2));
 }
 
+void GCS_MAVLINK::send_named_float(const char *name, float value) const
+{
+    char float_name[MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN+1] {};
+    strncpy(float_name, name, MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN);
+    mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value);
+}
+
 void GCS_MAVLINK::send_home(const Location &home) const
 {
     if (HAVE_PAYLOAD_SPACE(chan, HOME_POSITION)) {