MockLink.cc 47.9 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

10 11

#include "MockLink.h"
12
#include "QGCLoggingCategory.h"
13
#include "QGCApplication.h"
14 15 16

#ifdef UNITTEST_BUILD
    #include "UnitTest.h"
17
#endif
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

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

36 37 38 39 40 41 42
// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
// testing of a gazebo vehicle and a mocklink vehicle
double      MockLink::_defaultVehicleLatitude =     47.397f;
double      MockLink::_defaultVehicleLongitude =    8.5455f;
double      MockLink::_defaultVehicleAltitude =     488.056f;
int         MockLink::_nextVehicleSystemId =        128;
const char* MockLink::_failParam =                  "COM_FLTMODE6";
Don Gagne's avatar
Don Gagne committed
43

44
const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
45
const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";
46
const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";
Don Gagne's avatar
Don Gagne committed
47
const char* MockConfiguration::_failureModeKey =    "FailureMode";
48

49
MockLink::MockLink(SharedLinkConfigurationPointer& config)
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71
    : 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)
    , _fileServer                           (NULL)
    , _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
72
    , _currentParamRequestListComponentIndex(-1)
73 74 75
    , _currentParamRequestListParamIndex    (-1)
    , _logDownloadCurrentOffset             (0)
    , _logDownloadBytesRemaining            (0)
76
{
77 78 79 80 81
    MockConfiguration* mockConfig = qobject_cast<MockConfiguration*>(_config.data());
    _firmwareType = mockConfig->firmwareType();
    _vehicleType = mockConfig->vehicleType();
    _sendStatusText = mockConfig->sendStatusText();
    _failureMode = mockConfig->failureMode();
82

Don Gagne's avatar
Don Gagne committed
83 84 85 86 87 88
    union px4_custom_mode   px4_cm;

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

89 90
    _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
    Q_CHECK_PTR(_fileServer);
dogmaphobic's avatar
dogmaphobic committed
91

92
    moveToThread(this);
dogmaphobic's avatar
dogmaphobic committed
93

94 95 96 97 98
    _loadParams();
}

MockLink::~MockLink(void)
{
99
    _disconnect();
100 101 102
    if (!_logDownloadFilename.isEmpty()) {
        QFile::remove(_logDownloadFilename);
    }
103 104
}

105
bool MockLink::_connect(void)
106
{
107 108
    if (!_connected) {
        _connected = true;
109 110 111 112 113 114 115 116
        _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;
117 118 119
        start();
        emit connected();
    }
120

121 122 123
    return true;
}

Don Gagne's avatar
Don Gagne committed
124
void MockLink::_disconnect(void)
125
{
126
    if (_connected) {
127 128 129
        if (_mavlinkChannel != 0) {
            qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel);
        }
130
        _connected = false;
Daniel Agar's avatar
Daniel Agar committed
131 132
        quit();
        wait();
133 134
        emit disconnected();
    }
135 136 137 138
}

void MockLink::run(void)
{
Don Gagne's avatar
Don Gagne committed
139 140 141
    QTimer  timer1HzTasks;
    QTimer  timer10HzTasks;
    QTimer  timer500HzTasks;
142

Don Gagne's avatar
Don Gagne committed
143 144 145
    QObject::connect(&timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::connect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks);
146

Don Gagne's avatar
Don Gagne committed
147 148 149
    timer1HzTasks.start(1000);
    timer10HzTasks.start(100);
    timer500HzTasks.start(2);
150

151
    exec();
152

Don Gagne's avatar
Don Gagne committed
153 154 155
    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
156

157
    _missionItemHandler.shutdown();
158 159 160 161
}

void MockLink::_run1HzTasks(void)
{
162
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
163
        _sendVibration();
164
        _sendADSBVehicles();
165 166 167 168
        if (!qgcApp()->runningUnitTests()) {
            // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
            _sendRCChannels();
        }
169
        if (_sendHomePositionDelayCount > 0) {
170
            // We delay home position for better testing
171 172 173 174
            _sendHomePositionDelayCount--;
        } else {
            _sendHomePosition();
        }
175 176 177 178
        if (_sendStatusText) {
            _sendStatusText = false;
            _sendStatusTextMessages();
        }
179 180 181 182 183
    }
}

void MockLink::_run10HzTasks(void)
{
184
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
185
        _sendHeartBeat();
186 187 188 189 190 191
        if (_sendGPSPositionDelayCount > 0) {
            // We delay gps position for better testing
            _sendGPSPositionDelayCount--;
        } else {
            _sendGpsRawInt();
        }
192 193 194
    }
}

Don Gagne's avatar
Don Gagne committed
195
void MockLink::_run500HzTasks(void)
196
{
197
    if (_mavlinkStarted && _connected) {
Don Gagne's avatar
Don Gagne committed
198
        _paramRequestListWorker();
199
        _logDownloadWorker();
200 201 202 203 204
    }
}

void MockLink::_loadParams(void)
{
205 206 207 208
    QFile paramFile;

    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        if (_vehicleType == MAV_TYPE_FIXED_WING) {
209
            paramFile.setFileName(":/MockLink/APMArduPlaneMockLink.params");
210
        } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
211
            paramFile.setFileName(":/MockLink/APMArduSubMockLink.params");
212
        } else {
213
            paramFile.setFileName(":/MockLink/APMArduCopterMockLink.params");
214 215
        }
    } else {
216
        paramFile.setFileName(":/MockLink/PX4MockLink.params");
217 218
    }

219

220 221 222
    bool success = paramFile.open(QFile::ReadOnly);
    Q_UNUSED(success);
    Q_ASSERT(success);
223

224
    QTextStream paramStream(&paramFile);
225

226 227
    while (!paramStream.atEnd()) {
        QString line = paramStream.readLine();
228

229 230 231
        if (line.startsWith("#")) {
            continue;
        }
232

233 234
        QStringList paramData = line.split("\t");
        Q_ASSERT(paramData.count() == 5);
235

236 237 238
        QString paramName = paramData.at(2);
        QString valStr = paramData.at(3);
        uint paramType = paramData.at(4).toUInt();
239

240 241
        QVariant paramValue;
        switch (paramType) {
242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266
        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;
267
        }
268

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

271
        _mapParamName2Value[_vehicleComponentId][paramName] = paramValue;
272
        _mapParamName2MavParamType[paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
273 274 275 276 277 278 279
    }
}

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

280 281
    mavlink_msg_heartbeat_pack_chan(_vehicleSystemId,
                                    _vehicleComponentId,
282
                                    _mavlinkChannel,
283 284 285 286 287 288
                                    &msg,
                                    _vehicleType,        // MAV_TYPE
                                    _firmwareType,      // MAV_AUTOPILOT
                                    _mavBaseMode,        // MAV_MODE
                                    _mavCustomMode,      // custom mode
                                    _mavState);          // MAV_STATE
dogmaphobic's avatar
dogmaphobic committed
289

290 291
    respondWithMavlinkMessage(msg);
}
292

Don Gagne's avatar
Don Gagne committed
293 294 295 296
void MockLink::_sendVibration(void)
{
    mavlink_message_t   msg;

297 298
    mavlink_msg_vibration_pack_chan(_vehicleSystemId,
                                    _vehicleComponentId,
299
                                    _mavlinkChannel,
300 301 302 303 304 305 306 307
                                    &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
308 309 310 311

    respondWithMavlinkMessage(msg);
}

312 313 314
void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg)
{
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
dogmaphobic's avatar
dogmaphobic committed
315

316 317 318 319 320 321
    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
322
void MockLink::_writeBytes(const QByteArray bytes)
323 324 325 326 327 328 329 330
{
    if (_inNSH) {
        _handleIncomingNSHBytes(bytes.constData(), bytes.count());
    } else {
        if (bytes.startsWith(QByteArray("\r\r\r"))) {
            _inNSH  = true;
            _handleIncomingNSHBytes(&bytes.constData()[3], bytes.count() - 3);
        }
331

332 333 334 335 336 337 338 339
        _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);
340

341 342 343 344 345 346 347 348
    // 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;
349

Don Gagne's avatar
Don Gagne committed
350 351
#if 0
        // MockLink not quite ready to handle this correctly yet
352 353 354 355
        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
356
#endif
357 358 359 360 361 362 363 364
    }
}

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

366 367
    for (qint64 i=0; i<cBytes; i++)
    {
368
        if (!mavlink_parse_char(_mavlinkChannel, bytes[i], &msg, &comm)) {
369 370
            continue;
        }
dogmaphobic's avatar
dogmaphobic committed
371

372
        if (_missionItemHandler.handleMessage(msg)) {
Don Gagne's avatar
Don Gagne committed
373 374
            continue;
        }
375

376
        switch (msg.msgid) {
377 378 379
        case MAVLINK_MSG_ID_HEARTBEAT:
            _handleHeartBeat(msg);
            break;
380

381 382 383
        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
            _handleParamRequestList(msg);
            break;
384

385 386 387
        case MAVLINK_MSG_ID_SET_MODE:
            _handleSetMode(msg);
            break;
388

389 390 391
        case MAVLINK_MSG_ID_PARAM_SET:
            _handleParamSet(msg);
            break;
392

393 394 395
        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
            _handleParamRequestRead(msg);
            break;
dogmaphobic's avatar
dogmaphobic committed
396

397 398 399
        case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
            _handleFTP(msg);
            break;
dogmaphobic's avatar
dogmaphobic committed
400

401 402 403
        case MAVLINK_MSG_ID_COMMAND_LONG:
            _handleCommandLong(msg);
            break;
404

405 406 407
        case MAVLINK_MSG_ID_MANUAL_CONTROL:
            _handleManualControl(msg);
            break;
Don Gagne's avatar
Don Gagne committed
408

409 410 411 412 413 414 415 416
        case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
            _handleLogRequestList(msg);
            break;

        case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
            _handleLogRequestData(msg);
            break;

417 418
        default:
            break;
419 420 421 422 423 424 425
        }
    }
}

void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
{
    Q_UNUSED(msg);
426
    qCDebug(MockLinkLog) << "Heartbeat";
427 428 429 430 431 432
}

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

434
    Q_ASSERT(request.target_system == _vehicleSystemId);
435

436 437 438 439
    _mavBaseMode = request.base_mode;
    _mavCustomMode = request.custom_mode;
}

Don Gagne's avatar
Don Gagne committed
440 441 442 443 444 445 446 447
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;
}

448
void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
449 450
{
    mavlink_param_union_t   valueUnion;
451

452 453
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
454
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
455

456
    valueUnion.param_float = paramFloat;
457

458
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
459

460
    QVariant paramVariant;
461

462
    switch (paramType) {
463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494
    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;
495
    }
496

497
    qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
498
    _mapParamName2Value[componentId][paramName] = paramVariant;
499 500
}

501
/// Convert from a parameter variant to the float value from mavlink_param_union_t
502
float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
503
{
504
    mavlink_param_union_t   valueUnion;
505

506 507
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
508
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
509

510
    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
511
    QVariant paramVar = _mapParamName2Value[componentId][paramName];
512

513
    switch (paramType) {
514
    case MAV_PARAM_TYPE_REAL32:
515
        valueUnion.param_float = paramVar.toFloat();
516
        break;
517

518 519 520 521 522 523 524
    case MAV_PARAM_TYPE_UINT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint32 = paramVar.toUInt();
        }
        break;
525

526 527 528 529 530 531 532
    case MAV_PARAM_TYPE_INT32:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toInt();
        } else {
            valueUnion.param_int32 = paramVar.toInt();
        }
        break;
533

534 535 536 537 538 539 540
    case MAV_PARAM_TYPE_UINT16:
        if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            valueUnion.param_float = paramVar.toUInt();
        } else {
            valueUnion.param_uint16 = paramVar.toUInt();
        }
        break;
541

542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572
    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;
573
    }
574

575
    return valueUnion.param_float;
576 577 578 579
}

void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
Don Gagne's avatar
Don Gagne committed
580 581 582 583
    if (_failureMode == MockConfiguration::FailParamNoReponseToRequestList) {
        return;
    }

584
    mavlink_param_request_list_t request;
585

586
    mavlink_msg_param_request_list_decode(&msg, &request);
587

588
    Q_ASSERT(request.target_system == _vehicleSystemId);
589
    Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
dogmaphobic's avatar
dogmaphobic committed
590

Don Gagne's avatar
Don Gagne committed
591 592 593 594
    // Start the worker routine
    _currentParamRequestListComponentIndex = 0;
    _currentParamRequestListParamIndex = 0;
}
595

Don Gagne's avatar
Don Gagne committed
596 597 598 599 600 601 602
/// Sends the next parameter to the vehicle
void MockLink::_paramRequestListWorker(void)
{
    if (_currentParamRequestListComponentIndex == -1) {
        // Initial request complete
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
603

Don Gagne's avatar
Don Gagne committed
604 605 606
    int componentId = _mapParamName2Value.keys()[_currentParamRequestListComponentIndex];
    int cParameters = _mapParamName2Value[componentId].count();
    QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex];
607

Don Gagne's avatar
Don Gagne committed
608 609 610
    if ((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest || _failureMode == MockConfiguration::FailMissingParamOnAllRequests) && paramName == _failParam) {
        qCDebug(MockLinkLog) << "Skipping param send:" << paramName;
    } else {
611

Don Gagne's avatar
Don Gagne committed
612 613
        char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
        mavlink_message_t responseMsg;
614

Don Gagne's avatar
Don Gagne committed
615 616
        Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
        Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
617

Don Gagne's avatar
Don Gagne committed
618
        MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
619

Don Gagne's avatar
Don Gagne committed
620 621
        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
622

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

625 626
        mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                          componentId,                                   // component id
627
                                          _mavlinkChannel,
628 629 630 631 632 633
                                          &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
634
        respondWithMavlinkMessage(responseMsg);
635
    }
dogmaphobic's avatar
dogmaphobic committed
636

Don Gagne's avatar
Don Gagne committed
637 638 639 640 641 642 643 644
    // 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;
645
        }
646 647 648 649 650 651 652
    }
}

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

654
    Q_ASSERT(request.target_system == _vehicleSystemId);
655
    int componentId = request.target_component;
dogmaphobic's avatar
dogmaphobic committed
656

657 658
    // 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
659
    paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
660
    strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
661

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

664 665
    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
666
    Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);
667

668
    // Save the new value
669
    _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
670

671 672
    // Respond with a param_value to ack
    mavlink_message_t responseMsg;
673 674
    mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                      componentId,                                               // component id
675
                                      _mavlinkChannel,
676 677 678 679 680 681
                                      &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
682
    respondWithMavlinkMessage(responseMsg);
683 684 685 686
}

void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
{
687
    mavlink_message_t   responseMsg;
688 689
    mavlink_param_request_read_t request;
    mavlink_msg_param_request_read_decode(&msg, &request);
690

Don Gagne's avatar
Don Gagne committed
691
    const QString paramName(QString::fromLocal8Bit(request.param_id, strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)));
692
    int componentId = request.target_component;
693 694

    // special case for magic _HASH_CHECK value
Don Gagne's avatar
Don Gagne committed
695
    if (request.target_component == MAV_COMP_ID_ALL && paramName == "_HASH_CHECK") {
696 697 698 699
        mavlink_param_union_t   valueUnion;
        valueUnion.type = MAV_PARAM_TYPE_UINT32;
        valueUnion.param_uint32 = 0;
        // Special case of magic hash check value
700 701
        mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                          componentId,
702
                                          _mavlinkChannel,
703 704 705 706 707 708
                                          &responseMsg,
                                          request.param_id,
                                          valueUnion.param_float,
                                          MAV_PARAM_TYPE_UINT32,
                                          0,
                                          -1);
709 710 711 712
        respondWithMavlinkMessage(responseMsg);
        return;
    }

713
    Q_ASSERT(_mapParamName2Value.contains(componentId));
714

715 716
    char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
    paramId[0] = 0;
717

718
    Q_ASSERT(request.target_system == _vehicleSystemId);
719

720 721 722
    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);
723
    } else {
724
        // Request is by index
725

726
        Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count());
727

728
        QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
729 730
        Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
        strcpy(paramId, key.toLocal8Bit().constData());
731
    }
732

733
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
734
    Q_ASSERT(_mapParamName2MavParamType.contains(paramId));
735

Don Gagne's avatar
Don Gagne committed
736 737 738 739 740 741
    if (_failureMode == MockConfiguration::FailMissingParamOnAllRequests && strcmp(paramId, _failParam) == 0) {
        qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam;
        // Fail to send this param no matter what
        return;
    }

742 743
    mavlink_msg_param_value_pack_chan(_vehicleSystemId,
                                      componentId,                                               // component id
744
                                      _mavlinkChannel,
745 746 747 748 749 750
                                      &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
751
    respondWithMavlinkMessage(responseMsg);
752 753
}

754 755 756
void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
{
    uint16_t chanRaw[18];
dogmaphobic's avatar
dogmaphobic committed
757

758 759 760 761
    for (int i=0; i<18; i++) {
        chanRaw[i] = UINT16_MAX;
    }
    chanRaw[channel] = raw;
dogmaphobic's avatar
dogmaphobic committed
762

763
    mavlink_message_t responseMsg;
764 765
    mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
766
                                      _mavlinkChannel,
767 768 769 770
                                      &responseMsg,          // Outgoing message
                                      0,                     // time since boot, ignored
                                      18,                    // channel count
                                      chanRaw[0],            // channel raw value
771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788
            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
789 790 791 792 793 794 795
    respondWithMavlinkMessage(responseMsg);
}

void MockLink::_handleFTP(const mavlink_message_t& msg)
{
    Q_ASSERT(_fileServer);
    _fileServer->handleFTPMessage(msg);
796
}
797 798 799

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

803
    mavlink_command_long_t request;
804
    uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
dogmaphobic's avatar
dogmaphobic committed
805

806 807
    mavlink_msg_command_long_decode(&msg, &request);

808 809
    switch (request.command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
810 811 812 813 814
        if (request.param1 == 0.0f) {
            _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
        } else {
            _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
        }
815
        commandResult = MAV_RESULT_ACCEPTED;
816 817
        break;
    case MAV_CMD_PREFLIGHT_CALIBRATION:
818 819 820
        _handlePreFlightCalibration(request);
        commandResult = MAV_RESULT_ACCEPTED;
        break;
821 822 823
    case MAV_CMD_PREFLIGHT_STORAGE:
        commandResult = MAV_RESULT_ACCEPTED;
        break;
824 825 826 827
    case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
        commandResult = MAV_RESULT_ACCEPTED;
        _respondWithAutopilotVersion();
        break;
828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859
    case MAV_CMD_USER_1:
        // Test command which always returns MAV_RESULT_ACCEPTED
        commandResult = MAV_RESULT_ACCEPTED;
        break;
    case MAV_CMD_USER_2:
        // Test command which always returns MAV_RESULT_FAILED
        commandResult = MAV_RESULT_FAILED;
        break;
    case MAV_CMD_USER_3:
        // Test command which returns MAV_RESULT_ACCEPTED on second attempt
        if (firstCmdUser3) {
           firstCmdUser3 = false;
           return;
        } else {
            firstCmdUser3 = true;
            commandResult = MAV_RESULT_ACCEPTED;
        }
        break;
    case MAV_CMD_USER_4:
        // Test command which returns MAV_RESULT_FAILED on second attempt
        if (firstCmdUser4) {
           firstCmdUser4 = false;
           return;
        } else {
            firstCmdUser4 = true;
            commandResult = MAV_RESULT_FAILED;
        }
        break;
    case MAV_CMD_USER_5:
        // No response
        return;
        break;
860
    }
861 862

    mavlink_message_t commandAck;
863 864
    mavlink_msg_command_ack_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
865
                                      _mavlinkChannel,
866 867
                                      &commandAck,
                                      request.command,
DonLakeFlyer's avatar
DonLakeFlyer committed
868 869
                                      commandResult,
                                      0);
870
    respondWithMavlinkMessage(commandAck);
871 872
}

873 874 875 876
void MockLink::_respondWithAutopilotVersion(void)
{
    mavlink_message_t msg;

Don Gagne's avatar
Don Gagne committed
877 878 879 880
    uint8_t customVersion[8] = { };
    uint32_t flightVersion = 0;
    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        flightVersion |= 3 << (8*3);
881
        flightVersion |= 5 << (8*2);
Don Gagne's avatar
Don Gagne committed
882 883 884 885 886 887 888 889
        flightVersion |= 0 << (8*1);
        flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0);
    } else if (_firmwareType == MAV_AUTOPILOT_PX4) {
        flightVersion |= 1 << (8*3);
        flightVersion |= 4 << (8*2);
        flightVersion |= 1 << (8*1);
        flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0);
    }
890

891 892
    mavlink_msg_autopilot_version_pack_chan(_vehicleSystemId,
                                            _vehicleComponentId,
893
                                            _mavlinkChannel,
894 895 896 897 898 899 900 901 902 903 904 905
                                            &msg,
                                            0,                               // capabilities,
                                            flightVersion,                   // 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
906 907 908
    respondWithMavlinkMessage(msg);
}

909
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
910
{
911
    _missionItemHandler.setMissionItemFailureMode(failureMode);
912
}
913 914 915

void MockLink::_sendHomePosition(void)
{
916 917 918 919 920 921 922 923
    mavlink_message_t msg;

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

924 925
    mavlink_msg_home_position_pack_chan(_vehicleSystemId,
                                        _vehicleComponentId,
926
                                        _mavlinkChannel,
927 928 929 930 931 932
                                        &msg,
                                        (int32_t)(_vehicleLatitude * 1E7),
                                        (int32_t)(_vehicleLongitude * 1E7),
                                        (int32_t)(_vehicleAltitude * 1000),
                                        0.0f, 0.0f, 0.0f,
                                        &bogus[0],
933 934
                                        0.0f, 0.0f, 0.0f,
                                        0);
935
    respondWithMavlinkMessage(msg);
936
}
Don Gagne's avatar
Don Gagne committed
937 938 939 940 941 942

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

943 944
    mavlink_msg_gps_raw_int_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
945
                                      _mavlinkChannel,
946 947 948 949 950 951 952 953 954 955
                                      &msg,
                                      timeTick++,                            // time since boot
                                      3,                                     // 3D fix
                                      (int32_t)(_vehicleLatitude  * 1E7),
                                      (int32_t)(_vehicleLongitude * 1E7),
                                      (int32_t)(_vehicleAltitude  * 1000),
                                      UINT16_MAX, UINT16_MAX,                // HDOP/VDOP not known
                                      UINT16_MAX,                            // velocity not known
                                      UINT16_MAX,                            // course over ground not known
                                      8);                                    // satellite count
Don Gagne's avatar
Don Gagne committed
956 957
    respondWithMavlinkMessage(msg);
}
958

959 960 961 962 963 964 965 966
void MockLink::_sendStatusTextMessages(void)
{
    struct StatusMessage {
        MAV_SEVERITY        severity;
        const char*         msg;
    };

    static const struct StatusMessage rgMessages[] = {
967 968 969 970 971 972 973 974 975 976
    { 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" },
};
977 978 979 980 981

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

982 983
        mavlink_msg_statustext_pack_chan(_vehicleSystemId,
                                         _vehicleComponentId,
984
                                         _mavlinkChannel,
985 986 987
                                         &msg,
                                         status->severity,
                                         status->msg);
988 989 990 991
        respondWithMavlinkMessage(msg);
    }
}

992 993 994
MockConfiguration::MockConfiguration(const QString& name)
    : LinkConfiguration(name)
    , _firmwareType(MAV_AUTOPILOT_PX4)
995
    , _vehicleType(MAV_TYPE_QUADROTOR)
996
    , _sendStatusText(false)
Don Gagne's avatar
Don Gagne committed
997
    , _failureMode(FailNone)
998 999 1000 1001 1002 1003 1004
{

}

MockConfiguration::MockConfiguration(MockConfiguration* source)
    : LinkConfiguration(source)
{
1005
    _firmwareType =     source->_firmwareType;
dogmaphobic's avatar
dogmaphobic committed
1006
    _vehicleType =      source->_vehicleType;
1007
    _sendStatusText =   source->_sendStatusText;
Don Gagne's avatar
Don Gagne committed
1008
    _failureMode =      source->_failureMode;
1009 1010 1011 1012 1013 1014
}

void MockConfiguration::copyFrom(LinkConfiguration *source)
{
    LinkConfiguration::copyFrom(source);
    MockConfiguration* usource = dynamic_cast<MockConfiguration*>(source);
1015 1016 1017 1018 1019 1020 1021

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

    _firmwareType =     usource->_firmwareType;
1022
    _vehicleType =      usource->_vehicleType;
1023
    _sendStatusText =   usource->_sendStatusText;
Don Gagne's avatar
Don Gagne committed
1024
    _failureMode =      usource->_failureMode;
1025 1026 1027 1028 1029 1030
}

void MockConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    settings.setValue(_firmwareTypeKey, (int)_firmwareType);
1031
    settings.setValue(_vehicleTypeKey, (int)_vehicleType);
1032
    settings.setValue(_sendStatusTextKey, _sendStatusText);
Don Gagne's avatar
Don Gagne committed
1033
    settings.setValue(_failureModeKey, (int)_failureMode);
1034 1035 1036 1037 1038 1039 1040 1041
    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();
1042
    _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt();
1043
    _sendStatusText = settings.value(_sendStatusTextKey, false).toBool();
Don Gagne's avatar
Don Gagne committed
1044
    _failureMode = (FailureMode_t)settings.value(_failureModeKey, (int)FailNone).toInt();
1045 1046 1047 1048 1049 1050 1051 1052 1053
    settings.endGroup();
}

void MockConfiguration::updateSettings()
{
    if (_link) {
        MockLink* ulink = dynamic_cast<MockLink*>(_link);
        if (ulink) {
            // Restart connect not supported
1054
            qWarning() << "updateSettings not supported";
1055 1056 1057 1058
            //ulink->_restartConnection();
        }
    }
}
1059 1060 1061

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

    mockConfig->setDynamic(true);
1065
    SharedLinkConfigurationPointer config = linkMgr->addConfiguration(mockConfig);
1066

1067
    return qobject_cast<MockLink*>(linkMgr->createConnectedLink(config));
1068 1069
}

Don Gagne's avatar
Don Gagne committed
1070
MockLink*  MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
1071 1072 1073 1074 1075 1076
{
    MockConfiguration* mockConfig = new MockConfiguration("PX4 MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_PX4);
    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
    mockConfig->setSendStatusText(sendStatusText);
Don Gagne's avatar
Don Gagne committed
1077
    mockConfig->setFailureMode(failureMode);
1078 1079 1080 1081

    return _startMockLink(mockConfig);
}

Don Gagne's avatar
Don Gagne committed
1082
MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
1083 1084 1085 1086 1087 1088
{
    MockConfiguration* mockConfig = new MockConfiguration("Generic MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_GENERIC);
    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
    mockConfig->setSendStatusText(sendStatusText);
Don Gagne's avatar
Don Gagne committed
1089
    mockConfig->setFailureMode(failureMode);
1090 1091 1092 1093

    return _startMockLink(mockConfig);
}

Don Gagne's avatar
Don Gagne committed
1094
MockLink*  MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
1095 1096 1097 1098 1099 1100
{
    MockConfiguration* mockConfig = new MockConfiguration("APM ArduCopter MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
    mockConfig->setSendStatusText(sendStatusText);
Don Gagne's avatar
Don Gagne committed
1101
    mockConfig->setFailureMode(failureMode);
1102 1103 1104 1105

    return _startMockLink(mockConfig);
}

Don Gagne's avatar
Don Gagne committed
1106
MockLink*  MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
1107 1108 1109 1110 1111 1112
{
    MockConfiguration* mockConfig = new MockConfiguration("APM ArduPlane MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
    mockConfig->setVehicleType(MAV_TYPE_FIXED_WING);
    mockConfig->setSendStatusText(sendStatusText);
Don Gagne's avatar
Don Gagne committed
1113
    mockConfig->setFailureMode(failureMode);
1114 1115 1116 1117

    return _startMockLink(mockConfig);
}

1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129
MockLink*  MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    MockConfiguration* mockConfig = new MockConfiguration("APM ArduSub MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
    mockConfig->setVehicleType(MAV_TYPE_SUBMARINE);
    mockConfig->setSendStatusText(sendStatusText);
    mockConfig->setFailureMode(failureMode);

    return _startMockLink(mockConfig);
}

Don Gagne's avatar
Don Gagne committed
1130 1131 1132 1133
void MockLink::_sendRCChannels(void)
{
    mavlink_message_t   msg;

1134 1135
    mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
1136
                                      _mavlinkChannel,
1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149
                                      &msg,
                                      0,                     // time_boot_ms
                                      8,                     // chancount
                                      1500,                  // chan1_raw
                                      1500,                  // chan2_raw
                                      1500,                  // chan3_raw
                                      1500,                  // chan4_raw
                                      1500,                  // chan5_raw
                                      1500,                  // chan6_raw
                                      1500,                  // chan7_raw
                                      1500,                  // chan8_raw
                                      UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
                                      0);                    // rssi
Don Gagne's avatar
Don Gagne committed
1150 1151 1152 1153

    respondWithMavlinkMessage(msg);

}
1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175

void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
{
    const char* pCalMessage;
    static const char* gyroCalResponse = "[cal] calibration started: 2 gyro";
    static const char* magCalResponse = "[cal] calibration started: 2 mag";
    static const char* accelCalResponse = "[cal] calibration started: 2 accel";

    if (request.param1 == 1) {
        // Gyro cal
        pCalMessage = gyroCalResponse;
    } else if (request.param2 == 1) {
        // Mag cal
        pCalMessage = magCalResponse;
    } else if (request.param5 == 1) {
        // Accel cal
        pCalMessage = accelCalResponse;
    } else {
        return;
    }

    mavlink_message_t msg;
1176 1177
    mavlink_msg_statustext_pack_chan(_vehicleSystemId,
                                     _vehicleComponentId,
1178
                                     _mavlinkChannel,
1179 1180 1181
                                     &msg,
                                     MAV_SEVERITY_INFO,
                                     pCalMessage);
1182 1183 1184
    respondWithMavlinkMessage(msg);
}

1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196
void MockLink::_handleLogRequestList(const mavlink_message_t& msg)
{
    mavlink_log_request_list_t request;

    mavlink_msg_log_request_list_decode(&msg, &request);

    if (request.start != 0 && request.end != 0xffff) {
        qWarning() << "MockLink::_handleLogRequestList cannot handle partial requests";
        return;
    }

    mavlink_message_t responseMsg;
1197 1198
    mavlink_msg_log_entry_pack_chan(_vehicleSystemId,
                                    _vehicleComponentId,
1199
                                    _mavlinkChannel,
1200 1201 1202 1203 1204 1205
                                    &responseMsg,
                                    _logDownloadLogId,       // log id
                                    1,                       // num_logs
                                    1,                       // last_log_num
                                    0,                       // time_utc
                                    _logDownloadFileSize);   // size
1206 1207 1208 1209 1210 1211 1212 1213 1214 1215
    respondWithMavlinkMessage(responseMsg);
}

void MockLink::_handleLogRequestData(const mavlink_message_t& msg)
{
    mavlink_log_request_data_t request;

    mavlink_msg_log_request_data_decode(&msg, &request);

    if (_logDownloadFilename.isEmpty()) {
1216
        #ifdef UNITTEST_BUILD
1217
        _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize);
1218
        #endif
1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252
    }

    if (request.id != 0) {
        qWarning() << "MockLink::_handleLogRequestData id must be 0";
        return;
    }

    if (request.ofs > _logDownloadFileSize - 1) {
        qWarning() << "MockLink::_handleLogRequestData offset past end of file request.ofs:size" << request.ofs << _logDownloadFileSize;
        return;
    }

    // This will trigger _logDownloadWorker to send data
    _logDownloadCurrentOffset = request.ofs;
    if (request.ofs + request.count > _logDownloadFileSize) {
        request.count = _logDownloadFileSize - request.ofs;
    }
    _logDownloadBytesRemaining = request.count;
}

void MockLink::_logDownloadWorker(void)
{
    if (_logDownloadBytesRemaining != 0) {
        QFile file(_logDownloadFilename);
        if (file.open(QIODevice::ReadOnly)) {
            uint8_t buffer[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN];

            qint64 bytesToRead = qMin(_logDownloadBytesRemaining, (uint32_t)MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
            Q_ASSERT(file.seek(_logDownloadCurrentOffset));
            Q_ASSERT(file.read((char *)buffer, bytesToRead) == bytesToRead);

            qDebug() << "MockLink::_logDownloadWorker" << _logDownloadCurrentOffset << _logDownloadBytesRemaining;

            mavlink_message_t responseMsg;
1253 1254
            mavlink_msg_log_data_pack_chan(_vehicleSystemId,
                                           _vehicleComponentId,
1255
                                           _mavlinkChannel,
1256 1257 1258 1259 1260
                                           &responseMsg,
                                           _logDownloadLogId,
                                           _logDownloadCurrentOffset,
                                           bytesToRead,
                                           &buffer[0]);
1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271
            respondWithMavlinkMessage(responseMsg);

            _logDownloadCurrentOffset += bytesToRead;
            _logDownloadBytesRemaining -= bytesToRead;

            file.close();
        } else {
            qWarning() << "MockLink::_logDownloadWorker open failed" << file.errorString();
        }
    }
}
1272 1273 1274 1275 1276 1277 1278 1279 1280

void MockLink::_sendADSBVehicles(void)
{
    mavlink_message_t responseMsg;
    mavlink_msg_adsb_vehicle_pack_chan(_vehicleSystemId,
                                       _vehicleComponentId,
                                       _mavlinkChannel,
                                       &responseMsg,
                                       12345,                           // ICAO address
DonLakeFlyer's avatar
DonLakeFlyer committed
1281 1282
                                       (_vehicleLatitude + 0.001) * 1e7,
                                       (_vehicleLongitude + 0.001) * 1e7,
1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294
                                       ADSB_ALTITUDE_TYPE_GEOMETRIC,
                                       100 * 1000,                      // Altitude in millimeters
                                       10 * 100,                        // Heading in centidegress
                                       0, 0,                            // Horizontal/Vertical velocity
                                       "N1234500",                      // Callsign
                                       ADSB_EMITTER_TYPE_ROTOCRAFT,
                                       1,                               // Seconds since last communication
                                       ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED,
                                       0);                              // Squawk code

    respondWithMavlinkMessage(responseMsg);
}