MockLink.cc

                                            MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0),
                                            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
                                            0);                              // uid2
    respondWithMavlinkMessage(msg);
}

void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult)
{
    _missionItemHandler.setFailureMode(failureMode, failureAckResult);
}

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

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

    mavlink_msg_home_position_pack_chan(_vehicleSystemId,
                                        _vehicleComponentId,
                                        _mavlinkChannel,
                                        &msg,
                                        (int32_t)(_vehicleLatitude * 1E7),
                                        (int32_t)(_vehicleLongitude * 1E7),
                                        (int32_t)(_vehicleAltitude * 1000),
                                        0.0f, 0.0f, 0.0f,
                                        &bogus[0],
            0.0f, 0.0f, 0.0f,
            0);
    respondWithMavlinkMessage(msg);
}

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

    mavlink_msg_gps_raw_int_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
                                      _mavlinkChannel,
                                      &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,                                    // satellites visible
                                      //-- Extension
                                      0,                                    // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up).
                                      0,                                    // Position uncertainty in meters * 1000 (positive for up).
                                      0,                                    // Altitude uncertainty in meters * 1000 (positive for up).
                                      0,                                    // Speed uncertainty in meters * 1000 (positive for up).
                                      0,                                    // Heading / track uncertainty in degrees * 1e5.
                                      65535);                               // Yaw not provided
    respondWithMavlinkMessage(msg);
}

void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks)
{
    mavlink_message_t msg;

    int cChunks = 4;
    int num = 0;
    for (int i=0; i<cChunks; i++) {
        if (missingChunks && (i & 1)) {
            continue;
        }
        int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
        char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
        memset(msgBuf, 0, sizeof(msgBuf));
        if (i == cChunks - 1) {
            // Last chunk is partial
            cBuf /= 2;
        }
        for (int j=0; j<cBuf-1; j++) {
            msgBuf[j] = '0' + num++;
            if (num > 9) {
                num = 0;
            }
        }
        msgBuf[cBuf-1] = 'A' + i;

        mavlink_msg_statustext_pack_chan(_vehicleSystemId,
                                         _vehicleComponentId,
                                         _mavlinkChannel,
                                         &msg,
                                         MAV_SEVERITY_INFO,
                                         msgBuf,
                                         chunkId,
                                         i);                    // chunk sequence number
        respondWithMavlinkMessage(msg);
    }

}

void MockLink::_sendStatusTextMessages(void)
{
    struct StatusMessage {
        MAV_SEVERITY        severity;
        const char*         msg;
    };

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

    mavlink_message_t msg;

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

        mavlink_msg_statustext_pack_chan(_vehicleSystemId,
                                         _vehicleComponentId,
                                         _mavlinkChannel,
                                         &msg,
                                         status->severity,
                                         status->msg,
                                         0,                     // Not a chunked sequence
                                         0);                    // Not a chunked sequence
        //respondWithMavlinkMessage(msg);
    }

    _sendChunkedStatusText(1, false /* missingChunks */);
    _sendChunkedStatusText(2, true /* missingChunks */);
    _sendChunkedStatusText(3, false /* missingChunks */);   // This should cause the previous incomplete chunk to spit out
    _sendChunkedStatusText(4, true /* missingChunks */);    // This should cause the timeout to fire
}

MockConfiguration::MockConfiguration(const QString& name)
    : LinkConfiguration(name)
    , _firmwareType     (MAV_AUTOPILOT_PX4)
    , _vehicleType      (MAV_TYPE_QUADROTOR)
    , _sendStatusText   (false)
    , _highLatency      (false)
    , _failureMode      (FailNone)
{

}

MockConfiguration::MockConfiguration(MockConfiguration* source)
    : LinkConfiguration(source)
{
    _firmwareType =     source->_firmwareType;
    _vehicleType =      source->_vehicleType;
    _sendStatusText =   source->_sendStatusText;
    _highLatency =      source->_highLatency;
    _failureMode =      source->_failureMode;
}

void MockConfiguration::copyFrom(LinkConfiguration *source)
{
    LinkConfiguration::copyFrom(source);
    auto* usource = qobject_cast<MockConfiguration*>(source);

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

    _firmwareType =     usource->_firmwareType;
    _vehicleType =      usource->_vehicleType;
    _sendStatusText =   usource->_sendStatusText;
    _highLatency =      usource->_highLatency;
    _failureMode =      usource->_failureMode;
}

void MockConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    settings.setValue(_firmwareTypeKey, (int)_firmwareType);
    settings.setValue(_vehicleTypeKey, (int)_vehicleType);
    settings.setValue(_sendStatusTextKey, _sendStatusText);
    settings.setValue(_highLatencyKey, _highLatency);
    settings.setValue(_failureModeKey, (int)_failureMode);
    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();
    _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt();
    _sendStatusText = settings.value(_sendStatusTextKey, false).toBool();
    _highLatency = settings.value(_highLatencyKey, false).toBool();
    _failureMode = (FailureMode_t)settings.value(_failureModeKey, (int)FailNone).toInt();
    settings.endGroup();
}

void MockConfiguration::updateSettings()
{
    if (_link) {
        MockLink* ulink = dynamic_cast<MockLink*>(_link);
        if (ulink) {
            // Restart connect not supported
            qWarning() << "updateSettings not supported";
            //ulink->_restartConnection();
        }
    }
}

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

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

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

MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    MockConfiguration* mockConfig = new MockConfiguration(configName);

    mockConfig->setFirmwareType(firmwareType);
    mockConfig->setVehicleType(vehicleType);
    mockConfig->setSendStatusText(sendStatusText);
    mockConfig->setFailureMode(failureMode);

    return _startMockLink(mockConfig);
}

MockLink*  MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("PX4 MultiRotor MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
}

MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
}

MockLink*  MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
}

MockLink*  MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("ArduPlane MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode);
}

MockLink*  MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("ArduSub MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode);
}

MockLink*  MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
{
    return _startMockLinkWorker("ArduRover MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode);
}

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

    mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
                                      _vehicleComponentId,
                                      _mavlinkChannel,
                                      &msg,
                                      0,                     // time_boot_ms
                                      16,                    // chancount
                                      1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,   // channel 1-8
                                      1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,   // channel 9-16
                                      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_chan(_vehicleSystemId,
                                     _vehicleComponentId,
                                     _mavlinkChannel,
                                     &msg,
                                     MAV_SEVERITY_INFO,
                                     pCalMessage,
                                     0, 0);                 // Not chunked
    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_chan(_vehicleSystemId,
                                    _vehicleComponentId,
                                    _mavlinkChannel,
                                    &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()) {
#ifdef UNITTEST_BUILD
        _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize);
#endif
    }

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

            _logDownloadCurrentOffset += bytesToRead;
            _logDownloadBytesRemaining -= bytesToRead;

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

void MockLink::_sendADSBVehicles(void)
{
    _adsbAngle += 2;
    _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(500, _adsbAngle);
    _adsbVehicleCoordinate.setAltitude(100);

    mavlink_message_t responseMsg;
    mavlink_msg_adsb_vehicle_pack_chan(_vehicleSystemId,
                                       _vehicleComponentId,
                                       _mavlinkChannel,
                                       &responseMsg,
                                       12345,                                       // ICAO address
                                       _adsbVehicleCoordinate.latitude() * 1e7,
                                       _adsbVehicleCoordinate.longitude() * 1e7,
                                       ADSB_ALTITUDE_TYPE_GEOMETRIC,
                                       _adsbVehicleCoordinate.altitude() * 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);
}