MavlinkConsoleController.cc 7.62 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14
/****************************************************************************
 *
 *   (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()
15 16
    : QStringListModel(),
      _cursor_home_pos{-1},
17 18 19 20 21 22 23 24 25 26 27
      _cursor{0},
      _vehicle{nullptr}
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &MavlinkConsoleController::_setActiveVehicle);
    _setActiveVehicle(manager->activeVehicle());
}

MavlinkConsoleController::~MavlinkConsoleController()
{
    if (_vehicle) {
28
        QByteArray msg;
29 30 31 32 33 34 35
        _sendSerialData(msg, true);
    }
}

void
MavlinkConsoleController::sendCommand(QString command)
{
36
    _history.append(command);
37 38 39
    command.append("\n");
    _sendSerialData(qPrintable(command));
    _cursor_home_pos = -1;
40
    _cursor = rowCount();
41 42
}

43 44 45 46 47 48 49 50 51 52 53 54
QString
MavlinkConsoleController::historyUp(const QString& current)
{
    return _history.up(current);
}

QString
MavlinkConsoleController::historyDown(const QString& current)
{
    return _history.down(current);
}

55 56 57 58 59 60 61 62 63 64 65
void
MavlinkConsoleController::_setActiveVehicle(Vehicle* vehicle)
{
    for (auto &con : _uas_connections)
        disconnect(con);
    _uas_connections.clear();

    _vehicle = vehicle;

    if (_vehicle) {
        _incoming_buffer.clear();
66 67 68 69
        // Reset the model
        setStringList(QStringList());
        _cursor = 0;
        _cursor_home_pos = -1;
70 71 72 73 74 75 76 77 78 79 80 81
        _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);
82 83 84 85 86 87 88 89 90
    while(!_incoming_buffer.isEmpty()) {
        bool newline = false;
        int idx = _incoming_buffer.indexOf('\n');
        if (idx == -1) {
            // Read the whole incoming buffer
            idx = _incoming_buffer.size();
        } else {
            newline = true;
        }
91

92 93 94 95 96 97 98 99 100 101
        QByteArray fragment = _incoming_buffer.mid(0, idx);
        if (_processANSItext(fragment)) {
            writeLine(_cursor, fragment);
            if (newline)
                _cursor++;
            _incoming_buffer.remove(0, idx + (newline ? 1 : 0));
        } else {
            // ANSI processing failed, need more data
            return;
        }
102 103 104 105 106 107
    }
}

void
MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
108 109
    if (!_vehicle) {
        qWarning() << "Internal error";
110
        return;
DonLakeFlyer's avatar
DonLakeFlyer committed
111
    }
112 113 114 115

    // Send maximum sized chunks until the complete buffer is transmitted
    while(data.size()) {
        QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
116
        uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
        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()));
132
        _vehicle->sendMessageOnLinkThreadSafe(priority_link, msg);
133 134 135 136
        data.remove(0, chunk.size());
    }
}

137 138
bool
MavlinkConsoleController::_processANSItext(QByteArray &line)
139 140
{
    // Iterate over the incoming buffer to parse off known ANSI control codes
141 142 143 144
    for (int i = 0; i < line.size(); i++) {
        if (line.at(i) == '\x1B') {
            // For ANSI codes we expect at least 3 incoming chars
            if (i < line.size() - 2 && line.at(i+1) == '[') {
145
                // Parse ANSI code
146
                switch(line.at(i+2)) {
147 148 149 150 151 152 153 154 155 156 157 158 159
                    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
160 161 162
                        if (_cursor < rowCount()) {
                            setData(index(_cursor), "");
                        }
163 164
                        break;
                    case '2':
165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
                        // Check for sufficient buffer size
                        if ( i >= line.size() - 3)
                            return false;

                        if (line.at(i+3) == 'J' && _cursor_home_pos != -1) {
                            // Erase everything and rewind to home
                            bool blocked = blockSignals(true);
                            for (int j = _cursor_home_pos; j < rowCount(); j++)
                                setData(index(j), "");
                            blockSignals(blocked);
                            QVector<int> roles;
                            roles.reserve(2);
                            roles.append(Qt::DisplayRole);
                            roles.append(Qt::EditRole);
                            emit dataChanged(index(_cursor), index(rowCount()), roles);
180 181
                        }
                        // Even if we didn't understand this ANSI code, remove the 4th char
182
                        line.remove(i+3,1);
183 184 185
                        break;
                }
                // Remove the parsed ANSI code and decrement the bufferpos
186
                line.remove(i, 3);
187 188 189 190
                i--;
            } else {
                // We can reasonably expect a control code was fragemented
                // Stop parsing here and wait for it to come in
191
                return false;
192 193 194
            }
        }
    }
195 196
    return true;
}
197

198 199 200 201 202 203 204 205 206
void
MavlinkConsoleController::writeLine(int line, const QByteArray &text)
{
    auto rc = rowCount();
    if (line >= rc) {
        insertRows(rc, 1 + line - rc);
    }
    auto idx = index(line);
    setData(idx, data(idx, Qt::DisplayRole).toString() + text);
207
}
208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247

void MavlinkConsoleController::CommandHistory::append(const QString& command)
{
    if (command.length() > 0) {

        // do not append duplicates
        if (_history.length() == 0 || _history.last() != command) {

            if (_history.length() >= maxHistoryLength) {
                _history.removeFirst();
            }
            _history.append(command);
        }
    }
    _index = _history.length();
}

QString MavlinkConsoleController::CommandHistory::up(const QString& current)
{
    if (_index <= 0)
        return current;

    --_index;
    if (_index < _history.length()) {
        return _history[_index];
    }
    return "";
}

QString MavlinkConsoleController::CommandHistory::down(const QString& current)
{
    if (_index >= _history.length())
        return current;

    ++_index;
    if (_index < _history.length()) {
        return _history[_index];
    }
    return "";
}