Add mavlink console support

qgcresources.qrc

@@ -56,6 +56,7 @@
         <file alias="GeoFence.svg">src/AutoPilotPlugins/PX4/Images/GeoFence.svg</file>
         <file alias="GeoFenceLight.svg">src/AutoPilotPlugins/PX4/Images/GeoFenceLight.svg</file>
         <file alias="GeoTagIcon">src/AnalyzeView/GeoTagIcon.svg</file>
+        <file alias="MavlinkConsoleIcon">src/AnalyzeView/MavlinkConsoleIcon.svg</file>
         <file alias="LandMode.svg">src/AutoPilotPlugins/PX4/Images/LandMode.svg</file>
         <file alias="LandModeCopter.svg">src/AutoPilotPlugins/PX4/Images/LandModeCopter.svg</file>
         <file alias="LightsComponentIcon.png">src/AutoPilotPlugins/APM/Images/LightsComponentIcon.png</file>

qgroundcontrol.pro

@@ -555,6 +555,7 @@ HEADERS += \
 !MobileBuild {
 HEADERS += \
     src/AnalyzeView/GeoTagController.h \
+    src/AnalyzeView/MavlinkConsoleController.h \
     src/GPS/Drivers/src/gps_helper.h \
     src/GPS/Drivers/src/ubx.h \
     src/GPS/GPSManager.h \
@@ -718,6 +719,7 @@ contains(DEFINES, QGC_ENABLE_BLUETOOTH) {
 !MobileBuild {
 SOURCES += \
     src/AnalyzeView/GeoTagController.cc \
+    src/AnalyzeView/MavlinkConsoleController.cc \
     src/GPS/Drivers/src/gps_helper.cpp \
     src/GPS/Drivers/src/ubx.cpp \
     src/GPS/GPSManager.cc \

qgroundcontrol.qrc

@@ -28,6 +28,7 @@
         <file alias="FlightModesComponentSummary.qml">src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml</file>
         <file alias="GeneralSettings.qml">src/ui/preferences/GeneralSettings.qml</file>
         <file alias="GeoTagPage.qml">src/AnalyzeView/GeoTagPage.qml</file>
+        <file alias="MavlinkConsolePage.qml">src/AnalyzeView/MavlinkConsolePage.qml</file>
         <file alias="JoystickConfig.qml">src/VehicleSetup/JoystickConfig.qml</file>
         <file alias="LinkSettings.qml">src/ui/preferences/LinkSettings.qml</file>
         <file alias="LogDownloadPage.qml">src/AnalyzeView/LogDownloadPage.qml</file>

src/AnalyzeView/AnalyzeView.qml

@@ -18,6 +18,7 @@ import QtQuick.Controls 1.2
 import QGroundControl               1.0
 import QGroundControl.Palette       1.0
 import QGroundControl.Controls      1.0
+import QGroundControl.Controllers   1.0
 import QGroundControl.ScreenTools   1.0
 
 Rectangle {
@@ -35,6 +36,10 @@ Rectangle {
     readonly property real  _verticalMargin:        _defaultTextHeight / 2
     readonly property real  _buttonWidth:           _defaultTextWidth * 18
 
+    MavlinkConsoleController {
+        id: conController
+    }
+
     QGCFlickable {
         id:                 buttonScroll
         width:              buttonColumn.width
@@ -95,6 +100,11 @@ Rectangle {
                         buttonText:         qsTr("GeoTag Images")
                         pageSource:         "GeoTagPage.qml"
                     }
+                    ListElement {
+                        buttonImage:        "/qmlimages/MavlinkConsoleIcon"
+                        buttonText:         qsTr("Mavlink Console")
+                        pageSource:         "MavlinkConsolePage.qml"
+                    }
                 }
 
                 Component.onCompleted: itemAt(0).checked = true

src/AnalyzeView/MavlinkConsoleController.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "MavlinkConsoleController.h"
+#include "QGCApplication.h"
+#include "UAS.h"
+
+MavlinkConsoleController::MavlinkConsoleController()
+    : _cursor_home_pos{-1},
+      _cursor{0},
+      _vehicle{nullptr}
+{
+    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
+    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &MavlinkConsoleController::_setActiveVehicle);
+    _setActiveVehicle(manager->activeVehicle());
+}
+
+MavlinkConsoleController::~MavlinkConsoleController()
+{
+    if (_vehicle) {
+        QByteArray msg("");
+        _sendSerialData(msg, true);
+    }
+}
+
+void
+MavlinkConsoleController::sendCommand(QString command)
+{
+    command.append("\n");
+    _sendSerialData(qPrintable(command));
+    _cursor_home_pos = -1;
+    _cursor = _console_text.length();
+}
+
+void
+MavlinkConsoleController::_setActiveVehicle(Vehicle* vehicle)
+{
+    for (auto &con : _uas_connections)
+        disconnect(con);
+    _uas_connections.clear();
+
+    _vehicle = vehicle;
+
+    if (_vehicle) {
+        _incoming_buffer.clear();
+        _console_text.clear();
+        emit cursorChanged(0);
+        emit textChanged(_console_text);
+        _uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &MavlinkConsoleController::_receiveData);
+    }
+}
+
+void
+MavlinkConsoleController::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
+{
+    if (device != SERIAL_CONTROL_DEV_SHELL)
+        return;
+
+    // Append incoming data and parse for ANSI codes
+    _incoming_buffer.append(data);
+    auto old_size = _console_text.size();
+    _processANSItext();
+    auto new_size = _console_text.size();
+
+    // Update QML and cursor
+    if (old_size > new_size) {
+        // Rewind back so we don't get a warning to stderr
+        emit cursorChanged(new_size);
+    }
+    emit textChanged(_console_text);
+    emit cursorChanged(new_size);
+}
+
+void
+MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
+{
+    Q_ASSERT(_vehicle);
+    if (!_vehicle)
+        return;
+
+    // Send maximum sized chunks until the complete buffer is transmitted
+    while(data.size()) {
+        QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
+        uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND;
+        if (close) flags = 0;
+        auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
+        auto priority_link = _vehicle->priorityLink();
+        mavlink_message_t msg;
+        mavlink_msg_serial_control_pack_chan(
+                    protocol->getSystemId(),
+                    protocol->getComponentId(),
+                    priority_link->mavlinkChannel(),
+                    &msg,
+                    SERIAL_CONTROL_DEV_SHELL,
+                    flags,
+                    0,
+                    0,
+                    chunk.size(),
+                    reinterpret_cast<uint8_t*>(chunk.data()));
+        _vehicle->sendMessageOnLink(priority_link, msg);
+        data.remove(0, chunk.size());
+    }
+}
+
+void
+MavlinkConsoleController::_processANSItext()
+{
+    int i; // Position into the parsed buffer
+
+    // Iterate over the incoming buffer to parse off known ANSI control codes
+    for (i = 0; i < _incoming_buffer.size(); i++) {
+        if (_incoming_buffer.at(i) == '\x1B') {
+            // For ANSI codes we expect at most 4 incoming chars
+            if (i < _incoming_buffer.size() - 3 && _incoming_buffer.at(i+1) == '[') {
+                // Parse ANSI code
+                switch(_incoming_buffer.at(i+2)) {
+                    default:
+                        continue;
+                    case 'H':
+                        if (_cursor_home_pos == -1) {
+                            // Assign new home position if home is unset
+                            _cursor_home_pos = _cursor;
+                        } else {
+                            // Rewind write cursor position to home
+                            _cursor = _cursor_home_pos;
+                        }
+                        break;
+                    case 'K':
+                        // Erase the current line to the end
+                    {
+                        auto next_lf = _console_text.indexOf('\n', _cursor);
+                        if (next_lf > 0)
+                            _console_text.remove(_cursor, next_lf + 1 - _cursor);
+                    }
+                        break;
+                    case '2':
+                        // Erase everything and rewind to home
+                        if (_incoming_buffer.at(i+3) == 'J' && _cursor_home_pos != -1) {
+                            // Keep newlines so textedit doesn't scroll annoyingly
+                            int newlines = _console_text.mid(_cursor_home_pos).count('\n');
+                            _console_text.remove(_cursor_home_pos, _console_text.size());
+                            _console_text.append(QByteArray(newlines, '\n'));
+                            _cursor = _cursor_home_pos;
+                        }
+                        // Even if we didn't understand this ANSI code, remove the 4th char
+                        _incoming_buffer.remove(i+3,1);
+                        break;
+                }
+                // Remove the parsed ANSI code and decrement the bufferpos
+                _incoming_buffer.remove(i, 3);
+                i--;
+            } else {
+                // We can reasonably expect a control code was fragemented
+                // Stop parsing here and wait for it to come in
+                break;
+            }
+        }
+    }
+
+    // Insert the new data and increment the write cursor
+    _console_text.insert(_cursor, _incoming_buffer.left(i));
+    _cursor += i;
+
+    // Remove written data from the incoming buffer
+    _incoming_buffer.remove(0, i);
+}

src/AnalyzeView/MavlinkConsoleController.h

+/****************************************************************************
+ *
+ *   (c) 2009-2017 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QmlObjectListModel.h"
+#include "Fact.h"
+#include "FactMetaData.h"
+#include <QObject>
+#include <QString>
+#include <QThread>
+#include <QFileInfoList>
+#include <QElapsedTimer>
+#include <QDebug>
+#include <QGeoCoordinate>
+#include <QMetaObject>
+
+
+// Fordward decls
+class Vehicle;
+
+
+/// Controller for MavlinkConsole.qml.
+class MavlinkConsoleController : public QObject
+{
+    Q_OBJECT
+
+    Q_PROPERTY(int     cursor     READ cursor        NOTIFY cursorChanged)
+    Q_PROPERTY(QString text       READ text          NOTIFY textChanged)
+
+public:
+    MavlinkConsoleController();
+    ~MavlinkConsoleController();
+
+    int  cursor()    const { return _console_text.size(); }
+    QString text()   const { return _console_text; }
+public slots:
+    void sendCommand(QString command);
+
+signals:
+    void cursorChanged(int);
+    void textChanged(QString text);
+
+private slots:
+    void _setActiveVehicle  (Vehicle* vehicle);
+    void _receiveData(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
+
+private:
+    void _processANSItext();
+    void _sendSerialData(QByteArray, bool close = false);
+
+    int           _cursor_home_pos;
+    int           _cursor;
+    QByteArray    _incoming_buffer;
+    QString       _console_text;
+    Vehicle*      _vehicle;
+    QList<QMetaObject::Connection> _uas_connections;
+
+};

src/AnalyzeView/MavlinkConsoleIcon.svg

+<?xml version="1.0" encoding="utf-8"?>
+<svg
+   xmlns="http://www.w3.org/2000/svg"
+   width="512"
+   height="512"
+   id="caret"
+   version="1.1">
+  <g
+     id="layer1"
+     transform="translate(0,-540.36218)">
+    <g
+       style="fill:#ffffff;fill-opacity:1;stroke:none;"
+       id="text2985">
+      <path
+         d="m 191.125,654.48718 168.5,141.75 -168.5,142 -38.75,-39.5 123.75,-102 -123.75,-102.75 z"
+         style="fill:#ffffff;fill-opacity:1" />
+    </g>
+  </g>
+</svg>

src/AnalyzeView/MavlinkConsolePage.qml

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+import QtQuick          2.3
+import QtQuick.Controls 1.2
+import QtQuick.Dialogs  1.2
+import QtQuick.Layouts      1.2
+
+import QGroundControl               1.0
+import QGroundControl.Palette       1.0
+import QGroundControl.FactSystem    1.0
+import QGroundControl.FactControls  1.0
+import QGroundControl.Controls      1.0
+import QGroundControl.ScreenTools   1.0
+import QGroundControl.Controllers   1.0
+
+AnalyzePage {
+    id:                 mavlinkConsolePage
+    pageComponent:      pageComponent
+    pageName:           qsTr("Mavlink Console")
+    pageDescription:    qsTr("Mavlink Console provides a connection to the vehicle's system shell.")
+
+    Component {
+        id: pageComponent
+
+        ColumnLayout {
+            id:     consoleColumn
+            height: availableHeight
+            width:  availableWidth
+
+            TextArea {
+                id:                consoleEditor
+                Layout.fillHeight: true
+                anchors.left:      parent.left
+                anchors.right:     parent.right
+                font.family:       ScreenTools.fixedFontFamily
+                font.pointSize:    ScreenTools.defaultFontPointSize
+                readOnly:          true
+
+                cursorPosition:    conController.cursor
+                text:              conController.text
+            }
+
+            QGCTextField {
+                id:              command
+                anchors.left:    parent.left
+                anchors.right:   parent.right
+                placeholderText: "Enter Commands here..."
+
+                onAccepted: {
+                    conController.sendCommand(text)
+                    text = ""
+                }
+            }
+        }
+    } // Component
+} // AnalyzePage

src/QGCApplication.cc

@@ -91,6 +91,7 @@
 #include "FirmwareUpgradeController.h"
 #include "MainWindow.h"
 #include "GeoTagController.h"
+#include "MavlinkConsoleController.h"
 #endif
 
 #ifdef QGC_RTLAB_ENABLED
@@ -377,6 +378,7 @@ void QGCApplication::_initCommon(void)
     qmlRegisterType<CustomCommandWidgetController>  ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController");
     qmlRegisterType<FirmwareUpgradeController>      ("QGroundControl.Controllers", 1, 0, "FirmwareUpgradeController");
     qmlRegisterType<GeoTagController>               ("QGroundControl.Controllers", 1, 0, "GeoTagController");
+    qmlRegisterType<MavlinkConsoleController>       ("QGroundControl.Controllers", 1, 0, "MavlinkConsoleController");
 #endif
 
     // Register Qml Singletons

src/QmlControls/ScreenTools.qml

@@ -74,7 +74,7 @@ Item {
 
     readonly property string normalFontFamily:      "opensans"
     readonly property string demiboldFontFamily:    "opensans-demibold"
-
+    readonly property string fixedFontFamily:       ScreenToolsController.fixedFontFamily
     /* This mostly works but for some reason, reflowWidths() in SetupView doesn't change size.
        I've disabled (in release builds) until I figure out why. Changes require a restart for now.
     */

src/QmlControls/ScreenToolsController.cc

@@ -12,7 +12,9 @@
 /// @author Gus Grubba <mavlink@grubba.com>
 
 #include "ScreenToolsController.h"
+#include <QFontDatabase>
 #include <QScreen>
+
 #if defined(__ios__)
 #include <sys/utsname.h>
 #endif
@@ -23,7 +25,7 @@ ScreenToolsController::ScreenToolsController()
 }
 
 QString
-ScreenToolsController::iOSDevice()
+ScreenToolsController::iOSDevice() const
 {
 #if defined(__ios__)
     struct utsname systemInfo;
@@ -33,3 +35,9 @@ ScreenToolsController::iOSDevice()
     return QString();
 #endif
 }
+
+QString
+ScreenToolsController::fixedFontFamily() const
+{
+    return QFontDatabase::systemFont(QFontDatabase::FixedFont).family();
+}

src/QmlControls/ScreenToolsController.h

@@ -37,6 +37,7 @@ public:
     Q_PROPERTY(bool     isMacOS             READ isMacOS         CONSTANT)
     Q_PROPERTY(bool     isLinux             READ isLinux         CONSTANT)
     Q_PROPERTY(QString  iOSDevice           READ iOSDevice       CONSTANT)
+    Q_PROPERTY(QString  fixedFontFamily     READ fixedFontFamily CONSTANT)
 
     // Returns current mouse position
     Q_INVOKABLE int mouseX(void) { return QCursor::pos().x(); }
@@ -83,7 +84,8 @@ public:
     bool testHighDPI            () { return false; }
 #endif
 
-    QString  iOSDevice          ();
+    QString  iOSDevice          () const;
+    QString  fixedFontFamily    () const;
 
 };
 

src/Vehicle/Vehicle.cc

@@ -582,6 +582,13 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         _handleScaledPressure3(message);
         break;
 
+    case MAVLINK_MSG_ID_SERIAL_CONTROL:
+    {
+        mavlink_serial_control_t ser;
+        mavlink_msg_serial_control_decode(&message, &ser);
+        emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
+    }
+        break;
     // Following are ArduPilot dialect messages
 
     case MAVLINK_MSG_ID_WIND:

src/Vehicle/Vehicle.h

@@ -749,6 +749,9 @@ signals:
     ///     @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result
     void mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
 
+    // Mavlink Serial Data
+    void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
+
 private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _telemetryChanged(LinkInterface* link, unsigned rxerrors, unsigned fixed, int rssi, int remrssi, unsigned txbuf, unsigned noise, unsigned remnoise);