MockLink.cc 37.5 KB
Newer Older
1
2
3
4
5
6
7
8
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/
9

Don Gagne's avatar
Don Gagne committed
10
11

#include "MockLink.h"
12
#include "QGCLoggingCategory.h"
13
#include "QGCApplication.h"
Don Gagne's avatar
Don Gagne committed
14
15
16
17
18
19
20

#include <QTimer>
#include <QDebug>
#include <QFile>

#include <string.h>

21
QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")
Don Gagne's avatar
Don Gagne committed
22
QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
23

Don Gagne's avatar
Don Gagne committed
24
25
26
27
28
/// @file
///     @brief Mock implementation of a Link.
///
///     @author Don Gagne <don@thegagnes.com>

Don Gagne's avatar
Don Gagne committed
29
30
31
32
33
34
35
enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
36
37
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
Don Gagne's avatar
Don Gagne committed
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
};

enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};

union px4_custom_mode {
    struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
    float data_float;
};

Don Gagne's avatar
Don Gagne committed
60
61
float MockLink::_vehicleLatitude =  47.633033f;
float MockLink::_vehicleLongitude = -122.08794f;
Don Gagne's avatar
Don Gagne committed
62
float MockLink::_vehicleAltitude =  3.5f;
Don Gagne's avatar
Don Gagne committed
63

64
const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
65
const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";
66
const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";
Don Gagne's avatar
Don Gagne committed
67

68
MockLink::MockLink(MockConfiguration* config)
69
    : _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol())
70
71
72
73
74
    , _name("MockLink")
    , _connected(false)
    , _vehicleSystemId(128)     // FIXME: Pull from eventual parameter manager
    , _vehicleComponentId(200)  // FIXME: magic number?
    , _inNSH(false)
Don Gagne's avatar
Don Gagne committed
75
    , _mavlinkStarted(true)
76
77
    , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
    , _mavState(MAV_STATE_STANDBY)
Don Gagne's avatar
Don Gagne committed
78
    , _firmwareType(MAV_AUTOPILOT_PX4)
79
    , _vehicleType(MAV_TYPE_QUADROTOR)
80
    , _fileServer(NULL)
81
    , _sendStatusText(false)
Don Gagne's avatar
Don Gagne committed
82
    , _apmSendHomePositionOnEmptyList(false)
83
    , _sendHomePositionDelayCount(2)
Don Gagne's avatar
Don Gagne committed
84
{
85
    _config = config;
Don Gagne's avatar
Don Gagne committed
86
    if (_config) {
Don Gagne's avatar
Don Gagne committed
87
        _firmwareType = config->firmwareType();
88
        _vehicleType = config->vehicleType();
89
        _sendStatusText = config->sendStatusText();
Don Gagne's avatar
Don Gagne committed
90
        _config->setLink(this);
Don Gagne's avatar
Don Gagne committed
91
92
    }

Don Gagne's avatar
Don Gagne committed
93
94
95
96
97
98
    union px4_custom_mode   px4_cm;

    px4_cm.data = 0;
    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
    _mavCustomMode = px4_cm.data;

Don Gagne's avatar
Don Gagne committed
99
100
    _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
    Q_CHECK_PTR(_fileServer);
dogmaphobic's avatar
dogmaphobic committed
101

Don Gagne's avatar
Don Gagne committed
102
    moveToThread(this);
dogmaphobic's avatar
dogmaphobic committed
103

Don Gagne's avatar
Don Gagne committed
104
105
106
107
108
    _loadParams();
}

MockLink::~MockLink(void)
{
109
    _disconnect();
Don Gagne's avatar
Don Gagne committed
110
111
}

112
bool MockLink::_connect(void)
Don Gagne's avatar
Don Gagne committed
113
{
114
115
116
117
118
    if (!_connected) {
        _connected = true;
        start();
        emit connected();
    }
119

Don Gagne's avatar
Don Gagne committed
120
121
122
    return true;
}

Don Gagne's avatar
Don Gagne committed
123
void MockLink::_disconnect(void)
Don Gagne's avatar
Don Gagne committed
124
{
125
126
    if (_connected) {
        _connected = false;
Daniel Agar's avatar
Daniel Agar committed
127
128
        quit();
        wait();
129
130
        emit disconnected();
    }
Don Gagne's avatar
Don Gagne committed
131
132
133
134
135
136
137
}

