diff --git a/src/Taisync/TaisyncHandler.h b/src/Taisync/TaisyncHandler.h index 08aaa2d..4eab269 100644 --- a/src/Taisync/TaisyncHandler.h +++ b/src/Taisync/TaisyncHandler.h @@ -15,10 +15,11 @@ #include #if defined(__ios__) || defined(__android__) -#define TAISYNC_VIDEO_UDP_PORT 5600 -#define TAISYNC_VIDEO_TCP_PORT 8000 -#define TAISYNC_SETTINGS_PORT 8200 -#define TAISYNC_TELEM_PORT 8400 +#define TAISYNC_VIDEO_UDP_PORT 5600 +#define TAISYNC_VIDEO_TCP_PORT 8000 +#define TAISYNC_SETTINGS_PORT 8200 +#define TAISYNC_TELEM_PORT 8400 +#define TAISYNC_TELEM_TARGET_PORT 14550 #else #define TAISYNC_SETTINGS_PORT 80 #define TAISYNC_SETTINGS_TARGET "192.168.1.99" diff --git a/src/Taisync/TaisyncManager.cc b/src/Taisync/TaisyncManager.cc index 35121b4..31868b7 100644 --- a/src/Taisync/TaisyncManager.cc +++ b/src/Taisync/TaisyncManager.cc @@ -35,6 +35,11 @@ TaisyncManager::~TaisyncManager() _taiTelemetery->deleteLater(); _taiTelemetery = nullptr; } + if(_telemetrySocket) { + _telemetrySocket->close(); + _telemetrySocket->deleteLater(); + _telemetrySocket = nullptr; + } if (_taiVideo) { _taiVideo->close(); _taiVideo->deleteLater(); @@ -69,6 +74,11 @@ TaisyncManager::_setEnabled() #if defined(__ios__) || defined(__android__) _taiTelemetery = new TaisyncTelemetry(this); QObject::connect(_taiTelemetery, &TaisyncTelemetry::bytesReady, this, &TaisyncManager::_readTelemBytes); + _telemetrySocket = new QUdpSocket(this); + _telemetrySocket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption, 64 * 1024); + _telemetrySocket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 64 * 1024); + QObject::connect(_telemetrySocket, &QUdpSocket::readyRead, this, &TaisyncManager::_readUDPBytes); + _telemetrySocket->bind(QHostAddress::LocalHost, 0, QUdpSocket::ShareAddress); #endif _workTimer.start(1000); } else { @@ -131,6 +141,35 @@ TaisyncManager::_setVideoEnabled() } //----------------------------------------------------------------------------- +#if defined(__ios__) || defined(__android__) +void +TaisyncManager::_readTelemBytes(QByteArray bytesIn) +{ + //-- Send telemetry from vehicle to QGC (using normal UDP) + _telemetrySocket->writeDatagram(bytesIn, QHostAddress::LocalHost, TAISYNC_TELEM_TARGET_PORT); +} +#endif + +//----------------------------------------------------------------------------- +#if defined(__ios__) || defined(__android__) +void +TaisyncManager::_readUDPBytes() +{ + if (!_telemetrySocket || !_taiTelemetery) { + return; + } + //-- Read UDP data from QGC + while (_telemetrySocket->hasPendingDatagrams()) { + QByteArray datagram; + datagram.resize(static_cast(_telemetrySocket->pendingDatagramSize())); + _telemetrySocket->readDatagram(datagram.data(), datagram.size()); + //-- Send it to vehicle + _taiTelemetery->writeBytes(datagram); + } +} +#endif + +//----------------------------------------------------------------------------- void TaisyncManager::_connected() { diff --git a/src/Taisync/TaisyncManager.h b/src/Taisync/TaisyncManager.h index 16066e5..15d72a1 100644 --- a/src/Taisync/TaisyncManager.h +++ b/src/Taisync/TaisyncManager.h @@ -59,6 +59,10 @@ private slots: void _updateSettings (QByteArray jSonData); void _setEnabled (); void _setVideoEnabled (); +#if defined(__ios__) || defined(__android__) + void _readUDPBytes (); + void _readTelemBytes (QByteArray bytesIn); +#endif private: @@ -78,6 +82,7 @@ private: #if defined(__ios__) || defined(__android__) TaisyncTelemetry* _taiTelemetery = nullptr; TaisyncVideoReceiver* _taiVideo = nullptr; + QUdpSocket* _telemetrySocket= nullptr; #endif bool _enableVideo = true; bool _enabled = true;