GCS_MAVLink: added remote device operations

this makes debugging devices much easier. You can even write a
primitive SPI or I2C device driver over mavlink.

Support for this is in the devop MAVProxy module

libraries/GCS_MAVLink/GCS.h

@@ -277,6 +277,9 @@ protected:
     void handle_setup_signing(const mavlink_message_t *msg);
     uint8_t handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides);
     uint8_t handle_rc_bind(const mavlink_command_long_t &packet);
+
+    void handle_device_op_read(mavlink_message_t *msg);
+    void handle_device_op_write(mavlink_message_t *msg);
     
 private:
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1816,5 +1816,11 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
         handle_param_request_read(msg);
         break;
+    case MAVLINK_MSG_ID_DEVICE_OP_READ:
+        handle_device_op_read(msg);
+        break;
+    case MAVLINK_MSG_ID_DEVICE_OP_WRITE:
+        handle_device_op_write(msg);
+        break;
     }
 }

libraries/GCS_MAVLink/GCS_DeviceOp.cpp

@@ -0,0 +1,116 @@
+/*
+   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/>.
+ */
+/*
+  handle device operations over MAVLink
+ */
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Device.h>
+#include "GCS.h"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  handle DEVICE_OP_READ message
+ */
+void GCS_MAVLINK::handle_device_op_read(mavlink_message_t *msg)
+{
+    mavlink_device_op_read_t packet;
+    mavlink_msg_device_op_read_decode(msg, &packet);
+    AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
+    uint8_t retcode = 0;
+    uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {};
+    
+    if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
+        dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
+    } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
+        dev = hal.spi->get_device(packet.busname);
+    } else {
+        retcode = 1;
+        goto fail;
+    }
+    if (!dev) {
+        retcode = 2;
+        goto fail;
+    }
+    if (!dev->get_semaphore()->take(10)) {
+        retcode = 3;
+        goto fail;        
+    }
+    if (!dev->read_registers(packet.regstart, data, packet.count)) {
+        retcode = 4;
+        dev->get_semaphore()->give();
+        goto fail;
+    }
+    dev->get_semaphore()->give();
+    mavlink_msg_device_op_read_reply_send(
+        chan,
+        packet.request_id,
+        retcode,
+        packet.regstart,
+        packet.count,
+        data);
+    return;
+
+fail:
+    mavlink_msg_device_op_read_reply_send(
+        chan,
+        packet.request_id,
+        retcode,
+        packet.regstart,
+        0,
+        nullptr);
+}
+
+/*
+  handle DEVICE_OP_WRITE message
+ */
+void GCS_MAVLINK::handle_device_op_write(mavlink_message_t *msg)
+{
+    mavlink_device_op_write_t packet;
+    mavlink_msg_device_op_write_decode(msg, &packet);
+    AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
+    uint8_t retcode = 0;
+    
+    if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
+        dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
+    } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
+        dev = hal.spi->get_device(packet.busname);
+    } else {
+        retcode = 1;
+        goto fail;
+    }
+    if (!dev) {
+        retcode = 2;
+        goto fail;
+    }
+    if (!dev->get_semaphore()->take(10)) {
+        retcode = 3;
+        goto fail;        
+    }
+    for (uint8_t i=0; i<packet.count; i++) {
+        if (!dev->write_register(packet.regstart+i, packet.data[i])) {
+            retcode = 4;
+            break;
+        }
+    }
+    dev->get_semaphore()->give();
+
+fail:
+    mavlink_msg_device_op_write_reply_send(
+        chan,
+        packet.request_id,
+        retcode);
+}