void MockLink::run(void)
{
    QTimer  _timer1HzTasks;
    QTimer  _timer10HzTasks;
    QTimer  _timer50HzTasks;
138

dogmaphobic's avatar
dogmaphobic committed
139
    QObject::connect(&_timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
Don Gagne's avatar
Don Gagne committed
140
141
    QObject::connect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::connect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);
142

Don Gagne's avatar
Don Gagne committed
143
144
145
    _timer1HzTasks.start(1000);
    _timer10HzTasks.start(100);
    _timer50HzTasks.start(20);
146

Don Gagne's avatar
Don Gagne committed
147
    exec();
148

dogmaphobic's avatar
dogmaphobic committed
149
    QObject::disconnect(&_timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
150
151
    QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);
dogmaphobic's avatar
dogmaphobic committed
152

153
    _missionItemHandler.shutdown();
Don Gagne's avatar
Don Gagne committed
154
155
156
157
}

void MockLink::_run1HzTasks(void)
{
158
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
159
        _sendHeartBeat();
Don Gagne's avatar
Don Gagne committed
160
        _sendVibration();
161
162
163
164
        if (!qgcApp()->runningUnitTests()) {
            // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
            _sendRCChannels();
        }
165
166
167
168
169
170
        if (_sendHomePositionDelayCount > 0) {
            // We delay home position a bit to be more realistic
            _sendHomePositionDelayCount--;
        } else {
            _sendHomePosition();
        }
171
172
173
174
        if (_sendStatusText) {
            _sendStatusText = false;
            _sendStatusTextMessages();
        }
Don Gagne's avatar
Don Gagne committed
175
176
177
178
179
    }
}

void MockLink::_run10HzTasks(void)
{
180
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
181
        _sendGpsRawInt();
Don Gagne's avatar
Don Gagne committed
182
183
184
185
186
    }
}

void MockLink::_run50HzTasks(void)
{
187
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
188
189
190
191
192
    }
}

void MockLink::_loadParams(void)
{
193
194
195
196
197
198
199
200
201
202
203
204
    QFile paramFile;

    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        if (_vehicleType == MAV_TYPE_FIXED_WING) {
            paramFile.setFileName(":/unittest/APMArduPlaneMockLink.params");
        } else {
            paramFile.setFileName(":/unittest/APMArduCopterMockLink.params");
        }
    } else {
        paramFile.setFileName(":/unittest/PX4MockLink.params");
    }

205

Don Gagne's avatar
Don Gagne committed
206
207
208
    bool success = paramFile.open(QFile::ReadOnly);
    Q_UNUSED(success);
    Q_ASSERT(success);
209

Don Gagne's avatar
Don Gagne committed
210
    QTextStream paramStream(&paramFile);
211

Don Gagne's avatar
Don Gagne committed
212
213
    while (!paramStream.atEnd()) {
        QString line = paramStream.readLine();
214

Don Gagne's avatar
Don Gagne committed
215
216
217
        if (line.startsWith("#")) {
            continue;
        }
218

Don Gagne's avatar
Don Gagne committed
219
220
        QStringList paramData = line.split("\t");
        Q_ASSERT(paramData.count() == 5);
221

222
        int componentId = paramData.at(1).toInt();
Don Gagne's avatar
Don Gagne committed
223
224
225
        QString paramName = paramData.at(2);
        QString valStr = paramData.at(3);
        uint paramType = paramData.at(4).toUInt();
226

Don Gagne's avatar
Don Gagne committed
227
228
229
230
231
232
233
234
235
236
237
        QVariant paramValue;
        switch (paramType) {
            case MAV_PARAM_TYPE_REAL32:
                paramValue = QVariant(valStr.toFloat());
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramValue = QVariant(valStr.toUInt());
                break;
            case MAV_PARAM_TYPE_INT32:
                paramValue = QVariant(valStr.toInt());
                break;
238
239
240
241
242
243
244
245
246
            case MAV_PARAM_TYPE_UINT16:
                paramValue = QVariant((quint16)valStr.toUInt());
                break;
            case MAV_PARAM_TYPE_INT16:
                paramValue = QVariant((qint16)valStr.toInt());
                break;
            case MAV_PARAM_TYPE_UINT8:
                paramValue = QVariant((quint8)valStr.toUInt());
                break;
Don Gagne's avatar
Don Gagne committed
247
            case MAV_PARAM_TYPE_INT8:
248
                paramValue = QVariant((qint8)valStr.toUInt());
Don Gagne's avatar
Don Gagne committed
249
250
                break;
            default:
251
252
                qCritical() << "Unknown type" << paramType;
                paramValue = QVariant(valStr.toInt());
Don Gagne's avatar
Don Gagne committed
253
254
                break;
        }
255

Don Gagne's avatar
Don Gagne committed
256
        qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
257

258
        _mapParamName2Value[componentId][paramName] = paramValue;
259
        _mapParamName2MavParamType[paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
Don Gagne's avatar
Don Gagne committed
260
261
262
263
264
265
266
267
268
269
    }
}

