MockLink.cc

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

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

    return _startMockLink(mockConfig);
}

MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    MockConfiguration* mockConfig = new MockConfiguration("Generic MockLink");

    mockConfig->setFirmwareType(MAV_AUTOPILOT_GENERIC);
    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
    mockConfig->setSendStatusText(sendStatusText);
    mockConfig->setFailureMode(failureMode);

    return _startMockLink(mockConfig);
}

MockLink*  MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    MockConfiguration* mockConfig = new MockConfiguration("APM ArduCopter MockLink");

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

    return _startMockLink(mockConfig);
}

MockLink*  MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    MockConfiguration* mockConfig = new MockConfiguration("APM ArduPlane MockLink");

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

    return _startMockLink(mockConfig);
}

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);
}

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

    mavlink_msg_rc_channels_pack(_vehicleSystemId,
                                 _vehicleComponentId,
                                 &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

    respondWithMavlinkMessage(msg);

}

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;
    mavlink_msg_statustext_pack(_vehicleSystemId,
                                _vehicleComponentId,
                                &msg,
                                MAV_SEVERITY_INFO,
                                pCalMessage);
    respondWithMavlinkMessage(msg);
}

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;
    mavlink_msg_log_entry_pack(_vehicleSystemId,
                               _vehicleComponentId,
                               &responseMsg,
                               _logDownloadLogId,       // log id
                               1,                       // num_logs
                               1,                       // last_log_num
                               0,                       // time_utc
                               _logDownloadFileSize);   // size
    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()) {
        _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize);
    }

    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;
            mavlink_msg_log_data_pack(_vehicleSystemId,
                                       _vehicleComponentId,
                                       &responseMsg,
                                       _logDownloadLogId,
                                       _logDownloadCurrentOffset,
                                       bytesToRead,
                                       &buffer[0]);
            respondWithMavlinkMessage(responseMsg);

            _logDownloadCurrentOffset += bytesToRead;
            _logDownloadBytesRemaining -= bytesToRead;

            file.close();
        } else {
            qWarning() << "MockLink::_logDownloadWorker open failed" << file.errorString();
        }
    }
}