GCS_Console example: implement a trivial mavlink gcs

* Just heartbeats and sends a single parameter down
* all other messages unimplemented
* console is dumped to statustext at the moment (that will change soon)

libraries/GCS_Console/examples/Console/Console.pde

@@ -0,0 +1,51 @@
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
+
+//
+// Example code for the AP_HAL AVRUARTDriver, based on FastSerial
+//
+// This code is placed into the public domain.
+//
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
+#include <AP_Param.h>
+#include <AP_Math.h>
+#include <GCS_MAVLink.h>
+
+#include "simplegcs.h"
+
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
+
+void flush_console_to_statustext() {
+    uint8_t data[50];
+    int n = hal.console->backend_read(data, 50);
+    if (n > 0) {
+        try_send_statustext(MAVLINK_COMM_0, (char*) data, n);
+    }
+}
+
+void setup(void) {
+    /* Allocate large enough buffers on uart0 to support mavlink */
+    hal.uart0->begin(115200, 128, 256);
+
+    /* Setup GCS_Mavlink library's comm 0 port. */
+    mavlink_comm_0_port = hal.uart0;
+    
+    char hello[] = "Hello statustext\r\n";
+    try_send_statustext(MAVLINK_COMM_0, hello, strlen(hello));
+
+    hal.console->backend_open();
+    hal.console->printf_P(PSTR("Hello hal.console\r\n"));
+}
+
+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);
+}
+
+AP_HAL_MAIN();

libraries/GCS_Console/examples/Console/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/GCS_Console/examples/Console/simplegcs.cpp

@@ -0,0 +1,91 @@
+
+#include <AP_HAL.h>
+extern const AP_HAL::HAL& hal;
+
+#include <GCS_MAVLink.h>
+#include "simplegcs.h"
+
+void send_heartbeat(mavlink_channel_t chan) {
+    uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED
+                      | MAV_MODE_FLAG_GUIDED_ENABLED
+                      | MAV_MODE_FLAG_SAFETY_ARMED
+                      | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    uint8_t system_status = MAV_STATE_ACTIVE;
+    uint32_t custom_mode = 5 ; /* Loiter is mode 5. */
+
+    mavlink_msg_heartbeat_send(
+            chan,
+            MAV_TYPE_QUADROTOR,
+            MAV_AUTOPILOT_ARDUPILOTMEGA,
+            base_mode,
+            custom_mode,
+            system_status);
+}
+
+#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
+
+bool try_send_message(mavlink_channel_t chan, int msgid) {
+
+    int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+    switch (msgid) {
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        CHECK_PAYLOAD_SIZE(HEARTBEAT);
+        send_heartbeat(chan);
+        return true;
+    default:
+        return false;
+    }
+}
+
+bool try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
+
+    int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+    CHECK_PAYLOAD_SIZE(STATUSTEXT);
+
+    char statustext[50] = { 0 };
+    if (len < 50) {
+        memcpy(statustext, text, len);
+    }
+    mavlink_msg_statustext_send(
+            chan,
+            1, /* SEVERITY_LOW */
+            statustext);
+    return true;
+}
+// -----------------------------------------------------------------------
+
+
+void simplegcs_update(mavlink_channel_t chan) {
+    mavlink_message_t msg;
+    mavlink_status_t status;
+    while(comm_get_available(chan)){
+        uint8_t c = comm_receive_ch(chan);
+        bool newmsg = mavlink_parse_char(chan, c, &msg, &status);
+        if (newmsg) {
+            handle_message(chan, &msg);
+        }
+    }
+}
+
+
+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.*/
+            mavlink_msg_param_value_send(
+                    chan,
+                    param_name,
+                    0.0,                /* param value */
+                    MAVLINK_TYPE_FLOAT, /* param type */
+                    1,                  /* _queued_parameter_count */
+                    0);                 /* _queued_parameter_index */
+            break;
+            }
+    }
+}
+

libraries/GCS_Console/examples/Console/simplegcs.h

@@ -0,0 +1,15 @@
+
+#ifndef __SIMPLE_GCS_H__
+#define __SIMPLE_GCS_H__
+
+#include <GCS_MAVLink.h>
+
+void send_heartbeat(mavlink_channel_t chan);
+bool try_send_message(mavlink_channel_t chan, int msgid);
+bool try_send_statustext(mavlink_channel_t chan, const char *text, int len);
+
+void simplegcs_update(mavlink_channel_t chan);
+void handle_message(mavlink_channel_t chan, mavlink_message_t* msg);
+
+#endif // __SIMPLE_GCS_H__
+