GCS Console: add lib implementing message handling from DATA16 and DATA32 msgs

console demonstrates simple loopback
works with our branch of mavproxy at the moment

libraries/GCS_Console/GCS_Console.cpp

@@ -0,0 +1,50 @@
+
+#include <AP_HAL.h>
+#include <GCS_Console.h>
+
+extern const AP_HAL::HAL& hal;
+
+void gcs_console_handle_data16(mavlink_message_t* msg) {
+
+    mavlink_data16_t data16;
+    mavlink_msg_data16_decode(msg, &data16);
+    if (data16.type == DATAMSG_TYPE_CONSOLE) {
+        hal.console->backend_write(data16.data, data16.len);
+    }
+
+}
+
+void gcs_console_handle_data32(mavlink_message_t* msg) {
+
+    mavlink_data32_t data32;
+    mavlink_msg_data32_decode(msg, &data32);
+    if (data32.type == DATAMSG_TYPE_CONSOLE) {
+        hal.console->backend_write(data32.data, data32.len);
+    }
+
+}
+
+void gcs_console_send(mavlink_channel_t chan) {
+    uint8_t cons[32];
+    memset(cons, 0, 32);
+    int16_t txspace = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+    /* There are two bytes of overhead per packet (on top of the non payload
+     * bytes). We can read up to 32 bytes from the console if we have 34
+     * available for tx, up to 16 bytes if we have more than 18 available.
+     * otherwise we have to try again later. */
+    int readable = 32;
+    if (txspace >= 18 && txspace < 34) {
+        readable = 16;
+    } else if ( txspace < 18 ) {
+        return;
+    }
+    /* Read from the console backend */
+    int avail = hal.console->backend_read(cons, readable);
+    /* Send in the smallest packet available. (Don't send if 0.) */
+    if (avail > 0 && avail <= 16) {
+        mavlink_msg_data16_send(chan, DATAMSG_TYPE_CONSOLE, avail, cons);
+    } else if (avail > 16 && avail <= 32) {
+        mavlink_msg_data32_send(chan, DATAMSG_TYPE_CONSOLE, avail, cons);
+    }
+}
+

libraries/GCS_Console/GCS_Console.h

@@ -0,0 +1,25 @@
+
+#ifndef __GCS_CONSOLE_H__
+#define __GCS_CONSOLE_H__
+
+#include <GCS_MAVLink.h>
+
+/* Ensure compatibility with GCS_MAVLink library. We need the DATA16
+ * and DATA32 mesages. If these aren't present, get them from the mavlink
+ * repo message_definitions/ardupilotmega.xml and regenerate the GCS_MAVLink
+ * definitions. */
+#ifndef MAVLINK_MSG_ID_DATA16
+#error GCS_Console module requires Mavlink Message DATA16
+#endif
+#ifndef MAVLINK_MSG_ID_DATA32
+#error GCS_Console module requires Mavlink Message DATA32
+#endif
+
+#define DATAMSG_TYPE_CONSOLE 0xFE
+
+void gcs_console_handle_data16(mavlink_message_t* msg);
+void gcs_console_handle_data32(mavlink_message_t* msg);
+
+void gcs_console_send(mavlink_channel_t chan);
+
+#endif // __GCS_CONSOLE_H__

libraries/GCS_Console/examples/Console/Console.pde

@@ -13,6 +13,7 @@
 #include <AP_Param.h>
 #include <AP_Math.h>
 #include <GCS_MAVLink.h>
+#include <GCS_Console.h>
 
 #include "simplegcs.h"
 
@@ -26,6 +27,19 @@ void flush_console_to_statustext() {
     }
 }
 
+void console_loopback() {
+    int a = hal.console->available();
+    if (a > 0) {
+        hal.console->print("Console loopback:");
+        int r = hal.console->read();
+        while (r > 0) {
+            hal.console->write( (uint8_t) r );
+            r = hal.console->read();
+        }
+        hal.console->println();
+    }   
+}
+
 void setup(void) {
     /* Allocate large enough buffers on uart0 to support mavlink */
     hal.uart0->begin(115200, 128, 256);
@@ -44,8 +58,11 @@ int i = 0;
 void loop(void) {
     try_send_message(MAVLINK_COMM_0, MAVLINK_MSG_ID_HEARTBEAT);
     simplegcs_update(MAVLINK_COMM_0);
-    flush_console_to_statustext();
-    hal.scheduler->delay(500);
+    // flush_console_to_statustext();
+    gcs_console_send(MAVLINK_COMM_0);
+
+    console_loopback();
+    hal.scheduler->delay(100);
 }
 
 AP_HAL_MAIN();

libraries/GCS_Console/examples/Console/simplegcs.cpp

@@ -3,6 +3,7 @@
 extern const AP_HAL::HAL& hal;
 
 #include <GCS_MAVLink.h>
+#include <GCS_Console.h>
 #include "simplegcs.h"
 
 void send_heartbeat(mavlink_channel_t chan) {
@@ -73,7 +74,7 @@ void handle_message(mavlink_channel_t chan, mavlink_message_t* msg) {
     hal.console->printf_P(PSTR("SimpleGCS Handle Message %d\r\n"), msg->msgid);
     switch (msg->msgid) {
         case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
-            {
+        {
             char param_name[16] = "NOPARAMS";
 
             /* Send a single parameter.*/
@@ -85,7 +86,13 @@ void handle_message(mavlink_channel_t chan, mavlink_message_t* msg) {
                     1,                  /* _queued_parameter_count */
                     0);                 /* _queued_parameter_index */
             break;
-            }
+        }
+        case MAVLINK_MSG_ID_DATA16:
+            gcs_console_handle_data16(msg); 
+            break;
+        case MAVLINK_MSG_ID_DATA32:
+            gcs_console_handle_data32(msg); 
+            break;
     }
 }