GCS_MAVLink: implement SERIAL_CONTROL MAVLink message

libraries/GCS_MAVLink/GCS.h

@@ -312,6 +312,8 @@ private:
     void handle_param_request_read(mavlink_message_t *msg);
     void handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash);
     void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio);
+    void handle_serial_control(mavlink_message_t *msg, AP_GPS &gps);
+    void lock_channel(mavlink_channel_t chan, bool lock);
 
     // return true if this channel has hardware flow control
     bool have_flow_control(void);

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -24,6 +24,8 @@ This provides some support code and variables for MAVLink enabled sketches
 #include <AP_HAL.h>
 #include <AP_Common.h>
 #include <GCS_MAVLink.h>
+#include <GCS.h>
+#include <AP_GPS.h>
 
 #ifdef MAVLINK_SEPARATE_HELPERS
 #include "include/mavlink/v1.0/mavlink_helpers.h"
@@ -38,6 +40,24 @@ AP_HAL::BetterStream	*mavlink_comm_2_port;
 
 mavlink_system_t mavlink_system = {7,1,0,0};
 
+// mask of serial ports disabled to allow for SERIAL_CONTROL
+static uint8_t mavlink_locked_mask;
+
+/*
+  lock a channel, preventing use by MAVLink
+ */
+void GCS_MAVLINK::lock_channel(mavlink_channel_t _chan, bool lock)
+{
+    if (_chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return;
+    }
+    if (lock) {
+        mavlink_locked_mask |= (1U<<(unsigned)_chan);
+    } else {
+        mavlink_locked_mask &= ~(1U<<(unsigned)_chan);
+    }
+}
+
 uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
 {
     if (sysid != mavlink_system.sysid)
@@ -72,7 +92,6 @@ uint8_t mav_var_type(enum ap_var_type t)
 uint8_t comm_receive_ch(mavlink_channel_t chan)
 {
     uint8_t data = 0;
-
     switch(chan) {
 	case MAVLINK_COMM_0:
 		data = mavlink_comm_0_port->read();
@@ -97,6 +116,9 @@ uint8_t comm_receive_ch(mavlink_channel_t chan)
 /// @returns		Number of bytes available
 uint16_t comm_get_txspace(mavlink_channel_t chan)
 {
+    if ((1U<<chan) & mavlink_locked_mask) {
+        return 0;
+    }
 	int16_t ret = 0;
     switch(chan) {
 	case MAVLINK_COMM_0:
@@ -125,6 +147,9 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
 /// @returns		Number of bytes available
 uint16_t comm_get_available(mavlink_channel_t chan)
 {
+    if ((1U<<chan) & mavlink_locked_mask) {
+        return 0;
+    }
     int16_t bytes = 0;
     switch(chan) {
 	case MAVLINK_COMM_0:

libraries/GCS_MAVLink/GCS_serial_control.cpp

@@ -0,0 +1,144 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+  MAVLink SERIAL_CONTROL handling
+ */
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <AP_HAL.h>
+#include <GCS.h>
+#include <DataFlash.h>
+
+extern const AP_HAL::HAL& hal;
+
+/**
+   handle a SERIAL_CONTROL message
+ */
+void GCS_MAVLINK::handle_serial_control(mavlink_message_t *msg, AP_GPS &gps)
+{
+    mavlink_serial_control_t packet;
+    mavlink_msg_serial_control_decode(msg, &packet);
+
+    AP_HAL::UARTDriver *port = NULL;
+
+    if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {
+        // how did this packet get to us?
+        return;
+    }
+
+    bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
+
+    switch (packet.device) {
+    case SERIAL_CONTROL_DEV_TELEM1:
+        port = hal.uartC;
+        lock_channel(MAVLINK_COMM_1, exclusive);
+        break;
+    case SERIAL_CONTROL_DEV_TELEM2:
+        port = hal.uartD;
+        lock_channel(MAVLINK_COMM_2, exclusive);
+        break;
+    case SERIAL_CONTROL_DEV_GPS1:
+        port = hal.uartB;
+        gps.lock_port(0, exclusive);
+        break;
+    case SERIAL_CONTROL_DEV_GPS2:
+        port = hal.uartE;
+        gps.lock_port(1, exclusive);
+        break;
+    default:
+        // not supported yet
+        return;
+    }
+    
+    if (exclusive) {
+        // force flow control off for exclusive access. This protocol
+        // is used to talk to bootloaders which may not have flow
+        // control support
+        port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
+    }
+
+    // optionally change the baudrate
+    if (packet.baudrate != 0) {
+        port->begin(packet.baudrate);
+    }
+
+    // write the data
+    if (packet.count != 0) {
+        if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
+            port->write(packet.data, packet.count);
+        } else {
+            const uint8_t *data = &packet.data[0];
+            uint8_t count = packet.count;
+            while (count > 0) {
+                while (port->txspace() <= 0) {
+                    hal.scheduler->delay(5);
+                }
+                uint8_t n = port->txspace();
+                if (n > packet.count) {
+                    n = packet.count;
+                }
+                port->write(data, n);                
+                data += n;
+                count -= n;
+            }
+        }
+    }
+
+    if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
+        // no response expected
+        return;
+    }
+
+    uint8_t flags = packet.flags;
+
+more_data:
+    // sleep for the timeout
+    while (packet.timeout != 0 && 
+           port->available() < (int16_t)sizeof(packet.data)) {
+        hal.scheduler->delay(1);
+        packet.timeout--;
+    }
+
+    packet.flags = SERIAL_CONTROL_FLAG_REPLY;
+
+    // work out how many bytes are available
+    int16_t available = port->available();
+    if (available < 0) {
+        available = 0;
+    }
+    if (available > (int16_t)sizeof(packet.data)) {
+        available = sizeof(packet.data);
+    }
+
+    // read any reply data
+    packet.count = 0;
+    while (available > 0) {
+        packet.data[packet.count++] = (uint8_t)port->read();
+        available--;
+    }
+
+    // and send the reply
+    _mav_finalize_message_chan_send(chan, 
+                                    MAVLINK_MSG_ID_SERIAL_CONTROL,
+                                    (const char *)&packet,
+                                    MAVLINK_MSG_ID_SERIAL_CONTROL_LEN,
+                                    MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+    if ((flags & SERIAL_CONTROL_FLAG_MULTI) && packet.count != 0) {
+        hal.scheduler->delay(1);
+        goto more_data;
+    }
+}