void MockLink::_sendHeartBeat(void)
{
    mavlink_message_t   msg;

    mavlink_msg_heartbeat_pack(_vehicleSystemId,
                               _vehicleComponentId,
                               &msg,
Don Gagne's avatar
Don Gagne committed
270
                               _vehicleType,        // MAV_TYPE
Don Gagne's avatar
Don Gagne committed
271
                               _firmwareType,      // MAV_AUTOPILOT
Don Gagne's avatar
Don Gagne committed
272
273
                               _mavBaseMode,        // MAV_MODE
                               _mavCustomMode,      // custom mode
Don Gagne's avatar
Don Gagne committed
274
                               _mavState);          // MAV_STATE
dogmaphobic's avatar
dogmaphobic committed
275

Don Gagne's avatar
Don Gagne committed
276
277
    respondWithMavlinkMessage(msg);
}
Don Gagne's avatar
Don Gagne committed
278

Don Gagne's avatar
Don Gagne committed
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
void MockLink::_sendVibration(void)
{
    mavlink_message_t   msg;

    mavlink_msg_vibration_pack(_vehicleSystemId,
                               _vehicleComponentId,
                               &msg,
                               0,       // time_usec
                               50.5,    // vibration_x,
                               10.5,    // vibration_y,
                               60.0,    // vibration_z,
                               1,       // clipping_0
                               2,       // clipping_0
                               3);      // clipping_0

    respondWithMavlinkMessage(msg);
}

Don Gagne's avatar
Don Gagne committed
297
298
299
void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg)
{
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
dogmaphobic's avatar
dogmaphobic committed
300

Don Gagne's avatar
Don Gagne committed
301
302
303
304
305
306
    int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
    QByteArray bytes((char *)buffer, cBuffer);
    emit bytesReceived(this, bytes);
}

/// @brief Called when QGC wants to write bytes to the MAV
307
void MockLink::_writeBytes(const QByteArray bytes)
Don Gagne's avatar
Don Gagne committed
308
309
310
311
312
313
314
315
{
    if (_inNSH) {
        _handleIncomingNSHBytes(bytes.constData(), bytes.count());
    } else {
        if (bytes.startsWith(QByteArray("\r\r\r"))) {
            _inNSH  = true;
            _handleIncomingNSHBytes(&bytes.constData()[3], bytes.count() - 3);
        }
316

Don Gagne's avatar
Don Gagne committed
317
318
319
320
321
322
323
324
        _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count());
    }
}

/// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell
void MockLink::_handleIncomingNSHBytes(const char* bytes, int cBytes)
{
    Q_UNUSED(cBytes);
325

Don Gagne's avatar
Don Gagne committed
326
327
328
329
330
331
332
333
    // Drop back out of NSH
    if (cBytes == 4 && bytes[0] == '\r' && bytes[1] == '\r' && bytes[2] == '\r') {
        _inNSH  = false;
        return;
    }

    if (cBytes > 0) {
        qDebug() << "NSH:" << (const char*)bytes;
334

Don Gagne's avatar
Don Gagne committed
335
336
#if 0
        // MockLink not quite ready to handle this correctly yet
Don Gagne's avatar
Don Gagne committed
337
338
339
340
        if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
            // This is the mavlink start command
            _mavlinkStarted = true;
        }
Don Gagne's avatar
Don Gagne committed
341
#endif
Don Gagne's avatar
Don Gagne committed
342
343
344
345
346
347
348
349
    }
}

/// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol
void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
{
    mavlink_message_t msg;
    mavlink_status_t comm;
350

Don Gagne's avatar
Don Gagne committed
351
352
    for (qint64 i=0; i<cBytes; i++)
    {
353
        if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) {
Don Gagne's avatar
Don Gagne committed
354
355
            continue;
        }
dogmaphobic's avatar
dogmaphobic committed
356

357
        if (_missionItemHandler.handleMessage(msg)) {
Don Gagne's avatar
Don Gagne committed
358
359
            continue;
        }
360

Don Gagne's avatar
Don Gagne committed
361
362
363
364
        switch (msg.msgid) {
            case MAVLINK_MSG_ID_HEARTBEAT:
                _handleHeartBeat(msg);
                break;
365

Don Gagne's avatar
Don Gagne committed
366
367
368
            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
                _handleParamRequestList(msg);
                break;
369

Don Gagne's avatar
Don Gagne committed
370
371
372
            case MAVLINK_MSG_ID_SET_MODE:
                _handleSetMode(msg);
                break;
373

Don Gagne's avatar
Don Gagne committed
374
375
376
            case MAVLINK_MSG_ID_PARAM_SET:
                _handleParamSet(msg);
                break;
377

Don Gagne's avatar
Don Gagne committed
378
379
380
            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
                _handleParamRequestRead(msg);
                break;
dogmaphobic's avatar
dogmaphobic committed
381

Don Gagne's avatar
Don Gagne committed
382
383
384
            case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
                _handleFTP(msg);
                break;
dogmaphobic's avatar
dogmaphobic committed
385

386
387
388
            case MAVLINK_MSG_ID_COMMAND_LONG:
                _handleCommandLong(msg);
                break;
389

Don Gagne's avatar
Don Gagne committed
390
391
392
393
            case MAVLINK_MSG_ID_MANUAL_CONTROL:
                _handleManualControl(msg);
                break;

Don Gagne's avatar
Don Gagne committed
394
395
396
397
398
399
400
401
402
            default:
                break;
        }
    }
}

void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
{
    Q_UNUSED(msg);
403
    qCDebug(MockLinkLog) << "Heartbeat";
Don Gagne's avatar
Don Gagne committed
404
405
406
407
408
409
}

void MockLink::_handleSetMode(const mavlink_message_t& msg)
{
    mavlink_set_mode_t request;
    mavlink_msg_set_mode_decode(&msg, &request);
410

411
    Q_ASSERT(request.target_system == _vehicleSystemId);
412

413
414
415
416
    _mavBaseMode = request.base_mode;
    _mavCustomMode = request.custom_mode;
}

Don Gagne's avatar
Don Gagne committed
417
418
419
420
421
422
423
424
void MockLink::_handleManualControl(const mavlink_message_t& msg)
{
    mavlink_manual_control_t manualControl;
    mavlink_msg_manual_control_decode(&msg, &manualControl);

    qDebug() << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r;
}

425
void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
426
427
{
    mavlink_param_union_t   valueUnion;
428

429
430
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
431
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
432

433
    valueUnion.param_float = paramFloat;
434

435
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
436

437
    QVariant paramVariant;
438

439
    switch (paramType) {
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
    case MAV_PARAM_TYPE_REAL32:
        paramVariant = QVariant::fromValue(valueUnion.param_float);
        break;

    case MAV_PARAM_TYPE_UINT32:
        paramVariant = QVariant::fromValue(valueUnion.param_uint32);
        break;

    case MAV_PARAM_TYPE_INT32:
        paramVariant = QVariant::fromValue(valueUnion.param_int32);
        break;

    case MAV_PARAM_TYPE_UINT16:
        paramVariant = QVariant::fromValue(valueUnion.param_uint16);
        break;

    case MAV_PARAM_TYPE_INT16:
        paramVariant = QVariant::fromValue(valueUnion.param_int16);
        break;

    case MAV_PARAM_TYPE_UINT8:
        paramVariant = QVariant::fromValue(valueUnion.param_uint8);
        break;

    case MAV_PARAM_TYPE_INT8:
        paramVariant = QVariant::fromValue(valueUnion.param_int8);
        break;

    default:
        qCritical() << "Invalid parameter type" << paramType;
        paramVariant = QVariant::fromValue(valueUnion.param_int32);
        break;
Don Gagne's avatar
Don Gagne committed
472
    }
473

474
    qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
475
    _mapParamName2Value[componentId][paramName] = paramVariant;
Don Gagne's avatar
Don Gagne committed
476
477
}

478
/// Convert from a parameter variant to the float value from mavlink_param_union_t
479
float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
Don Gagne's avatar
Don Gagne committed
480
{
481
    mavlink_param_union_t   valueUnion;
482

483
484
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
485
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
486

487
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
488
    QVariant paramVar = _mapParamName2Value[componentId][paramName];
489

490
    switch (paramType) {
491
492
493
    case MAV_PARAM_TYPE_REAL32:
            valueUnion.param_float = paramVar.toFloat();
        break;
494

495
496
497
498
499
500
501
    case MAV_PARAM_TYPE_UINT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint32 = paramVar.toUInt();
        }
        break;
502

503
504
505
506
507
508
509
    case MAV_PARAM_TYPE_INT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toInt();
        } else {
            valueUnion.param_int32 = paramVar.toInt();
        }
        break;
510

511
512
513
514
515
516
517
    case MAV_PARAM_TYPE_UINT16:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint16 = paramVar.toUInt();
        }
        break;
518

519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
    case MAV_PARAM_TYPE_INT16:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toInt();
        } else {
            valueUnion.param_int16 = paramVar.toInt();
        }
        break;

    case MAV_PARAM_TYPE_UINT8:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint8 = paramVar.toUInt();
        }
        break;

    case MAV_PARAM_TYPE_INT8:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
        } else {
            valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
        }
        break;

    default:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toInt();
        } else {
            valueUnion.param_int32 = paramVar.toInt();
        }
        qCritical() << "Invalid parameter type" << paramType;
550
    }
551

552
    return valueUnion.param_float;
Don Gagne's avatar
Don Gagne committed
553
554
555
556
557
}

void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
    mavlink_param_request_list_t request;
558

Don Gagne's avatar
Don Gagne committed
559
    mavlink_msg_param_request_list_decode(&msg, &request);
560

561
    Q_ASSERT(request.target_system == _vehicleSystemId);
562
    Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
dogmaphobic's avatar
dogmaphobic committed
563

564
565
    // We must send the first parameter for each component first. Otherwise system won't correctly know
    // when all parameters are loaded.
566
567

    foreach (int componentId, _mapParamName2Value.keys()) {
Don Gagne's avatar
Don Gagne committed
568
        uint16_t paramIndex = 0;
569
        int cParameters = _mapParamName2Value[componentId].count();
dogmaphobic's avatar
dogmaphobic committed
570

571
        foreach(const QString &paramName, _mapParamName2Value[componentId].keys()) {
572
573
574
575
576
577
578
579
580
581
582
            char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
            mavlink_message_t       responseMsg;

            Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
            Q_ASSERT(_mapParamName2MavParamType.contains(paramName));

            MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];

            Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
            strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);

Don Gagne's avatar
Don Gagne committed
583
            qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];
584
585
586
587
588
589
590
591
592

            mavlink_msg_param_value_pack(_vehicleSystemId,
                                         componentId,                       // component id
                                         &responseMsg,                      // Outgoing message
                                         paramId,                           // Parameter name
                                         _floatUnionForParam(componentId, paramName),    // Parameter value
                                         paramType,                         // MAV_PARAM_TYPE
                                         cParameters,                       // Total number of parameters
                                         paramIndex++);                     // Index of this parameter
Don Gagne's avatar
Don Gagne committed
593
            respondWithMavlinkMessage(responseMsg);
dogmaphobic's avatar
dogmaphobic committed
594

595
596
597
598
            // Only first parameter the first time through
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
599

600
601
602
603
    foreach (int componentId, _mapParamName2Value.keys()) {
        uint16_t paramIndex = 0;
        int cParameters = _mapParamName2Value[componentId].count();
        bool skipParam = true;
dogmaphobic's avatar
dogmaphobic committed
604

605
        foreach(const QString &paramName, _mapParamName2Value[componentId].keys()) {
606
607
            if (skipParam) {
                // We've already sent the first param
Don Gagne's avatar
Don Gagne committed
608
                skipParam = false;
609
610
611
612
                paramIndex++;
            } else {
                char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
                mavlink_message_t       responseMsg;
dogmaphobic's avatar
dogmaphobic committed
613

614
615
                Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
                Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
dogmaphobic's avatar
dogmaphobic committed
616

617
                MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
dogmaphobic's avatar
dogmaphobic committed
618

619
620
                Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
                strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
dogmaphobic's avatar
dogmaphobic committed
621

622
                qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];
dogmaphobic's avatar
dogmaphobic committed
623

624
625
626
627
628
629
630
631
                mavlink_msg_param_value_pack(_vehicleSystemId,
                                             componentId,                       // component id
                                             &responseMsg,                      // Outgoing message
                                             paramId,                           // Parameter name
                                             _floatUnionForParam(componentId, paramName),    // Parameter value
                                             paramType,                         // MAV_PARAM_TYPE
                                             cParameters,                       // Total number of parameters
                                             paramIndex++);                     // Index of this parameter
Don Gagne's avatar
Don Gagne committed
632
                respondWithMavlinkMessage(responseMsg);
633
            }
634
        }
Don Gagne's avatar
Don Gagne committed
635
636
637
638
639
640
641
    }
}

void MockLink::_handleParamSet(const mavlink_message_t& msg)
{
    mavlink_param_set_t request;
    mavlink_msg_param_set_decode(&msg, &request);
642

643
    Q_ASSERT(request.target_system == _vehicleSystemId);
644
    int componentId = request.target_component;
dogmaphobic's avatar
dogmaphobic committed
645

646
647
    // Param may not be null terminated if exactly fits
    char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
Don Gagne's avatar
Don Gagne committed
648
    paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
649
    strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
650

Don Gagne's avatar
Don Gagne committed
651
    qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type;
dogmaphobic's avatar
dogmaphobic committed
652

653
654
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
655
    Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);
656

657
    // Save the new value
658
    _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
659

660
661
662
    // Respond with a param_value to ack
    mavlink_message_t responseMsg;
    mavlink_msg_param_value_pack(_vehicleSystemId,
663
664
665
666
667
668
669
                                 componentId,                                               // component id
                                 &responseMsg,                                              // Outgoing message
                                 paramId,                                                   // Parameter name
                                 request.param_value,                                       // Send same value back
                                 request.param_type,                                        // Send same type back
                                 _mapParamName2Value[componentId].count(),                  // Total number of parameters
                                 _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
Don Gagne's avatar
Don Gagne committed
670
    respondWithMavlinkMessage(responseMsg);
Don Gagne's avatar
Don Gagne committed
671
672
673
674
}

void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
{
675
    mavlink_message_t   responseMsg;
Don Gagne's avatar
Don Gagne committed
676
677
    mavlink_param_request_read_t request;
    mavlink_msg_param_request_read_decode(&msg, &request);
678
679

    const QString param_name(QString::fromLocal8Bit(request.param_id, strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)));
680
    int componentId = request.target_component;
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699

    // special case for magic _HASH_CHECK value
    if (request.target_component == MAV_COMP_ID_ALL && param_name == "_HASH_CHECK") {
        mavlink_param_union_t   valueUnion;
        valueUnion.type = MAV_PARAM_TYPE_UINT32;
        valueUnion.param_uint32 = 0;
        // Special case of magic hash check value
        mavlink_msg_param_value_pack(_vehicleSystemId,
                                     componentId,
                                     &responseMsg,
                                     request.param_id,
                                     valueUnion.param_float,
                                     MAV_PARAM_TYPE_UINT32,
                                     0,
                                     -1);
        respondWithMavlinkMessage(responseMsg);
        return;
    }

700
    Q_ASSERT(_mapParamName2Value.contains(componentId));
701

Don Gagne's avatar
Don Gagne committed
702
703
    char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
    paramId[0] = 0;
704

705
    Q_ASSERT(request.target_system == _vehicleSystemId);
706

707
708
709
    if (request.param_index == -1) {
        // Request is by param name. Param may not be null terminated if exactly fits
        strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
Don Gagne's avatar
Don Gagne committed
710
    } else {
711
        // Request is by index
712

Don Gagne's avatar
Don Gagne committed
713
        Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count());
714

715
        QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
716
717
        Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
        strcpy(paramId, key.toLocal8Bit().constData());
Don Gagne's avatar
Don Gagne committed
718
    }
719

720
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
721
    Q_ASSERT(_mapParamName2MavParamType.contains(paramId));
722

723
    mavlink_msg_param_value_pack(_vehicleSystemId,
724
725
726
727
728
729
730
                                 componentId,                                               // component id
                                 &responseMsg,                                              // Outgoing message
                                 paramId,                                                   // Parameter name
                                 _floatUnionForParam(componentId, paramId),                 // Parameter value
                                 _mapParamName2MavParamType[paramId],                       // Parameter type
                                 _mapParamName2Value[componentId].count(),                  // Total number of parameters
                                 _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
Don Gagne's avatar
Don Gagne committed
731
    respondWithMavlinkMessage(responseMsg);
Don Gagne's avatar
Don Gagne committed
732
733
}

Don Gagne's avatar
Don Gagne committed
734
735
736
void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
{
    uint16_t chanRaw[18];
dogmaphobic's avatar
dogmaphobic committed
737

Don Gagne's avatar
Don Gagne committed
738
739
740
741
    for (int i=0; i<18; i++) {
        chanRaw[i] = UINT16_MAX;
    }
    chanRaw[channel] = raw;
dogmaphobic's avatar
dogmaphobic committed
742

Don Gagne's avatar
Don Gagne committed
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
    mavlink_message_t responseMsg;
    mavlink_msg_rc_channels_pack(_vehicleSystemId,
                                 _vehicleComponentId,
                                 &responseMsg,          // Outgoing message
                                 0,                     // time since boot, ignored
                                 18,                    // channel count
                                 chanRaw[0],            // channel raw value
                                 chanRaw[1],            // channel raw value
                                 chanRaw[2],            // channel raw value
                                 chanRaw[3],            // channel raw value
                                 chanRaw[4],            // channel raw value
                                 chanRaw[5],            // channel raw value
                                 chanRaw[6],            // channel raw value
                                 chanRaw[7],            // channel raw value
                                 chanRaw[8],            // channel raw value
                                 chanRaw[9],            // channel raw value
                                 chanRaw[10],           // channel raw value
                                 chanRaw[11],           // channel raw value
                                 chanRaw[12],           // channel raw value
                                 chanRaw[13],           // channel raw value
                                 chanRaw[14],           // channel raw value
                                 chanRaw[15],           // channel raw value
                                 chanRaw[16],           // channel raw value
                                 chanRaw[17],           // channel raw value
                                 0);                    // rss
Don Gagne's avatar
Don Gagne committed
768
769
770
771
772
773
774
    respondWithMavlinkMessage(responseMsg);
}

void MockLink::_handleFTP(const mavlink_message_t& msg)
{
    Q_ASSERT(_fileServer);
    _fileServer->handleFTPMessage(msg);
Don Gagne's avatar
Don Gagne committed
775
}
776
777
778
779

void MockLink::_handleCommandLong(const mavlink_message_t& msg)
{
    mavlink_command_long_t request;
780
    uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
dogmaphobic's avatar
dogmaphobic committed
781

782
783
    mavlink_msg_command_long_decode(&msg, &request);

784
785
    switch (request.command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
786
787
788
789
790
        if (request.param1 == 0.0f) {
            _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
        } else {
            _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
        }
791
        commandResult = MAV_RESULT_ACCEPTED;
792
793
794
795
796
        break;
    case MAV_CMD_PREFLIGHT_CALIBRATION:
    case MAV_CMD_PREFLIGHT_STORAGE:
        commandResult = MAV_RESULT_ACCEPTED;
        break;
797
798
799
800
    case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
        commandResult = MAV_RESULT_ACCEPTED;
        _respondWithAutopilotVersion();
        break;
801
    }
802
803
804
805
806
807
808
809

    mavlink_message_t commandAck;
    mavlink_msg_command_ack_pack(_vehicleSystemId,
                                 _vehicleComponentId,
                                 &commandAck,
                                 request.command,
                                 commandResult);
    respondWithMavlinkMessage(commandAck);
810
811
}

812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
void MockLink::_respondWithAutopilotVersion(void)
{
    mavlink_message_t msg;

    uint8_t customVersion[8];
    memset(customVersion, 0, sizeof(customVersion));

    // Only flight_sw_version is supported a this point
    mavlink_msg_autopilot_version_pack(_vehicleSystemId,
                                       _vehicleComponentId,
                                       &msg,
                                       0,                                           // capabilities,
                                       (1 << (8*3)) | FIRMWARE_VERSION_TYPE_DEV,    // flight_sw_version,
                                       0,                                           // middleware_sw_version,
                                       0,                                           // os_sw_version,
                                       0,                                           // board_version,
                                       (uint8_t *)&customVersion,                   // flight_custom_version,
                                       (uint8_t *)&customVersion,                   // middleware_custom_version,
                                       (uint8_t *)&customVersion,                   // os_custom_version,
                                       0,                                           // vendor_id,
                                       0,                                           // product_id,
                                       0);                                          // uid
    respondWithMavlinkMessage(msg);
}

837
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
838
{
839
    _missionItemHandler.setMissionItemFailureMode(failureMode);
840
}
841
842
843

void MockLink::_sendHomePosition(void)
{
Don Gagne's avatar
Don Gagne committed
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
    mavlink_message_t msg;

    float bogus[4];
    bogus[0] = 0.0f;
    bogus[1] = 0.0f;
    bogus[2] = 0.0f;
    bogus[3] = 0.0f;

    mavlink_msg_home_position_pack(_vehicleSystemId,
                                   _vehicleComponentId,
                                   &msg,
                                   (int32_t)(_vehicleLatitude * 1E7),
                                   (int32_t)(_vehicleLongitude * 1E7),
                                   (int32_t)(_vehicleAltitude * 1000),
                                   0.0f, 0.0f, 0.0f,
                                   &bogus[0],
            0.0f, 0.0f, 0.0f);
    respondWithMavlinkMessage(msg);
862
}
Don Gagne's avatar
Don Gagne committed
863
864
865
866
867
868
869
870
871
872
873

void MockLink::_sendGpsRawInt(void)
{
    static uint64_t timeTick = 0;
    mavlink_message_t msg;

    mavlink_msg_gps_raw_int_pack(_vehicleSystemId,
                                 _vehicleComponentId,
                                 &msg,
                                 timeTick++,                            // time since boot
                                 3,                                     // 3D fix
dogmaphobic's avatar
dogmaphobic committed
874
                                 (int32_t)(_vehicleLatitude  * 1E7),
Don Gagne's avatar
Don Gagne committed
875
                                 (int32_t)(_vehicleLongitude * 1E7),
dogmaphobic's avatar
dogmaphobic committed
876
                                 (int32_t)(_vehicleAltitude  * 1000),
Don Gagne's avatar
Don Gagne committed
877
878
879
880
881
882
                                 UINT16_MAX, UINT16_MAX,                // HDOP/VDOP not known
                                 UINT16_MAX,                            // velocity not known
                                 UINT16_MAX,                            // course over ground not known
                                 8);                                    // satellite count
    respondWithMavlinkMessage(msg);
}
Don Gagne's avatar
Don Gagne committed
883

884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
void MockLink::_sendStatusTextMessages(void)
{
    struct StatusMessage {
        MAV_SEVERITY        severity;
        const char*         msg;
    };

    static const struct StatusMessage rgMessages[] = {
        { MAV_SEVERITY_INFO,        "#Testing audio output" },
        { MAV_SEVERITY_EMERGENCY,   "Status text emergency" },
        { MAV_SEVERITY_ALERT,       "Status text alert" },
        { MAV_SEVERITY_CRITICAL,    "Status text critical" },
        { MAV_SEVERITY_ERROR,       "Status text error" },
        { MAV_SEVERITY_WARNING,     "Status text warning" },
        { MAV_SEVERITY_NOTICE,      "Status text notice" },
        { MAV_SEVERITY_INFO,        "Status text info" },
        { MAV_SEVERITY_DEBUG,       "Status text debug" },
    };

    for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
        mavlink_message_t msg;
        const struct StatusMessage* status = &rgMessages[i];

        mavlink_msg_statustext_pack(_vehicleSystemId,
                                    _vehicleComponentId,
                                    &msg,
                                    status->severity,
                                    status->msg);
        respondWithMavlinkMessage(msg);
    }
}

Don Gagne's avatar
Don Gagne committed
916
917
918
MockConfiguration::MockConfiguration(const QString& name)
    : LinkConfiguration(name)
    , _firmwareType(MAV_AUTOPILOT_PX4)
919
    , _vehicleType(MAV_TYPE_QUADROTOR)
920
    , _sendStatusText(false)
Don Gagne's avatar
Don Gagne committed
921
922
923
924
925
926
927
{

}

MockConfiguration::MockConfiguration(MockConfiguration* source)
    : LinkConfiguration(source)
{
928
    _firmwareType =     source->_firmwareType;
dogmaphobic's avatar
dogmaphobic committed
929
    _vehicleType =      source->_vehicleType;
930
    _sendStatusText =   source->_sendStatusText;
Don Gagne's avatar
Don Gagne committed
931
932
933
934
935
936
}

void MockConfiguration::copyFrom(LinkConfiguration *source)
{
    LinkConfiguration::copyFrom(source);
    MockConfiguration* usource = dynamic_cast<MockConfiguration*>(source);
937
938
939
940
941
942
943

    if (!usource) {
        qWarning() << "dynamic_cast failed" << source << usource;
        return;
    }

    _firmwareType =     usource->_firmwareType;
944
    _vehicleType =      usource->_vehicleType;
945
    _sendStatusText =   usource->_sendStatusText;
Don Gagne's avatar
Don Gagne committed
946
947
948
949
950
951
}

void MockConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    settings.setValue(_firmwareTypeKey, (int)_firmwareType);
952
    settings.setValue(_vehicleTypeKey, (int)_vehicleType);
953
    settings.setValue(_sendStatusTextKey, _sendStatusText);
Don Gagne's avatar
Don Gagne committed
954
955
956
957
958
959
960
961
    settings.sync();
    settings.endGroup();
}

void MockConfiguration::loadSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt();
962
    _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt();
963
    _sendStatusText = settings.value(_sendStatusTextKey, false).toBool();
Don Gagne's avatar
Don Gagne committed
964
965
966
967
968
969
970
971
972
    settings.endGroup();
}

void MockConfiguration::updateSettings()
{
    if (_link) {
        MockLink* ulink = dynamic_cast<MockLink*>(_link);
        if (ulink) {
            // Restart connect not supported
973
            qWarning() << "updateSettings not supported";
Don Gagne's avatar
Don Gagne committed
974
975
976
977
            //ulink->_restartConnection();
        }
    }
}
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000

MockLink*  MockLink::_startMockLink(MockConfiguration* mockConfig)
{
    LinkManager* linkManager = qgcApp()->toolbox()->linkManager();

    mockConfig->setDynamic(true);
    linkManager->linkConfigurations()->append(mockConfig);

    return qobject_cast<MockLink*>(linkManager->createConnectedLink(mockConfig));
}

MockLink*  MockLink::startPX4MockLink(bool sendStatusText)
{
    MockConfiguration* mockConfig = new MockConfiguration("PX4 MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_PX4);
    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
    mockConfig->setSendStatusText(sendStatusText);

    return _startMockLink(mockConfig);
}

MockLink*  MockLink::startGenericMockLink(bool sendStatusText)
For faster browsing, not all history is shown. View entire blame