From a44c1378f7dbd9b9ffe8e4a2343502a5cb114bc2 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 1 Aug 2017 15:13:34 +1000
Subject: [PATCH] GCS_MAVLink: add send_textv functions

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

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index 7843130c44..f059d704ae 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -5,15 +5,19 @@ extern const AP_HAL::HAL& hal;
 /*
   send a text message to all GCS
  */
-void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
+void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
 {
     char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
+    hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list);
+    send_statustext(severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), text);
+}
+
+void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
+{
     va_list arg_list;
     va_start(arg_list, fmt);
-    hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list);
+    send_textv(severity, fmt, arg_list);
     va_end(arg_list);
-    text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0;
-    send_statustext(severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), text);
 }
 
 #define FOR_EACH_ACTIVE_CHANNEL(methodcall)          \
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 76e810b227..565921bf16 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -109,6 +109,7 @@ public:
     void        setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance);
     void        send_message(enum ap_message id);
     void        send_text(MAV_SEVERITY severity, const char *fmt, ...);
+    void        send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
     void        data_stream_send();
     void        queued_param_send();
     void        queued_waypoint_send();
@@ -580,6 +581,7 @@ public:
     }
 
     void send_text(MAV_SEVERITY severity, const char *fmt, ...);
+    void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
     virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
     void service_statustext(void);
     virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0;
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 321879d7b6..5b5f7d1d1d 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -606,15 +606,18 @@ void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg)
 }
 
 
-void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...)
+void GCS_MAVLINK::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
 {
     char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
+    hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list);
+    gcs().send_statustext(severity, (1<<chan), text);
+}
+void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...)
+{
     va_list arg_list;
     va_start(arg_list, fmt);
-    hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list);
+    send_textv(severity, fmt, arg_list);
     va_end(arg_list);
-    text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0;
-    gcs().send_statustext(severity, (1<<chan), text);
 }
 
 void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio)