From 129d7220e6039c9aeb635dcb9185b3de8f06614e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 12 Jul 2017 17:21:15 +1000
Subject: [PATCH] GCS_MAVLink: move handling of incoming statutext messages up

---
 libraries/GCS_MAVLink/GCS.h                   |  5 ++++-
 libraries/GCS_MAVLink/GCS_Common.cpp          | 21 +++++++++++++++++++
 libraries/GCS_MAVLink/GCS_Dummy.h             |  1 +
 .../GCS_MAVLink/examples/routing/routing.cpp  |  1 +
 4 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index a33a38221d..5fbc9ecc7d 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -103,6 +103,8 @@ public:
     // accessor for uart
     AP_HAL::UARTDriver *get_uart() { return _port; }
 
+    virtual uint8_t sysid_my_gcs() const = 0;
+
     static const struct AP_Param::GroupInfo        var_info[];
 
     // set to true if this GCS link is active
@@ -265,7 +267,8 @@ protected:
     void handle_device_op_write(mavlink_message_t *msg);
 
     void handle_timesync(mavlink_message_t *msg);
-    
+    void handle_statustext(mavlink_message_t *msg);
+
 private:
 
     float       adjust_rate_for_stream_trigger(enum streams stream_num);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 44a9d34000..34b99bf89d 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1685,6 +1685,22 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
         );
 }
 
+void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg)
+{
+    DataFlash_Class *df = DataFlash_Class::instance();
+    if (df == nullptr) {
+        return;
+    }
+    // ignore any statustext messages not from our GCS:
+    if (msg->sysid != sysid_my_gcs()) {
+        return;
+    }
+    mavlink_statustext_t packet;
+    mavlink_msg_statustext_decode(msg, &packet);
+    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G','C','S',':'};
+    memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
+    df->Log_Write_Message(text);
+}
 
 /*
   handle messages which don't require vehicle specific data
@@ -1739,7 +1755,12 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
         handle_common_mission_message(msg);
         break;
+
+    case MAVLINK_MSG_ID_STATUSTEXT:
+        handle_statustext(msg);
+        break;
     }
+
 }
 
 void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h
index d02537329f..2e15693d3a 100644
--- a/libraries/GCS_MAVLink/GCS_Dummy.h
+++ b/libraries/GCS_MAVLink/GCS_Dummy.h
@@ -15,6 +15,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK
 protected:
 
     AP_Mission *get_mission() override { return nullptr; }
+    uint8_t sysid_my_gcs() const override { return 1; }
 
 };
 
diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp
index 7260039d96..0b55d79ed7 100644
--- a/libraries/GCS_MAVLink/examples/routing/routing.cpp
+++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp
@@ -22,6 +22,7 @@ protected:
 
     uint32_t telem_delay() const override { return 0; }
     AP_Mission *get_mission() override { return nullptr; }
+    uint8_t sysid_my_gcs() const override { return 1; }
 
 private: