MockLink.cc 62 KB
Newer Older
1
2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4
5
6
7
8
 *
 * 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"
14
15

#ifdef UNITTEST_BUILD
DonLakeFlyer's avatar
DonLakeFlyer committed
16
#include "UnitTest.h"
17
#endif
Don Gagne's avatar
Don Gagne committed
18
19
20
21
22
23
24

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

#include <string.h>

25
26
// FIXME: Hack to work around clean headers
#include "FirmwarePlugin/PX4/px4_custom_mode.h"
Daniel Agar's avatar
Daniel Agar committed
27

28
QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")
Don Gagne's avatar
Don Gagne committed
29
QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
30

Don Gagne's avatar
Don Gagne committed
31
32
33
34
35
/// @file
///     @brief Mock implementation of a Link.
///
///     @author Don Gagne <don@thegagnes.com>

36
37
// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
// testing of a gazebo vehicle and a mocklink vehicle
DonLakeFlyer's avatar
DonLakeFlyer committed
38
39
40
41
42
43
44
45
46
#if 1
double      MockLink::_defaultVehicleLatitude =     47.397;
double      MockLink::_defaultVehicleLongitude =    8.5455;
double      MockLink::_defaultVehicleAltitude =     488.056;
#else
double      MockLink::_defaultVehicleLatitude =     47.6333022928789;
double      MockLink::_defaultVehicleLongitude =    -122.08833157994995;
double      MockLink::_defaultVehicleAltitude =     19.0;
#endif
47
48
int         MockLink::_nextVehicleSystemId =        128;
const char* MockLink::_failParam =                  "COM_FLTMODE6";
Don Gagne's avatar
Don Gagne committed
49

50
const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
51
const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";
52
const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";
Don Gagne's avatar
Don Gagne committed
53
const char* MockConfiguration::_highLatencyKey =    "HighLatency";
Don Gagne's avatar
Don Gagne committed
54
const char* MockConfiguration::_failureModeKey =    "FailureMode";
Don Gagne's avatar
Don Gagne committed
55

56
MockLink::MockLink(SharedLinkConfigurationPointer& config)
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
    : LinkInterface                         (config)
    , _missionItemHandler                   (this, qgcApp()->toolbox()->mavlinkProtocol())
    , _name                                 ("MockLink")
    , _connected                            (false)
    , _mavlinkChannel                       (0)
    , _vehicleSystemId                      (_nextVehicleSystemId++)
    , _vehicleComponentId                   (MAV_COMP_ID_AUTOPILOT1)
    , _inNSH                                (false)
    , _mavlinkStarted                       (true)
    , _mavBaseMode                          (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
    , _mavState                             (MAV_STATE_STANDBY)
    , _firmwareType                         (MAV_AUTOPILOT_PX4)
    , _vehicleType                          (MAV_TYPE_QUADROTOR)
    , _vehicleLatitude                      (_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))     // Slight offset for each vehicle
    , _vehicleLongitude                     (_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
    , _vehicleAltitude                      (_defaultVehicleAltitude)
    , _sendStatusText                       (false)
    , _apmSendHomePositionOnEmptyList       (false)
    , _failureMode                          (MockConfiguration::FailNone)
    , _sendHomePositionDelayCount           (10)    // No home position for 4 seconds
    , _sendGPSPositionDelayCount            (100)   // No gps lock for 5 seconds
Don Gagne's avatar
Don Gagne committed
78
    , _currentParamRequestListComponentIndex(-1)
79
80
81
    , _currentParamRequestListParamIndex    (-1)
    , _logDownloadCurrentOffset             (0)
    , _logDownloadBytesRemaining            (0)
82
    , _adsbAngle                            (0)
Don Gagne's avatar
Don Gagne committed
83
{
84
85
86
87
    MockConfiguration* mockConfig = qobject_cast<MockConfiguration*>(_config.data());
    _firmwareType = mockConfig->firmwareType();
    _vehicleType = mockConfig->vehicleType();
    _sendStatusText = mockConfig->sendStatusText();
Don Gagne's avatar
Don Gagne committed
88
    _highLatency = mockConfig->highLatency();
89
    _failureMode = mockConfig->failureMode();
Don Gagne's avatar
Don Gagne committed
90

DonLakeFlyer's avatar
   
DonLakeFlyer committed
91
    QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
92

DonLakeFlyer's avatar
   
DonLakeFlyer committed
93
    union px4_custom_mode   px4_cm;
Don Gagne's avatar
Don Gagne committed
94
95
96
97
    px4_cm.data = 0;
    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
    _mavCustomMode = px4_cm.data;

DonLakeFlyer's avatar
   
DonLakeFlyer committed
98
    _mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this);
dogmaphobic's avatar
dogmaphobic committed
99

Don Gagne's avatar
Don Gagne committed
100
    moveToThread(this);
dogmaphobic's avatar
dogmaphobic committed
101

Don Gagne's avatar
Don Gagne committed
102
    _loadParams();
103
104
105

    _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(1000, _adsbAngle);
    _adsbVehicleCoordinate.setAltitude(100);
Gus Grubba's avatar
Gus Grubba committed
106
    _runningTime.start();
Don Gagne's avatar
Don Gagne committed
107
108
109
110
}

MockLink::~MockLink(void)
{
111
    _disconnect();
112
113
114
    if (!_logDownloadFilename.isEmpty()) {
        QFile::remove(_logDownloadFilename);
    }
Don Gagne's avatar
Don Gagne committed
115
116
}

117
bool MockLink::_connect(void)
Don Gagne's avatar
Don Gagne committed
118
{
119
120
    if (!_connected) {
        _connected = true;
Don Gagne's avatar
Don Gagne committed
121
122
123
124
125
126
127
128
        _mavlinkChannel = qgcApp()->toolbox()->linkManager()->_reserveMavlinkChannel();
        if (_mavlinkChannel == 0) {
            qWarning() << "No mavlink channels available";
            return false;
        }
        // MockLinks use Mavlink 2.0
        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(_mavlinkChannel);
        mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
129
130
131
        start();
        emit connected();
    }
132

Don Gagne's avatar
Don Gagne committed
133
134
135
    return true;
}

Don Gagne's avatar
Don Gagne committed
136
void MockLink::_disconnect(void)
Don Gagne's avatar
Don Gagne committed
137
{
138
    if (_connected) {
Don Gagne's avatar
Don Gagne committed
139
140
141
        if (_mavlinkChannel != 0) {
            qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel);
        }
142
        _connected = false;
Daniel Agar's avatar
Daniel Agar committed
143
144
        quit();
        wait();
145
146
        emit disconnected();
    }
Don Gagne's avatar
Don Gagne committed
147
148
149
150
}

void MockLink::run(void)
{
Don Gagne's avatar
Don Gagne committed
151
152
153
    QTimer  timer1HzTasks;
    QTimer  timer10HzTasks;
    QTimer  timer500HzTasks;
154

Don Gagne's avatar
Don Gagne committed
155
156
157
    QObject::connect(&timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::connect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks);
158

Don Gagne's avatar
Don Gagne committed
159
160
161
    timer1HzTasks.start(1000);
    timer10HzTasks.start(100);
    timer500HzTasks.start(2);
162

Don Gagne's avatar
Don Gagne committed
163
    exec();
164

Don Gagne's avatar
Don Gagne committed
165
166
167
    QObject::disconnect(&timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::disconnect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::disconnect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks);
dogmaphobic's avatar
dogmaphobic committed
168

169
    _missionItemHandler.shutdown();
Don Gagne's avatar
Don Gagne committed
170
171
172
173
}

void MockLink::_run1HzTasks(void)
{
174
    if (_mavlinkStarted && _connected) {
DonLakeFlyer's avatar
DonLakeFlyer committed
175
176
        if (_highLatency) {
            _sendHighLatency2();
177
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
178
            _sendVibration();
Gus Grubba's avatar
Gus Grubba committed
179
            _sendSysStatus();
DonLakeFlyer's avatar
DonLakeFlyer committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
            _sendADSBVehicles();
            if (!qgcApp()->runningUnitTests()) {
                // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
                _sendRCChannels();
            }
            if (_sendHomePositionDelayCount > 0) {
                // We delay home position for better testing
                _sendHomePositionDelayCount--;
            } else {
                _sendHomePosition();
            }
            if (_sendStatusText) {
                _sendStatusText = false;
                _sendStatusTextMessages();
            }
195
        }
Don Gagne's avatar
Don Gagne committed
196
197
198
199
200
    }
}

void MockLink::_run10HzTasks(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
201
202
203
204
    if (_highLatency) {
        return;
    }

205
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
206
        _sendHeartBeat();
Don Gagne's avatar
Don Gagne committed
207
208
209
210
211
212
        if (_sendGPSPositionDelayCount > 0) {
            // We delay gps position for better testing
            _sendGPSPositionDelayCount--;
        } else {
            _sendGpsRawInt();
        }
Don Gagne's avatar
Don Gagne committed
213
214
215
    }
}

Don Gagne's avatar
Don Gagne committed
216
void MockLink::_run500HzTasks(void)
Don Gagne's avatar
Don Gagne committed
217
{
DonLakeFlyer's avatar
DonLakeFlyer committed
218
219
220
221
    if (_highLatency) {
        return;
    }

222
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
223
        _paramRequestListWorker();
224
        _logDownloadWorker();
Don Gagne's avatar
Don Gagne committed
225
226
227
228
229
    }
}

void MockLink::_loadParams(void)
{
230
231
232
233
    QFile paramFile;

    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        if (_vehicleType == MAV_TYPE_FIXED_WING) {
DonLakeFlyer's avatar
   
DonLakeFlyer committed
234
            paramFile.setFileName(":/FirmwarePlugin/APM/Plane.OfflineEditing.params");
Rustom Jehangir's avatar
Rustom Jehangir committed
235
        } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
236
            paramFile.setFileName(":/MockLink/APMArduSubMockLink.params");
DonLakeFlyer's avatar
   
DonLakeFlyer committed
237
238
        } else if (_vehicleType == MAV_TYPE_GROUND_ROVER ) {
            paramFile.setFileName(":/FirmwarePlugin/APM/Rover.OfflineEditing.params");
239
        } else {
DonLakeFlyer's avatar
   
DonLakeFlyer committed
240
            paramFile.setFileName(":/FirmwarePlugin/APM/Copter.OfflineEditing.params");
241
242
        }
    } else {
243
        paramFile.setFileName(":/MockLink/PX4MockLink.params");
244
245
    }

246

Don Gagne's avatar
Don Gagne committed
247
248
249
    bool success = paramFile.open(QFile::ReadOnly);
    Q_UNUSED(success);
    Q_ASSERT(success);
250

Don Gagne's avatar
Don Gagne committed
251
    QTextStream paramStream(&paramFile);
252

Don Gagne's avatar
Don Gagne committed
253
254
    while (!paramStream.atEnd()) {
        QString line = paramStream.readLine();
255

Don Gagne's avatar
Don Gagne committed
256
257
258
        if (line.startsWith("#")) {
            continue;
        }
259

Don Gagne's avatar
Don Gagne committed
260
261
        QStringList paramData = line.split("\t");
        Q_ASSERT(paramData.count() == 5);
262

Don Gagne's avatar
   
Don Gagne committed
263
        int compId = paramData.at(1).toInt();
Don Gagne's avatar
Don Gagne committed
264
265
266
        QString paramName = paramData.at(2);
        QString valStr = paramData.at(3);
        uint paramType = paramData.at(4).toUInt();
267

Don Gagne's avatar
Don Gagne committed
268
269
        QVariant paramValue;
        switch (paramType) {
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
        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;
        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;
        case MAV_PARAM_TYPE_INT8:
            paramValue = QVariant((qint8)valStr.toUInt());
            break;
        default:
            qCritical() << "Unknown type" << paramType;
            paramValue = QVariant(valStr.toInt());
            break;
Don Gagne's avatar
Don Gagne committed
295
        }
296

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

Don Gagne's avatar
   
Don Gagne committed
299
300
        _mapParamName2Value[compId][paramName] = paramValue;
        _mapParamName2MavParamType[compId][paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
Don Gagne's avatar
Don Gagne committed
301
302
303
304
305
306
307
    }
}

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

Don Gagne's avatar
Don Gagne committed
308
309
    mavlink_msg_heartbeat_pack_chan(_vehicleSystemId,
                                    _vehicleComponentId,
Don Gagne's avatar
Don Gagne committed
310
                                    _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
311
312
313
314
315
316
                                    &msg,
                                    _vehicleType,        // MAV_TYPE
                                    _firmwareType,      // MAV_AUTOPILOT
                                    _mavBaseMode,        // MAV_MODE
                                    _mavCustomMode,      // custom mode
                                    _mavState);          // MAV_STATE
dogmaphobic's avatar
dogmaphobic committed
317

Don Gagne's avatar
Don Gagne committed
318
319
    respondWithMavlinkMessage(msg);
}
Don Gagne's avatar
Don Gagne committed
320

DonLakeFlyer's avatar
DonLakeFlyer committed
321
322
323
324
void MockLink::_sendHighLatency2(void)
{
    mavlink_message_t   msg;

325
326
327
328
    union px4_custom_mode   px4_cm;
    px4_cm.data = _mavCustomMode;

    qDebug() << "Sending" << _mavCustomMode;
DonLakeFlyer's avatar
DonLakeFlyer committed
329
330
331
332
333
334
335
    mavlink_msg_high_latency2_pack_chan(_vehicleSystemId,
                                        _vehicleComponentId,
                                        _mavlinkChannel,
                                        &msg,
                                        0,                          // timestamp
                                        _vehicleType,               // MAV_TYPE
                                        _firmwareType,              // MAV_AUTOPILOT
336
                                        px4_cm.custom_mode_hl,      // custom_mode
DonLakeFlyer's avatar
DonLakeFlyer committed
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
                                        (int32_t)(_vehicleLatitude  * 1E7),
                                        (int32_t)(_vehicleLongitude * 1E7),
                                        (int16_t)_vehicleAltitude,
                                        (int16_t)_vehicleAltitude,  // target_altitude,
                                        0,                          // heading
                                        0,                          // target_heading
                                        0,                          // target_distance
                                        0,                          // throttle
                                        0,                          // airspeed
                                        0,                          // airspeed_sp
                                        0,                          // groundspeed
                                        0,                          // windspeed,
                                        0,                          // wind_heading
                                        UINT8_MAX,                  // eph not known
                                        UINT8_MAX,                  // epv not known
                                        0,                          // temperature_air
                                        0,                          // climb_rate
                                        -1,                         // battery, do not use?
                                        0,                          // wp_num
                                        0,                          // failure_flags
                                        0, 0, 0);                   // custom0, custom1, custom2
    respondWithMavlinkMessage(msg);
}

Gus Grubba's avatar
Gus Grubba committed
361
362
363
364
365
366
367
void MockLink::_sendSysStatus(void)
{
    if(_batteryRemaining > 50) {
        _batteryRemaining = static_cast<int8_t>(100 - (_runningTime.elapsed() / 1000));
    }
    mavlink_message_t   msg;
    mavlink_msg_sys_status_pack_chan(
DonLakeFlyer's avatar
   
DonLakeFlyer committed
368
369
370
371
372
373
374
375
376
377
378
379
                _vehicleSystemId,
                _vehicleComponentId,
                static_cast<uint8_t>(_mavlinkChannel),
                &msg,
                0,          // onboard_control_sensors_present
                0,          // onboard_control_sensors_enabled
                0,          // onboard_control_sensors_health
                250,        // load
                4200 * 4,   // voltage_battery
                8000,       // current_battery
                _batteryRemaining, // battery_remaining
                0,0,0,0,0,0);
Gus Grubba's avatar
Gus Grubba committed
380
381
382
    respondWithMavlinkMessage(msg);
}

Don Gagne's avatar
Don Gagne committed
383
384
385
386
void MockLink::_sendVibration(void)
{
    mavlink_message_t   msg;

Don Gagne's avatar
Don Gagne committed
387
388
    mavlink_msg_vibration_pack_chan(_vehicleSystemId,
                                    _vehicleComponentId,
Don Gagne's avatar
Don Gagne committed
389
                                    _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
390
391
392
393
394
395
396
397
                                    &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
Don Gagne's avatar
Don Gagne committed
398
399
400
401

    respondWithMavlinkMessage(msg);
}

Don Gagne's avatar
Don Gagne committed
402
403
404
void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg)
{
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
dogmaphobic's avatar
dogmaphobic committed
405

Don Gagne's avatar
Don Gagne committed
406
407
408
409
410
411
    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
412
void MockLink::_writeBytes(const QByteArray bytes)
DonLakeFlyer's avatar
   
DonLakeFlyer committed
413
414
415
416
417
418
{
    // This prevents the responses to mavlink messages from being sent until the _writeBytes returns.
    emit writeBytesQueuedSignal(bytes);
}

void MockLink::_writeBytesQueued(const QByteArray bytes)
Don Gagne's avatar
Don Gagne committed
419
420
421
422
423
424
425
426
{
    if (_inNSH) {
        _handleIncomingNSHBytes(bytes.constData(), bytes.count());
    } else {
        if (bytes.startsWith(QByteArray("\r\r\r"))) {
            _inNSH  = true;
            _handleIncomingNSHBytes(&bytes.constData()[3], bytes.count() - 3);
        }
427

Don Gagne's avatar
Don Gagne committed
428
429
430
431
432
433
434
435
        _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);
436

Don Gagne's avatar
Don Gagne committed
437
438
439
440
441
442
443
444
    // 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;
445

Don Gagne's avatar
Don Gagne committed
446
447
#if 0
        // MockLink not quite ready to handle this correctly yet
Don Gagne's avatar
Don Gagne committed
448
449
450
451
        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
452
#endif
Don Gagne's avatar
Don Gagne committed
453
454
455
456
457
458
459
460
    }
}

/// @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;
461

Don Gagne's avatar
Don Gagne committed
462
463
    for (qint64 i=0; i<cBytes; i++)
    {
Don Gagne's avatar
Don Gagne committed
464
        if (!mavlink_parse_char(_mavlinkChannel, bytes[i], &msg, &comm)) {
Don Gagne's avatar
Don Gagne committed
465
466
            continue;
        }
dogmaphobic's avatar
dogmaphobic committed
467

468
        if (_missionItemHandler.handleMessage(msg)) {
Don Gagne's avatar
Don Gagne committed
469
470
            continue;
        }
471

Don Gagne's avatar
Don Gagne committed
472
        switch (msg.msgid) {
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
        case MAVLINK_MSG_ID_HEARTBEAT:
            _handleHeartBeat(msg);
            break;
        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
            _handleParamRequestList(msg);
            break;
        case MAVLINK_MSG_ID_SET_MODE:
            _handleSetMode(msg);
            break;
        case MAVLINK_MSG_ID_PARAM_SET:
            _handleParamSet(msg);
            break;
        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
            _handleParamRequestRead(msg);
            break;
        case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
            _handleFTP(msg);
            break;
        case MAVLINK_MSG_ID_COMMAND_LONG:
            _handleCommandLong(msg);
            break;
        case MAVLINK_MSG_ID_MANUAL_CONTROL:
            _handleManualControl(msg);
            break;
497
498
499
500
501
502
        case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
            _handleLogRequestList(msg);
            break;
        case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
            _handleLogRequestData(msg);
            break;
DoinLakeFlyer's avatar
   
DoinLakeFlyer committed
503
504
505
        case MAVLINK_MSG_ID_PARAM_MAP_RC:
            _handleParamMapRC(msg);
            break;
506
507
        default:
            break;
Don Gagne's avatar
Don Gagne committed
508
509
510
511
512
513
514
        }
    }
}

void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
{
    Q_UNUSED(msg);
515
    qCDebug(MockLinkLog) << "Heartbeat";
Don Gagne's avatar
Don Gagne committed
516
517
}

DoinLakeFlyer's avatar
   
DoinLakeFlyer committed
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
void MockLink::_handleParamMapRC(const mavlink_message_t& msg)
{
    mavlink_param_map_rc_t paramMapRC;
    mavlink_msg_param_map_rc_decode(&msg, &paramMapRC);

    const QString paramName(QString::fromLocal8Bit(paramMapRC.param_id, static_cast<int>(strnlen(paramMapRC.param_id, MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN))));

    if (paramMapRC.param_index == -1) {
        qDebug() << QStringLiteral("MockLink - PARAM_MAP_RC: param(%1) tuningID(%2) centerValue(%3) scale(%4) min(%5) max(%6)").arg(paramName).arg(paramMapRC.parameter_rc_channel_index).arg(paramMapRC.param_value0).arg(paramMapRC.scale).arg(paramMapRC.param_value_min).arg(paramMapRC.param_value_max);
    } else if (paramMapRC.param_index == -2) {
        qDebug() << QStringLiteral("MockLink - PARAM_MAP_RC: Clear tuningID(%1)").arg(paramMapRC.parameter_rc_channel_index);
    } else {
        qWarning() << QStringLiteral("MockLink - PARAM_MAP_RC: Unsupported param_index(%1)").arg(paramMapRC.param_index);
    }
}

Don Gagne's avatar
Don Gagne committed
534
535
536
537
void MockLink::_handleSetMode(const mavlink_message_t& msg)
{
    mavlink_set_mode_t request;
    mavlink_msg_set_mode_decode(&msg, &request);
538

539
    Q_ASSERT(request.target_system == _vehicleSystemId);
540

541
542
543
544
    _mavBaseMode = request.base_mode;
    _mavCustomMode = request.custom_mode;
}

Don Gagne's avatar
Don Gagne committed
545
546
547
548
549
void MockLink::_handleManualControl(const mavlink_message_t& msg)
{
    mavlink_manual_control_t manualControl;
    mavlink_msg_manual_control_decode(&msg, &manualControl);

Gus Grubba's avatar
Gus Grubba committed
550
    qCDebug(MockLinkLog) << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r;
Don Gagne's avatar
Don Gagne committed
551
552
}

553
void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
554
555
{
    mavlink_param_union_t   valueUnion;
556

557
558
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Don Gagne's avatar
   
Don Gagne committed
559
    Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
560

561
    valueUnion.param_float = paramFloat;
562

Don Gagne's avatar
   
Don Gagne committed
563
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
564

565
    QVariant paramVariant;
566

567
    switch (paramType) {
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
    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
600
    }
601

602
    qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
603
    _mapParamName2Value[componentId][paramName] = paramVariant;
Don Gagne's avatar
Don Gagne committed
604
605
}

606
/// Convert from a parameter variant to the float value from mavlink_param_union_t
607
float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
Don Gagne's avatar
Don Gagne committed
608
{
609
    mavlink_param_union_t   valueUnion;
610

611
612
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Don Gagne's avatar
   
Don Gagne committed
613
    Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
614

Don Gagne's avatar
   
Don Gagne committed
615
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
616
    QVariant paramVar = _mapParamName2Value[componentId][paramName];
617

618
    switch (paramType) {
619
    case MAV_PARAM_TYPE_REAL32:
620
        valueUnion.param_float = paramVar.toFloat();
621
        break;
622

623
624
625
626
627
628
629
    case MAV_PARAM_TYPE_UINT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint32 = paramVar.toUInt();
        }
        break;
630

631
632
633
634
635
636
637
    case MAV_PARAM_TYPE_INT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toInt();
        } else {
            valueUnion.param_int32 = paramVar.toInt();
        }
        break;
638

639
640
641
642
643
644
645
    case MAV_PARAM_TYPE_UINT16:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint16 = paramVar.toUInt();
        }
        break;
646

647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
    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;
678
    }
679

680
    return valueUnion.param_float;
Don Gagne's avatar
Don Gagne committed
681
682
683
684
}

void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
Don Gagne's avatar
Don Gagne committed
685
686
687
688
    if (_failureMode == MockConfiguration::FailParamNoReponseToRequestList) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
689
    mavlink_param_request_list_t request;
690

Don Gagne's avatar
Don Gagne committed
691
    mavlink_msg_param_request_list_decode(&msg, &request);
692

693
    Q_ASSERT(request.target_system == _vehicleSystemId);
694
    Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
dogmaphobic's avatar
dogmaphobic committed
695

Don Gagne's avatar
Don Gagne committed
696
697
698
699
    // Start the worker routine
    _currentParamRequestListComponentIndex = 0;
    _currentParamRequestListParamIndex = 0;
}
700

Don Gagne's avatar
Don Gagne committed
701
702
703
704
705
706
707
/// Sends the next parameter to the vehicle
void MockLink::_paramRequestListWorker(void)
{
    if (_currentParamRequestListComponentIndex == -1) {
        // Initial request complete
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
708

Don Gagne's avatar
Don Gagne committed
709
710
711
    int componentId = _mapParamName2Value.keys()[_currentParamRequestListComponentIndex];
    int cParameters = _mapParamName2Value[componentId].count();
    QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex];
712

Don Gagne's avatar
Don Gagne committed
713
714
715
    if ((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest || _failureMode == MockConfiguration::FailMissingParamOnAllRequests) && paramName == _failParam) {
        qCDebug(MockLinkLog) << "Skipping param send:" << paramName;
    } else {
716

Don Gagne's avatar
Don Gagne committed
717
718
        char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
        mavlink_message_t responseMsg;
719

Don Gagne's avatar
Don Gagne committed
720
        Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
Don Gagne's avatar
   
Don Gagne committed
721
        Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
722

Don Gagne's avatar
   
Don Gagne committed
723
        MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
724

Don Gagne's avatar
Don Gagne committed
725
726
        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
727

Don Gagne's avatar
Don Gagne committed
728
729
        qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];

Don Gagne's avatar
Don Gagne committed
730
731
        mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                          componentId,                                   // component id
Don Gagne's avatar
Don Gagne committed
732
                                          _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
733
734
735
736
737
738
                                          &responseMsg,                                  // Outgoing message
                                          paramId,                                       // Parameter name
                                          _floatUnionForParam(componentId, paramName),   // Parameter value
                                          paramType,                                     // MAV_PARAM_TYPE
                                          cParameters,                                   // Total number of parameters
                                          _currentParamRequestListParamIndex);           // Index of this parameter
Don Gagne's avatar
Don Gagne committed
739
        respondWithMavlinkMessage(responseMsg);
740
    }
dogmaphobic's avatar
dogmaphobic committed
741

Don Gagne's avatar
Don Gagne committed
742
743
744
745
746
747
748
749
    // Move to next param index
    if (++_currentParamRequestListParamIndex >= cParameters) {
        // We've sent the last parameter for this component, move to next component
        if (++_currentParamRequestListComponentIndex >= _mapParamName2Value.keys().count()) {
            // We've finished sending the last parameter for the last component, request is complete
            _currentParamRequestListComponentIndex = -1;
        } else {
            _currentParamRequestListParamIndex = 0;
750
        }
Don Gagne's avatar
Don Gagne committed
751
752
753
754
755
756
757
    }
}

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

759
    Q_ASSERT(request.target_system == _vehicleSystemId);
760
    int componentId = request.target_component;
dogmaphobic's avatar
dogmaphobic committed
761

762
763
    // 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
764
    paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
765
    strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
766

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

769
    Q_ASSERT(_mapParamName2Value.contains(componentId));
Don Gagne's avatar
   
Don Gagne committed
770
    Q_ASSERT(_mapParamName2MavParamType.contains(componentId));
771
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
Don Gagne's avatar
   
Don Gagne committed
772
    Q_ASSERT(request.param_type == _mapParamName2MavParamType[componentId][paramId]);
773

774
    // Save the new value
775
    _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
776

777
778
    // Respond with a param_value to ack
    mavlink_message_t responseMsg;
Don Gagne's avatar
Don Gagne committed
779
780
    mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                      componentId,                                               // component id
Don Gagne's avatar
Don Gagne committed
781
                                      _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
782
783
784
785
786
787
                                      &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
788
    respondWithMavlinkMessage(responseMsg);
Don Gagne's avatar
Don Gagne committed
789
790
791
792
}

void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
{
793
    mavlink_message_t   responseMsg;
Don Gagne's avatar
Don Gagne committed
794
795
    mavlink_param_request_read_t request;
    mavlink_msg_param_request_read_decode(&msg, &request);
796

DonLakeFlyer's avatar
   
DonLakeFlyer committed
797
    const QString paramName(QString::fromLocal8Bit(request.param_id, static_cast<int>(strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))));
798
    int componentId = request.target_component;
799
800

    // special case for magic _HASH_CHECK value
Don Gagne's avatar
Don Gagne committed
801
    if (request.target_component == MAV_COMP_ID_ALL && paramName == "_HASH_CHECK") {
802
803
804
805
        mavlink_param_union_t   valueUnion;
        valueUnion.type = MAV_PARAM_TYPE_UINT32;
        valueUnion.param_uint32 = 0;
        // Special case of magic hash check value
Don Gagne's avatar
Don Gagne committed
806
807
        mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                          componentId,
Don Gagne's avatar
Don Gagne committed
808
                                          _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
809
810
811
812
813
814
                                          &responseMsg,
                                          request.param_id,
                                          valueUnion.param_float,
                                          MAV_PARAM_TYPE_UINT32,
                                          0,
                                          -1);
815
816
817
818
        respondWithMavlinkMessage(responseMsg);
        return;
    }

819
    Q_ASSERT(_mapParamName2Value.contains(componentId));
820

Don Gagne's avatar
Don Gagne committed
821
822
    char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
    paramId[0] = 0;
823

824
    Q_ASSERT(request.target_system == _vehicleSystemId);
825

826
827
828
    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
829
    } else {
830
        // Request is by index
831

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

834
        QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
835
836
        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
837
    }
838

839
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
Don Gagne's avatar
   
Don Gagne committed
840
    Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramId));
841

Don Gagne's avatar
Don Gagne committed
842
843
844
845
846
847
    if (_failureMode == MockConfiguration::FailMissingParamOnAllRequests && strcmp(paramId, _failParam) == 0) {
        qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam;
        // Fail to send this param no matter what
        return;
    }

Don Gagne's avatar
Don Gagne committed
848
849
    mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                      componentId,                                               // component id
Don Gagne's avatar
Don Gagne committed
850
                                      _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
851
852
853
                                      &responseMsg,                                              // Outgoing message
                                      paramId,                                                   // Parameter name
                                      _floatUnionForParam(componentId, paramId),                 // Parameter value
Don Gagne's avatar
   
Don Gagne committed
854
                                      _mapParamName2MavParamType[componentId][paramId],          // Parameter type
Don Gagne's avatar
Don Gagne committed
855
856
                                      _mapParamName2Value[componentId].count(),                  // Total number of parameters
                                      _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
Don Gagne's avatar
Don Gagne committed
857
    respondWithMavlinkMessage(responseMsg);
Don Gagne's avatar
Don Gagne committed
858
859
}

Don Gagne's avatar
Don Gagne committed
860
861
862
void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
{
    uint16_t chanRaw[18];
dogmaphobic's avatar
dogmaphobic committed
863

Don Gagne's avatar
Don Gagne committed
864
865
866
867
    for (int i=0; i<18; i++) {
        chanRaw[i] = UINT16_MAX;
    }
    chanRaw[channel] = raw;
dogmaphobic's avatar
dogmaphobic committed
868

Don Gagne's avatar
Don Gagne committed
869
    mavlink_message_t responseMsg;
Don Gagne's avatar
Don Gagne committed
870
871
    mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
Don Gagne's avatar
Don Gagne committed
872
                                      _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
873
874
875
876
                                      &responseMsg,          // Outgoing message
                                      0,                     // time since boot, ignored
                                      18,                    // channel count
                                      chanRaw[0],            // channel raw value
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
            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
895
896
897
898
899
    respondWithMavlinkMessage(responseMsg);
}

void MockLink::_handleFTP(const mavlink_message_t& msg)
{
DonLakeFlyer's avatar
   
DonLakeFlyer committed
900
    _mockLinkFTP->mavlinkMessageReceived(msg);
Don Gagne's avatar
Don Gagne committed
901
}
902
903
904

void MockLink::_handleCommandLong(const mavlink_message_t& msg)
{
Don Gagne's avatar
Don Gagne committed
905
906
907
    static bool firstCmdUser3 = true;
    static bool firstCmdUser4 = true;

DonLakeFlyer's avatar
   
DonLakeFlyer committed
908
    bool noAck = false;
909
    mavlink_command_long_t request;
910
    uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
dogmaphobic's avatar
dogmaphobic committed
911

912
913
    mavlink_msg_command_long_decode(&msg, &request);

914
915
    switch (request.command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
916
917
918
919
920
        if (request.param1 == 0.0f) {
            _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
        } else {
            _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
        }
921
        commandResult = MAV_RESULT_ACCEPTED;
922
923
        break;
    case MAV_CMD_PREFLIGHT_CALIBRATION:
924
925
926
        _handlePreFlightCalibration(request);
        commandResult = MAV_RESULT_ACCEPTED;
        break;
927
928
929
    case MAV_CMD_PREFLIGHT_STORAGE:
        commandResult = MAV_RESULT_ACCEPTED;
        break;
930
931
932
933
    case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
        commandResult = MAV_RESULT_ACCEPTED;
        _respondWithAutopilotVersion();
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
934
    case MAV_CMD_REQUEST_MESSAGE:
DonLakeFlyer's avatar
   
DonLakeFlyer committed
935
936
937
938
        if (_handleRequestMessage(request, noAck)) {
            if (noAck) {
                return;
            }
DonLakeFlyer's avatar
   
DonLakeFlyer committed
939
940
941
            commandResult = MAV_RESULT_ACCEPTED;
        }
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
942
    case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
Don Gagne's avatar
Don Gagne committed
943
944
945
        // Test command which always returns MAV_RESULT_ACCEPTED
        commandResult = MAV_RESULT_ACCEPTED;
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
946
    case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
Don Gagne's avatar
Don Gagne committed
947
948
949
        // Test command which always returns MAV_RESULT_FAILED
        commandResult = MAV_RESULT_FAILED;
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
950
    case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
Don Gagne's avatar
Don Gagne committed
951
952
        // Test command which returns MAV_RESULT_ACCEPTED on second attempt
        if (firstCmdUser3) {
DonLakeFlyer's avatar
DonLakeFlyer committed
953
954
            firstCmdUser3 = false;
            return;
Don Gagne's avatar
Don Gagne committed
955
956
957
958
959
        } else {
            firstCmdUser3 = true;
            commandResult = MAV_RESULT_ACCEPTED;
        }
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
960
    case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
Don Gagne's avatar
Don Gagne committed
961
962
        // Test command which returns MAV_RESULT_FAILED on second attempt
        if (firstCmdUser4) {
DonLakeFlyer's avatar
DonLakeFlyer committed
963
964
            firstCmdUser4 = false;
            return;
Don Gagne's avatar
Don Gagne committed
965
966
967
968
969
        } else {
            firstCmdUser4 = true;
            commandResult = MAV_RESULT_FAILED;
        }
        break;
DonLakeFlyer's avatar
   
DonLakeFlyer committed
970
    case MAV_CMD_MOCKLINK_NO_RESPONSE:
Don Gagne's avatar
Don Gagne committed
971
972
973
        // No response
        return;
        break;
974
    }
975
976

    mavlink_message_t commandAck;
Don Gagne's avatar
Don Gagne committed
977
978
    mavlink_msg_command_ack_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
Don Gagne's avatar
Don Gagne committed
979
                                      _mavlinkChannel,
Don Gagne's avatar
Don Gagne committed
980
981
                                      &commandAck,
                                      request.command,
DonLakeFlyer's avatar
DonLakeFlyer committed
982
                                      commandResult,
Gus Grubba's avatar
Gus Grubba committed
983
984
985
986
                                      0,    // progress
                                      0,    // result_param2
                                      0,    // target_system
                                      0);   // target_component
987
    respondWithMavlinkMessage(commandAck);
988
989
}

DonLakeFlyer's avatar
   
DonLakeFlyer committed
990
991
992
993
994
995
996
997
998
999
1000
void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult)
{
    mavlink_message_t commandAck;
    mavlink_msg_command_ack_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
                                      _mavlinkChannel,
                                      &commandAck,
                                      command,
                                      ackResult,
                                      0,    // progress
                                      0,    // result_param2
For faster browsing, not all history is shown. View entire blame