Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
58fd244a
Commit
58fd244a
authored
Dec 22, 2018
by
Gus Grubba
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
iOS fully functional.
parent
339da87a
Changes
11
Hide whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
237 additions
and
113 deletions
+237
-113
TaisyncHandler.cc
src/Taisync/TaisyncHandler.cc
+26
-11
TaisyncHandler.h
src/Taisync/TaisyncHandler.h
+3
-1
TaisyncManager.cc
src/Taisync/TaisyncManager.cc
+99
-45
TaisyncManager.h
src/Taisync/TaisyncManager.h
+11
-3
TaisyncSettings.cc
src/Taisync/TaisyncSettings.cc
+18
-22
TaisyncSettings.h
src/Taisync/TaisyncSettings.h
+3
-0
TaisyncSettings.qml
src/Taisync/TaisyncSettings.qml
+1
-1
TaisyncTelemetry.cc
src/Taisync/TaisyncTelemetry.cc
+49
-9
TaisyncTelemetry.h
src/Taisync/TaisyncTelemetry.h
+6
-3
TaisyncVideoReceiver.cc
src/Taisync/TaisyncVideoReceiver.cc
+20
-15
TaisyncVideoReceiver.h
src/Taisync/TaisyncVideoReceiver.h
+1
-3
No files found.
src/Taisync/TaisyncHandler.cc
View file @
58fd244a
...
...
@@ -29,15 +29,22 @@ TaisyncHandler::~TaisyncHandler()
}
//-----------------------------------------------------------------------------
void
TaisyncHandler
::
close
()
bool
TaisyncHandler
::
close
()
{
bool
res
=
(
_tcpSocket
||
_tcpServer
);
if
(
_tcpSocket
)
{
qCDebug
(
TaisyncLog
)
<<
"Close Taisync TCP
"
;
qCDebug
(
TaisyncLog
)
<<
"Close Taisync TCP
socket on port"
<<
_tcpSocket
->
localPort
()
;
_tcpSocket
->
close
();
_tcpSocket
->
deleteLater
();
_tcpSocket
=
nullptr
;
}
if
(
_tcpServer
)
{
qCDebug
(
TaisyncLog
)
<<
"Close Taisync TCP server on port"
<<
_tcpServer
->
serverPort
();;
_tcpServer
->
close
();
_tcpServer
->
deleteLater
();
_tcpServer
=
nullptr
;
}
return
res
;
}
//-----------------------------------------------------------------------------
...
...
@@ -47,10 +54,12 @@ TaisyncHandler::_start(uint16_t port, QHostAddress addr)
close
();
_serverMode
=
addr
==
QHostAddress
::
AnyIPv4
;
if
(
_serverMode
)
{
qCDebug
(
TaisyncLog
)
<<
"Listen for Taisync TCP on port"
<<
port
;
_tcpServer
=
new
QTcpServer
(
this
);
QObject
::
connect
(
_tcpServer
,
&
QTcpServer
::
newConnection
,
this
,
&
TaisyncHandler
::
_newConnection
);
_tcpServer
->
listen
(
QHostAddress
::
AnyIPv4
,
port
);
if
(
!
_tcpServer
)
{
qCDebug
(
TaisyncLog
)
<<
"Listen for Taisync TCP on port"
<<
port
;
_tcpServer
=
new
QTcpServer
(
this
);
QObject
::
connect
(
_tcpServer
,
&
QTcpServer
::
newConnection
,
this
,
&
TaisyncHandler
::
_newConnection
);
_tcpServer
->
listen
(
QHostAddress
::
AnyIPv4
,
port
);
}
}
else
{
_tcpSocket
=
new
QTcpSocket
();
QObject
::
connect
(
_tcpSocket
,
&
QIODevice
::
readyRead
,
this
,
&
TaisyncHandler
::
_readBytes
);
...
...
@@ -69,24 +78,30 @@ TaisyncHandler::_start(uint16_t port, QHostAddress addr)
void
TaisyncHandler
::
_newConnection
()
{
qCDebug
(
TaisyncLog
)
<<
"New Taisync TCP Connection
"
;
qCDebug
(
TaisyncLog
)
<<
"New Taisync TCP Connection
on port"
<<
_tcpServer
->
serverPort
()
;
if
(
_tcpSocket
)
{
_tcpSocket
->
close
();
_tcpSocket
->
deleteLater
();
}
_tcpSocket
=
_tcpServer
->
nextPendingConnection
();
QObject
::
connect
(
_tcpSocket
,
&
QIODevice
::
readyRead
,
this
,
&
TaisyncHandler
::
_readBytes
);
emit
connected
();
if
(
_tcpSocket
)
{
QObject
::
connect
(
_tcpSocket
,
&
QIODevice
::
readyRead
,
this
,
&
TaisyncHandler
::
_readBytes
);
QObject
::
connect
(
_tcpSocket
,
&
QAbstractSocket
::
disconnected
,
this
,
&
TaisyncHandler
::
_socketDisconnected
);
emit
connected
();
}
else
{
qCWarning
(
TaisyncLog
)
<<
"New Taisync TCP Connection provided no socket"
;
}
}
//-----------------------------------------------------------------------------
void
TaisyncHandler
::
_socketDisconnected
()
{
qCDebug
(
TaisyncLog
)
<<
"Taisync T
elemetry Connection Closed"
;
qCDebug
(
TaisyncLog
)
<<
"Taisync T
CP Connection Closed on port"
<<
_tcpSocket
->
localPort
()
;
if
(
_tcpSocket
)
{
_tcpSocket
->
close
();
_tcpSocket
->
deleteLater
();
_tcpSocket
=
nullptr
;
}
emit
disconnected
();
}
src/Taisync/TaisyncHandler.h
View file @
58fd244a
...
...
@@ -36,7 +36,8 @@ public:
explicit
TaisyncHandler
(
QObject
*
parent
=
nullptr
);
~
TaisyncHandler
();
virtual
bool
start
()
=
0
;
virtual
void
close
();
virtual
bool
close
();
virtual
bool
isServerRunning
()
{
return
(
_serverMode
&&
_tcpServer
);
}
protected:
virtual
bool
_start
(
uint16_t
port
,
QHostAddress
addr
=
QHostAddress
::
AnyIPv4
);
...
...
@@ -48,6 +49,7 @@ protected slots:
signals:
void
connected
();
void
disconnected
();
protected:
bool
_serverMode
=
true
;
...
...
src/Taisync/TaisyncManager.cc
View file @
58fd244a
...
...
@@ -28,6 +28,13 @@ TaisyncManager::TaisyncManager(QGCApplication* app, QGCToolbox* toolbox)
//-----------------------------------------------------------------------------
TaisyncManager
::~
TaisyncManager
()
{
_close
();
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
_close
()
{
if
(
_taiSettings
)
{
_taiSettings
->
close
();
...
...
@@ -55,21 +62,32 @@ TaisyncManager::~TaisyncManager()
//-----------------------------------------------------------------------------
void
TaisyncManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
TaisyncManager
::
_reset
(
)
{
QGCTool
::
setToolbox
(
toolbox
);
_close
(
);
_taiSettings
=
new
TaisyncSettings
(
this
);
connect
(
_taiSettings
,
&
TaisyncSettings
::
updateSettings
,
this
,
&
TaisyncManager
::
_updateSettings
);
connect
(
_taiSettings
,
&
TaisyncSettings
::
connected
,
this
,
&
TaisyncManager
::
_connected
);
_appSettings
=
toolbox
->
settingsManager
()
->
appSettings
();
connect
(
_appSettings
->
enableTaisync
(),
&
Fact
::
rawValueChanged
,
this
,
&
TaisyncManager
::
_setEnabled
);
connect
(
_appSettings
->
enableTaisyncVideo
(),
&
Fact
::
rawValueChanged
,
this
,
&
TaisyncManager
::
_setVideoEnabled
);
connect
(
_taiSettings
,
&
TaisyncSettings
::
disconnected
,
this
,
&
TaisyncManager
::
_disconnected
);
if
(
!
_appSettings
)
{
_appSettings
=
_toolbox
->
settingsManager
()
->
appSettings
();
connect
(
_appSettings
->
enableTaisync
(),
&
Fact
::
rawValueChanged
,
this
,
&
TaisyncManager
::
_setEnabled
);
connect
(
_appSettings
->
enableTaisyncVideo
(),
&
Fact
::
rawValueChanged
,
this
,
&
TaisyncManager
::
_setVideoEnabled
);
}
_setEnabled
();
if
(
_enabled
)
{
_setVideoEnabled
();
}
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
{
QGCTool
::
setToolbox
(
toolbox
);
_reset
();
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
setDecodeIndex
(
int
idx
)
...
...
@@ -91,24 +109,22 @@ TaisyncManager::_setEnabled()
bool
enable
=
_appSettings
->
enableTaisync
()
->
rawValue
().
toBool
();
if
(
enable
)
{
#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
);
if
(
!
_taiTelemetery
)
{
_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
);
_taiTelemetery
->
start
();
}
#endif
_workTimer
.
start
(
1000
);
}
else
{
#if defined(__ios__) || defined(__android__)
if
(
_taiTelemetery
)
{
_taiTelemetery
->
close
();
_taiTelemetery
->
deleteLater
();
_taiTelemetery
=
nullptr
;
}
#endif
//-- Stop everything
_workTimer
.
stop
();
_close
();
}
_enabled
=
enable
;
}
...
...
@@ -119,23 +135,27 @@ TaisyncManager::_setVideoEnabled()
{
bool
enable
=
_appSettings
->
enableTaisyncVideo
()
->
rawValue
().
toBool
();
if
(
enable
)
{
//-- Hide video selection as we will be fixed to Taisync video and set the way we need it.
VideoSettings
*
pVSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
videoSettings
();
//-- First save current state
_savedVideoSource
=
pVSettings
->
videoSource
()
->
rawValue
();
_savedVideoUDP
=
pVSettings
->
udpPort
()
->
rawValue
();
_savedAR
=
pVSettings
->
aspectRatio
()
->
rawValue
();
_savedVideoState
=
pVSettings
->
visible
();
//-- Now set it up the way we need it do be
pVSettings
->
setVisible
(
false
);
pVSettings
->
udpPort
()
->
setRawValue
(
5600
);
pVSettings
->
aspectRatio
()
->
setRawValue
(
1024.0
/
768.0
);
pVSettings
->
videoSource
()
->
setRawValue
(
QString
(
VideoSettings
::
videoSourceUDP
));
if
(
!
_savedVideoSource
.
isValid
())
{
//-- Hide video selection as we will be fixed to Taisync video and set the way we need it.
VideoSettings
*
pVSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
videoSettings
();
//-- First save current state
_savedVideoSource
=
pVSettings
->
videoSource
()
->
rawValue
();
_savedVideoUDP
=
pVSettings
->
udpPort
()
->
rawValue
();
_savedAR
=
pVSettings
->
aspectRatio
()
->
rawValue
();
_savedVideoState
=
pVSettings
->
visible
();
//-- Now set it up the way we need it do be
pVSettings
->
setVisible
(
false
);
pVSettings
->
udpPort
()
->
setRawValue
(
5600
);
pVSettings
->
aspectRatio
()
->
setRawValue
(
1024.0
/
768.0
);
pVSettings
->
videoSource
()
->
setRawValue
(
QString
(
VideoSettings
::
videoSourceUDP
));
}
#if defined(__ios__) || defined(__android__)
//-- iOS and Android receive raw h.264 and need a different pipeline
qgcApp
()
->
toolbox
()
->
videoManager
()
->
setIsTaisync
(
true
);
_taiVideo
=
new
TaisyncVideoReceiver
(
this
);
_taiVideo
->
start
();
if
(
!
_taiVideo
)
{
//-- iOS and Android receive raw h.264 and need a different pipeline
qgcApp
()
->
toolbox
()
->
videoManager
()
->
setIsTaisync
(
true
);
_taiVideo
=
new
TaisyncVideoReceiver
(
this
);
_taiVideo
->
start
();
}
#endif
}
else
{
//-- Restore video settings
...
...
@@ -197,13 +217,25 @@ TaisyncManager::_connected()
emit
connectedChanged
();
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
_disconnected
()
{
qCDebug
(
TaisyncLog
)
<<
"Taisync Settings Disconnected"
;
_isConnected
=
false
;
emit
connectedChanged
();
_reset
();
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
_checkTaisync
()
{
if
(
_enabled
)
{
if
(
!
_isConnected
)
{
_taiSettings
->
start
();
if
(
!
_taiSettings
->
isServerRunning
())
{
_taiSettings
->
start
();
}
}
else
{
if
(
++
_currReq
>=
REQ_LAST
)
{
_currReq
=
REQ_LINK_STATUS
;
...
...
@@ -221,9 +253,12 @@ TaisyncManager::_checkTaisync()
case
REQ_VIDEO_SETTINGS
:
_taiSettings
->
requestVideoSettings
();
break
;
case
REQ_RADIO_SETTINGS
:
_taiSettings
->
requestRadioSettings
();
break
;
}
}
_workTimer
.
start
(
1
000
);
_workTimer
.
start
(
_isConnected
?
500
:
5
000
);
}
}
...
...
@@ -241,18 +276,37 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
QJsonObject
jObj
=
doc
.
object
();
//-- Link Status?
if
(
jSonData
.
contains
(
"
\"
flight
\"
:"
))
{
_linkConnected
=
jObj
[
"flight"
].
toString
(
""
)
==
"online"
;
_linkVidFormat
=
jObj
[
"videoformat"
].
toString
(
_linkVidFormat
);
_downlinkRSSI
=
jObj
[
"radiorssi"
].
toInt
(
_downlinkRSSI
);
_uplinkRSSI
=
jObj
[
"hdrssi"
].
toInt
(
_uplinkRSSI
);
emit
linkChanged
();
bool
tlinkConnected
=
jObj
[
"flight"
].
toString
(
""
)
==
"online"
;
if
(
tlinkConnected
!=
_linkConnected
)
{
_linkConnected
=
tlinkConnected
;
emit
linkConnectedChanged
();
}
QString
tlinkVidFormat
=
jObj
[
"videoformat"
].
toString
(
_linkVidFormat
);
int
tdownlinkRSSI
=
jObj
[
"radiorssi"
].
toInt
(
_downlinkRSSI
);
int
tuplinkRSSI
=
jObj
[
"hdrssi"
].
toInt
(
_uplinkRSSI
);
if
(
_linkVidFormat
!=
tlinkVidFormat
||
_downlinkRSSI
!=
tdownlinkRSSI
||
_uplinkRSSI
!=
tuplinkRSSI
)
{
_linkVidFormat
=
tlinkVidFormat
;
_downlinkRSSI
=
tdownlinkRSSI
;
_uplinkRSSI
=
tuplinkRSSI
;
emit
linkChanged
();
}
//-- Device Info?
}
else
if
(
jSonData
.
contains
(
"
\"
firmwareversion
\"
:"
))
{
_fwVersion
=
jObj
[
"firmwareversion"
].
toString
(
_fwVersion
);
_serialNumber
=
jObj
[
"sn"
].
toString
(
_serialNumber
);
QString
tfwVersion
=
jObj
[
"firmwareversion"
].
toString
(
_fwVersion
);
QString
tserialNumber
=
jObj
[
"sn"
].
toString
(
_serialNumber
);
if
(
tfwVersion
!=
_fwVersion
||
tserialNumber
!=
_serialNumber
)
{
_fwVersion
=
tfwVersion
;
_serialNumber
=
tserialNumber
;
emit
infoChanged
();
}
//-- Radio Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
freq
\"
:"
))
{
// {\"mode\":\"auto\",\"freq\":\"ch12\"}
//-- Video Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
maxbitrate
\"
:"
))
{
//{\"decode\":\"phone\",\"mode\":\"h264\",\"maxbitrate\":\"high\"}
}
...
...
src/Taisync/TaisyncManager.h
View file @
58fd244a
...
...
@@ -29,12 +29,12 @@ class TaisyncManager : public QGCTool
public:
Q_PROPERTY
(
bool
connected
READ
connected
NOTIFY
connectedChanged
)
Q_PROPERTY
(
bool
linkConnected
READ
linkConnected
NOTIFY
linkChanged
)
Q_PROPERTY
(
bool
linkConnected
READ
linkConnected
NOTIFY
linkC
onnectedC
hanged
)
Q_PROPERTY
(
QString
linkVidFormat
READ
linkVidFormat
NOTIFY
linkChanged
)
Q_PROPERTY
(
int
uplinkRSSI
READ
uplinkRSSI
NOTIFY
linkChanged
)
Q_PROPERTY
(
int
downlinkRSSI
READ
downlinkRSSI
NOTIFY
linkChanged
)
Q_PROPERTY
(
QString
serialNumber
READ
serialNumber
NOTIFY
link
Changed
)
Q_PROPERTY
(
QString
fwVersion
READ
fwVersion
NOTIFY
link
Changed
)
Q_PROPERTY
(
QString
serialNumber
READ
serialNumber
NOTIFY
info
Changed
)
Q_PROPERTY
(
QString
fwVersion
READ
fwVersion
NOTIFY
info
Changed
)
Q_PROPERTY
(
QStringList
decodeList
READ
decodeList
NOTIFY
decodeIndexChanged
)
Q_PROPERTY
(
int
decodeIndex
READ
decodeIndex
WRITE
setDecodeIndex
NOTIFY
decodeIndexChanged
)
Q_PROPERTY
(
QStringList
rateList
READ
rateList
NOTIFY
rateIndexChanged
)
...
...
@@ -61,12 +61,15 @@ public:
signals:
void
linkChanged
();
void
linkConnectedChanged
();
void
infoChanged
();
void
connectedChanged
();
void
decodeIndexChanged
();
void
rateIndexChanged
();
private
slots
:
void
_connected
();
void
_disconnected
();
void
_checkTaisync
();
void
_updateSettings
(
QByteArray
jSonData
);
void
_setEnabled
();
...
...
@@ -76,6 +79,10 @@ private slots:
void
_readTelemBytes
(
QByteArray
bytesIn
);
#endif
private:
void
_close
();
void
_reset
();
private:
enum
{
...
...
@@ -83,6 +90,7 @@ private:
REQ_DEV_INFO
,
REQ_FREQ_SCAN
,
REQ_VIDEO_SETTINGS
,
REQ_RADIO_SETTINGS
,
REQ_LAST
};
...
...
src/Taisync/TaisyncSettings.cc
View file @
58fd244a
...
...
@@ -44,47 +44,43 @@ bool TaisyncSettings::start()
bool
TaisyncSettings
::
requestLinkStatus
()
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/baseband.json"
);
//qCDebug(TaisyncVerbose) << "Request" << req;
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
}
return
false
;
return
_request
(
"/v1/baseband.json"
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestDevInfo
()
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/device.json"
);
//qCDebug(TaisyncVerbose) << "Request" << req;
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
}
return
false
;
return
_request
(
"/v1/device.json"
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestFreqScan
()
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/freqscan.json"
);
//qCDebug(TaisyncVerbose) << "Request" << req;
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
}
return
false
;
return
_request
(
"/v1/freqscan.json"
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestVideoSettings
()
{
return
_request
(
"/v1/video.json"
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestRadioSettings
()
{
return
_request
(
"/v1/radio.json"
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
_request
(
const
QString
&
request
)
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/video.json"
);
QString
req
=
QString
(
kGetReq
).
arg
(
request
);
//qCDebug(TaisyncVerbose) << "Request" << req;
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
...
...
src/Taisync/TaisyncSettings.h
View file @
58fd244a
...
...
@@ -21,6 +21,7 @@ public:
bool
requestDevInfo
();
bool
requestFreqScan
();
bool
requestVideoSettings
();
bool
requestRadioSettings
();
signals:
void
updateSettings
(
QByteArray
jSonData
);
...
...
@@ -28,4 +29,6 @@ signals:
protected
slots
:
void
_readBytes
()
override
;
private:
bool
_request
(
const
QString
&
request
);
};
src/Taisync/TaisyncSettings.qml
View file @
58fd244a
...
...
@@ -39,7 +39,7 @@ QGCView {
property
real
_panelWidth
:
_qgcView
.
width
*
_internalWidthRatio
property
Fact
_taisyncEnabledFact
:
QGroundControl
.
settingsManager
.
appSettings
.
enableTaisync
property
Fact
_taisyncVideoEnabledFact
:
QGroundControl
.
settingsManager
.
appSettings
.
enableTaisyncVideo
property
bool
_taisyncEnabled
:
_taisync
Video
EnabledFact
.
rawValue
property
bool
_taisyncEnabled
:
_taisyncEnabledFact
.
rawValue
readonly
property
real
_internalWidthRatio
:
0.8
...
...
src/Taisync/TaisyncTelemetry.cc
View file @
58fd244a
...
...
@@ -12,9 +12,6 @@
#include "QGCApplication.h"
#include "VideoManager.h"
QGC_LOGGING_CATEGORY
(
TaisyncTelemetryLog
,
"TaisyncTelemetryLog"
)
//-----------------------------------------------------------------------------
TaisyncTelemetry
::
TaisyncTelemetry
(
QObject
*
parent
)
:
TaisyncHandler
(
parent
)
...
...
@@ -22,18 +19,33 @@ TaisyncTelemetry::TaisyncTelemetry(QObject* parent)
}
//-----------------------------------------------------------------------------
void
bool
TaisyncTelemetry
::
close
()
{
TaisyncHandler
::
close
();
qCDebug
(
TaisyncTelemetryLog
)
<<
"Close Taisync Telemetry"
;
if
(
_mavlinkChannel
>=
0
)
{
qgcApp
()
->
toolbox
()
->
linkManager
()
->
_freeMavlinkChannel
(
_mavlinkChannel
);
_mavlinkChannel
=
-
1
;
}
if
(
TaisyncHandler
::
close
())
{
qCDebug
(
TaisyncLog
)
<<
"Close Taisync Telemetry"
;
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
bool
TaisyncTelemetry
::
start
()
{
qCDebug
(
TaisyncTelemetryLog
)
<<
"Start Taisync Telemetry"
;
return
_start
(
TAISYNC_TELEM_PORT
);
qCDebug
(
TaisyncLog
)
<<
"Start Taisync Telemetry"
;
if
(
_start
(
TAISYNC_TELEM_PORT
))
{
_mavlinkChannel
=
qgcApp
()
->
toolbox
()
->
linkManager
()
->
_reserveMavlinkChannel
();
_heartbeatTimer
.
setInterval
(
1000
);
_heartbeatTimer
.
setSingleShot
(
false
);
connect
(
&
_heartbeatTimer
,
&
QTimer
::
timeout
,
this
,
&
TaisyncTelemetry
::
_sendGCSHeartbeat
);
_heartbeatTimer
.
start
();
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
...
...
@@ -50,15 +62,43 @@ void
TaisyncTelemetry
::
_newConnection
()
{
TaisyncHandler
::
_newConnection
();
qCDebug
(
Taisync
Telemetry
Log
)
<<
"New Taisync Temeletry Connection"
;
qCDebug
(
TaisyncLog
)
<<
"New Taisync Temeletry Connection"
;
}
//-----------------------------------------------------------------------------
void
TaisyncTelemetry
::
_readBytes
()
{
_heartbeatTimer
.
stop
();
while
(
_tcpSocket
->
bytesAvailable
())
{
QByteArray
bytesIn
=
_tcpSocket
->
read
(
_tcpSocket
->
bytesAvailable
());
emit
bytesReady
(
bytesIn
);
}
}
//-----------------------------------------------------------------------------
void
TaisyncTelemetry
::
_sendGCSHeartbeat
()
{
//-- TODO: This is temporary. We should not have to send out heartbeats for a link to start.
if
(
_tcpSocket
)
{
MAVLinkProtocol
*
pMavlinkProtocol
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
if
(
pMavlinkProtocol
&&
_mavlinkChannel
>=
0
)
{
qCDebug
(
TaisyncLog
)
<<
"Taisync heartbeat out"
;
mavlink_message_t
message
;
mavlink_msg_heartbeat_pack_chan
(
static_cast
<
uint8_t
>
(
pMavlinkProtocol
->
getSystemId
()),
static_cast
<
uint8_t
>
(
pMavlinkProtocol
->
getComponentId
()),
static_cast
<
uint8_t
>
(
_mavlinkChannel
),
&
message
,
MAV_TYPE_GCS
,
// MAV_TYPE
MAV_AUTOPILOT_INVALID
,
// MAV_AUTOPILOT
MAV_MODE_MANUAL_ARMED
,
// MAV_MODE
0
,
// custom mode
MAV_STATE_ACTIVE
);
// MAV_STATE
uint8_t
buffer
[
MAVLINK_MAX_PACKET_LEN
];
int
len
=
mavlink_msg_to_send_buffer
(
buffer
,
&
message
);
_tcpSocket
->
write
(
reinterpret_cast
<
const
char
*>
(
buffer
),
len
);
}
}
}
src/Taisync/TaisyncTelemetry.h
View file @
58fd244a
...
...
@@ -13,15 +13,13 @@
#include <QUdpSocket>
#include <QTimer>
Q_DECLARE_LOGGING_CATEGORY
(
TaisyncTelemetryLog
)
class
TaisyncTelemetry
:
public
TaisyncHandler
{
Q_OBJECT
public:
explicit
TaisyncTelemetry
(
QObject
*
parent
=
nullptr
);
void
close
()
override
;
bool
close
()
override
;
bool
start
()
override
;
void
writeBytes
(
QByteArray
bytes
);
...
...
@@ -31,5 +29,10 @@ signals:
private
slots
:
void
_newConnection
()
override
;
void
_readBytes
()
override
;
void
_sendGCSHeartbeat
();
private:
int
_mavlinkChannel
=
-
1
;
QTimer
_heartbeatTimer
;
};
src/Taisync/TaisyncVideoReceiver.cc
View file @
58fd244a
...
...
@@ -12,9 +12,6 @@
#include "QGCApplication.h"
#include "VideoManager.h"
QGC_LOGGING_CATEGORY
(
TaisyncVideoReceiverLog
,
"TaisyncVideoReceiverLog"
)
//-----------------------------------------------------------------------------
TaisyncVideoReceiver
::
TaisyncVideoReceiver
(
QObject
*
parent
)
:
TaisyncHandler
(
parent
)
...
...
@@ -22,31 +19,39 @@ TaisyncVideoReceiver::TaisyncVideoReceiver(QObject* parent)
}
//-----------------------------------------------------------------------------
void
bool
TaisyncVideoReceiver
::
close
()
{
TaisyncHandler
::
close
();
qCDebug
(
TaisyncVideoReceiverLog
)
<<
"Close Taisync Video Receiver"
;
if
(
_udpVideoSocket
)
{
_udpVideoSocket
->
close
();
_udpVideoSocket
->
deleteLater
();
_udpVideoSocket
=
nullptr
;
if
(
TaisyncHandler
::
close
()
||
_udpVideoSocket
)
{
qCDebug
(
TaisyncLog
)
<<
"Close Taisync Video Receiver"
;
if
(
_udpVideoSocket
)
{
_udpVideoSocket
->
close
();
_udpVideoSocket
->
deleteLater
();
_udpVideoSocket
=
nullptr
;
}
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
bool
TaisyncVideoReceiver
::
start
()
{
qCDebug
(
TaisyncVideoReceiverLog
)
<<
"Start Taisync Video Receiver"
;
_udpVideoSocket
=
new
QUdpSocket
(
this
);
return
_start
(
TAISYNC_VIDEO_TCP_PORT
);
qCDebug
(
TaisyncLog
)
<<
"Start Taisync Video Receiver"
;
if
(
_start
(
TAISYNC_VIDEO_TCP_PORT
))
{
_udpVideoSocket
=
new
QUdpSocket
(
this
);
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
void
TaisyncVideoReceiver
::
_readBytes
()
{
QByteArray
bytesIn
=
_tcpSocket
->
read
(
_tcpSocket
->
bytesAvailable
());
_udpVideoSocket
->
writeDatagram
(
bytesIn
,
QHostAddress
::
LocalHost
,
TAISYNC_VIDEO_UDP_PORT
);
if
(
_udpVideoSocket
)
{
QByteArray
bytesIn
=
_tcpSocket
->
read
(
_tcpSocket
->
bytesAvailable
());
_udpVideoSocket
->
writeDatagram
(
bytesIn
,
QHostAddress
::
LocalHost
,
TAISYNC_VIDEO_UDP_PORT
);
}
}
src/Taisync/TaisyncVideoReceiver.h
View file @
58fd244a
...
...
@@ -12,8 +12,6 @@
#include "TaisyncHandler.h"
#include <QUdpSocket>
Q_DECLARE_LOGGING_CATEGORY
(
TaisyncVideoReceiverLog
)
class
TaisyncVideoReceiver
:
public
TaisyncHandler
{
Q_OBJECT
...
...
@@ -21,7 +19,7 @@ public:
explicit
TaisyncVideoReceiver
(
QObject
*
parent
=
nullptr
);
bool
start
()
override
;
void
close
()
override
;
bool
close
()
override
;
private
slots
:
void
_readBytes
()
override
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment