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
bab3c35a
Commit
bab3c35a
authored
Dec 14, 2018
by
Gus Grubba
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Get (and expose) link status
parent
6b405306
Changes
6
Hide whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
109 additions
and
1 deletion
+109
-1
TaisyncHandler.cc
src/Taisync/TaisyncHandler.cc
+1
-0
TaisyncHandler.h
src/Taisync/TaisyncHandler.h
+3
-0
TaisyncSettings.cc
src/Taisync/TaisyncSettings.cc
+52
-0
TaisyncSettings.h
src/Taisync/TaisyncSettings.h
+20
-0
TaisyncLink.cc
src/comm/TaisyncLink.cc
+28
-1
TaisyncLink.h
src/comm/TaisyncLink.h
+5
-0
No files found.
src/Taisync/TaisyncHandler.cc
View file @
bab3c35a
...
...
@@ -60,6 +60,7 @@ TaisyncHandler::_newConnection()
}
_tcpSocket
=
_tcpServer
->
nextPendingConnection
();
QObject
::
connect
(
_tcpSocket
,
&
QIODevice
::
readyRead
,
this
,
&
TaisyncHandler
::
_readBytes
);
emit
connected
();
}
//-----------------------------------------------------------------------------
...
...
src/Taisync/TaisyncHandler.h
View file @
bab3c35a
...
...
@@ -39,6 +39,9 @@ protected slots:
virtual
void
_socketDisconnected
();
virtual
void
_readBytes
()
=
0
;
signals:
void
connected
();
protected:
QTcpServer
*
_tcpServer
=
nullptr
;
QTcpSocket
*
_tcpSocket
=
nullptr
;
...
...
src/Taisync/TaisyncSettings.cc
View file @
bab3c35a
...
...
@@ -15,6 +15,14 @@
QGC_LOGGING_CATEGORY
(
TaisyncSettingsLog
,
"TaisyncSettingsLog"
)
static
const
char
*
kPostReq
=
"POST %1 HTTP/1.1
\r\n
"
"Content-Type: application/json
\r\n
"
"Content-Length: %2
\r\n\r\n
"
"%3"
;
static
const
char
*
kGetReq
=
"GET %1 HTTP/1.1
\r\n\r\n
"
;
//-----------------------------------------------------------------------------
TaisyncSettings
::
TaisyncSettings
(
QObject
*
parent
)
:
TaisyncHandler
(
parent
)
...
...
@@ -29,11 +37,55 @@ TaisyncSettings::start()
_start
(
TAISYNC_SETTINGS_PORT
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestSettings
()
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/baseband.json"
);
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestFreqScan
()
{
if
(
_tcpSocket
)
{
QString
req
=
QString
(
kGetReq
).
arg
(
"/v1/freqscan.json"
);
_tcpSocket
->
write
(
req
.
toUtf8
());
return
true
;
}
return
false
;
}
//-----------------------------------------------------------------------------
void
TaisyncSettings
::
_readBytes
()
{
QByteArray
bytesIn
=
_tcpSocket
->
read
(
_tcpSocket
->
bytesAvailable
());
qCDebug
(
TaisyncSettingsLog
)
<<
"Taisync settings data:"
<<
bytesIn
.
size
();
qCDebug
(
TaisyncSettingsLog
)
<<
QString
(
bytesIn
);
if
(
bytesIn
.
contains
(
"200 OK"
))
{
//-- Link Status?
int
idx
=
bytesIn
.
indexOf
(
'{'
);
QJsonParseError
jsonParseError
;
QJsonDocument
doc
=
QJsonDocument
::
fromJson
(
bytesIn
.
mid
(
idx
),
&
jsonParseError
);
if
(
jsonParseError
.
error
!=
QJsonParseError
::
NoError
)
{
qWarning
()
<<
"Unable to parse Taisync response:"
<<
jsonParseError
.
errorString
()
<<
jsonParseError
.
offset
;
return
;
}
QJsonObject
jObj
=
doc
.
object
();
//-- Link Status?
if
(
bytesIn
.
contains
(
"
\"
flight
\"
:"
))
{
_linkConnected
=
jObj
[
"flight"
].
toBool
(
_linkConnected
);
_linkVidFormat
=
jObj
[
"videoformat"
].
toString
(
_linkVidFormat
);
_downlinkRSSI
=
jObj
[
"radiorssi"
].
toInt
(
_downlinkRSSI
);
_uplinkRSSI
=
jObj
[
"hdrssi"
].
toInt
(
_uplinkRSSI
);
emit
linkChanged
();
}
}
}
src/Taisync/TaisyncSettings.h
View file @
bab3c35a
...
...
@@ -18,10 +18,30 @@ class TaisyncSettings : public TaisyncHandler
Q_OBJECT
public:
Q_PROPERTY
(
bool
linkConnected
READ
linkConnected
NOTIFY
linkChanged
)
Q_PROPERTY
(
QString
linkVidFormat
READ
linkVidFormat
NOTIFY
linkChanged
)
Q_PROPERTY
(
int
uplinkRSSI
READ
uplinkRSSI
NOTIFY
linkChanged
)
Q_PROPERTY
(
int
downlinkRSSI
READ
downlinkRSSI
NOTIFY
linkChanged
)
explicit
TaisyncSettings
(
QObject
*
parent
=
nullptr
);
void
start
()
override
;
bool
requestSettings
();
bool
requestFreqScan
();
bool
linkConnected
()
{
return
_linkConnected
;
}
QString
linkVidFormat
()
{
return
_linkVidFormat
;
}
int
uplinkRSSI
()
{
return
_downlinkRSSI
;
}
int
downlinkRSSI
()
{
return
_uplinkRSSI
;
}
signals:
void
linkChanged
();
protected
slots
:
void
_readBytes
()
override
;
private:
bool
_linkConnected
=
false
;
QString
_linkVidFormat
;
int
_downlinkRSSI
=
0
;
int
_uplinkRSSI
=
0
;
};
src/comm/TaisyncLink.cc
View file @
bab3c35a
...
...
@@ -50,6 +50,7 @@ TaisyncLink::~TaisyncLink()
void
TaisyncLink
::
run
()
{
// Thread
if
(
_hardwareConnect
())
{
exec
();
}
...
...
@@ -96,9 +97,11 @@ TaisyncLink::_readBytes(QByteArray bytes)
void
TaisyncLink
::
_disconnect
()
{
//-- Stop thread
_running
=
false
;
quit
();
wait
();
//-- Kill Taisync handlers
if
(
_taiTelemetery
)
{
_hardwareDisconnect
();
emit
disconnected
();
...
...
@@ -129,6 +132,7 @@ TaisyncLink::_connect(void)
if
(
_taiConfig
->
videoEnabled
())
{
//-- 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
();
...
...
@@ -137,6 +141,7 @@ TaisyncLink::_connect(void)
//-- iOS and Android receive raw h.264 and need a different pipeline
qgcApp
()
->
toolbox
()
->
videoManager
()
->
setIsTaisync
(
true
);
#endif
//-- 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
);
...
...
@@ -167,6 +172,7 @@ TaisyncLink::_hardwareDisconnect()
_taiVideo
=
nullptr
;
}
#endif
_connected
=
false
;
}
//-----------------------------------------------------------------------------
...
...
@@ -176,9 +182,11 @@ TaisyncLink::_hardwareConnect()
_hardwareDisconnect
();
_taiTelemetery
=
new
TaisyncTelemetry
(
this
);
QObject
::
connect
(
_taiTelemetery
,
&
TaisyncTelemetry
::
bytesReady
,
this
,
&
TaisyncLink
::
_readBytes
);
QObject
::
connect
(
_taiTelemetery
,
&
TaisyncTelemetry
::
connected
,
this
,
&
TaisyncLink
::
_telemetryReady
);
_taiTelemetery
->
start
();
_taiSettings
=
new
TaisyncSettings
(
this
);
_taiSettings
->
start
();
QObject
::
connect
(
_taiSettings
,
&
TaisyncSettings
::
connected
,
this
,
&
TaisyncLink
::
_settingsReady
);
#if defined(__ios__) || defined(__android__)
if
(
_taiConfig
->
videoEnabled
())
{
_taiVideo
=
new
TaisyncVideoReceiver
(
this
);
...
...
@@ -192,7 +200,7 @@ TaisyncLink::_hardwareConnect()
bool
TaisyncLink
::
isConnected
()
const
{
return
_
taiTelemetery
!=
nullptr
;
return
_
connected
;
}
//-----------------------------------------------------------------------------
...
...
@@ -216,6 +224,25 @@ TaisyncLink::getCurrentOutDataRate() const
return
0
;
}
//-----------------------------------------------------------------------------
void
TaisyncLink
::
_telemetryReady
()
{
qCDebug
(
TaisyncLog
)
<<
"Taisync telemetry ready"
;
if
(
!
_connected
)
{
_connected
=
true
;
emit
connected
();
}
}
//-----------------------------------------------------------------------------
void
TaisyncLink
::
_settingsReady
()
{
qCDebug
(
TaisyncLog
)
<<
"Taisync settings ready"
;
_taiSettings
->
requestSettings
();
}
//--------------------------------------------------------------------------
//-- TaisyncConfiguration
...
...
src/comm/TaisyncLink.h
View file @
bab3c35a
...
...
@@ -81,6 +81,8 @@ public:
bool
isConnected
()
const
override
;
QString
getName
()
const
override
;
TaisyncSettings
*
taisyncSettings
()
{
return
_taiSettings
;
}
// Extensive statistics for scientific purposes
qint64
getConnectionSpeed
()
const
override
;
qint64
getCurrentInDataRate
()
const
;
...
...
@@ -97,6 +99,8 @@ public:
private
slots
:
void
_writeBytes
(
const
QByteArray
data
)
override
;
void
_readBytes
(
QByteArray
bytes
);
void
_telemetryReady
();
void
_settingsReady
();
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
...
...
@@ -121,6 +125,7 @@ private:
TaisyncVideoReceiver
*
_taiVideo
=
nullptr
;
#endif
bool
_savedVideoState
=
true
;
bool
_connected
=
false
;
QVariant
_savedVideoSource
;
QVariant
_savedVideoUDP
;
QVariant
_savedAR
;
...
...
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