GCS_MAVLink: move handling of incoming statutext messages up

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);

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)

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; }
 
 };
 

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: