MockLink.cc

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

}

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

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

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

    _firmwareType =     usource->_firmwareType;
    _vehicleType =      usource->_vehicleType;
    _sendStatusText =   usource->_sendStatusText;
    _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(_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();
    _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::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_chan(_vehicleSystemId,
                                      _vehicleComponentId,
                                      _mavlinkChannel,
                                      &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_chan(_vehicleSystemId,
                                     _vehicleComponentId,
                                     _mavlinkChannel,
                                     &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_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);
}