Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
df27e66b
Commit
df27e66b
authored
Dec 25, 2018
by
Gus Grubba
Browse files
IP and RTSP Settings
Remove telemetry heartbeats
parent
c6a366bf
Changes
6
Hide whitespace changes
Inline
Side-by-side
src/Taisync/TaisyncManager.cc
View file @
df27e66b
...
...
@@ -168,14 +168,39 @@ TaisyncManager::setToolbox(QGCToolbox* toolbox)
_reset
();
}
//-----------------------------------------------------------------------------
bool
TaisyncManager
::
setRTSPSettings
(
QString
uri
,
QString
account
,
QString
password
)
{
if
(
_taiSettings
)
{
return
_taiSettings
->
setRTSPSettings
(
uri
,
account
,
password
);
}
return
false
;
}
//-----------------------------------------------------------------------------
bool
TaisyncManager
::
setIPSettings
(
QString
localIP
,
QString
remoteIP
,
QString
netMask
)
{
#if !defined(__ios__) && !defined(__android__)
if
(
_taiSettings
)
{
return
_taiSettings
->
setIPSettings
(
localIP
,
remoteIP
,
netMask
);
}
#endif
return
false
;
}
//-----------------------------------------------------------------------------
void
TaisyncManager
::
_radioSettingsChanged
(
QVariant
)
{
if
(
_taiSettings
)
{
_workTimer
.
stop
();
_taiSettings
->
setRadioSettings
(
_radioModeList
[
_radioMode
->
rawValue
().
toInt
()],
_radioChannel
->
enumStringValue
());
_reqMask
|=
REQ_RADIO_SETTINGS
;
_workTimer
.
start
(
3000
);
}
}
...
...
@@ -184,10 +209,13 @@ void
TaisyncManager
::
_videoSettingsChanged
(
QVariant
)
{
if
(
_taiSettings
)
{
_workTimer
.
stop
();
_taiSettings
->
setVideoSettings
(
_videoOutputList
[
_videoOutput
->
rawValue
().
toInt
()],
_videoMode
->
enumStringValue
(),
_videoRateList
[
_videoRate
->
rawValue
().
toInt
()]);
_reqMask
|=
REQ_VIDEO_SETTINGS
;
_workTimer
.
start
(
500
);
}
}
...
...
@@ -209,6 +237,7 @@ TaisyncManager::_setEnabled()
_taiTelemetery
->
start
();
}
#endif
_reqMask
=
REQ_ALL
;
_workTimer
.
start
(
1000
);
}
else
{
//-- Stop everything
...
...
@@ -326,24 +355,38 @@ TaisyncManager::_checkTaisync()
_taiSettings
->
start
();
}
}
else
{
if
(
++
_currReq
>=
REQ_LAST
)
{
_currReq
=
REQ_LINK_STATUS
;
}
switch
(
_currReq
)
{
case
REQ_LINK_STATUS
:
_taiSettings
->
requestLinkStatus
();
break
;
case
REQ_DEV_INFO
:
_taiSettings
->
requestDevInfo
();
break
;
case
REQ_FREQ_SCAN
:
_taiSettings
->
requestFreqScan
();
break
;
case
REQ_VIDEO_SETTINGS
:
_taiSettings
->
requestVideoSettings
();
break
;
case
REQ_RADIO_SETTINGS
:
_taiSettings
->
requestRadioSettings
();
while
(
true
)
{
if
(
_reqMask
&
REQ_LINK_STATUS
)
{
_taiSettings
->
requestLinkStatus
();
break
;
}
if
(
_reqMask
&
REQ_DEV_INFO
)
{
_taiSettings
->
requestDevInfo
();
break
;
}
if
(
_reqMask
&
REQ_FREQ_SCAN
)
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_FREQ_SCAN
);
_taiSettings
->
requestFreqScan
();
break
;
}
if
(
_reqMask
&
REQ_VIDEO_SETTINGS
)
{
_taiSettings
->
requestVideoSettings
();
break
;
}
if
(
_reqMask
&
REQ_RADIO_SETTINGS
)
{
_taiSettings
->
requestRadioSettings
();
break
;
}
if
(
_reqMask
&
REQ_RTSP_SETTINGS
)
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_RTSP_SETTINGS
);
_taiSettings
->
requestRTSPURISettings
();
break
;
}
if
(
_reqMask
&
REQ_IP_SETTINGS
)
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_IP_SETTINGS
);
_taiSettings
->
requestIPSettings
();
break
;
}
break
;
}
}
...
...
@@ -365,6 +408,7 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
QJsonObject
jObj
=
doc
.
object
();
//-- Link Status?
if
(
jSonData
.
contains
(
"
\"
flight
\"
:"
))
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_LINK_STATUS
);
bool
tlinkConnected
=
jObj
[
"flight"
].
toString
(
""
)
==
"online"
;
if
(
tlinkConnected
!=
_linkConnected
)
{
_linkConnected
=
tlinkConnected
;
...
...
@@ -381,6 +425,7 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
}
//-- Device Info?
}
else
if
(
jSonData
.
contains
(
"
\"
firmwareversion
\"
:"
))
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_DEV_INFO
);
QString
tfwVersion
=
jObj
[
"firmwareversion"
].
toString
(
_fwVersion
);
QString
tserialNumber
=
jObj
[
"sn"
].
toString
(
_serialNumber
);
if
(
tfwVersion
!=
_fwVersion
||
tserialNumber
!=
_serialNumber
)
{
...
...
@@ -390,6 +435,7 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
}
//-- Radio Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
freq
\"
:"
))
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_RADIO_SETTINGS
);
int
idx
=
_radioModeList
.
indexOf
(
jObj
[
"mode"
].
toString
(
_radioMode
->
enumStringValue
()));
if
(
idx
>=
0
)
_radioMode
->
_containerSetRawValue
(
idx
);
idx
=
_radioChannel
->
valueIndex
(
jObj
[
"freq"
].
toString
(
_radioChannel
->
enumStringValue
()));
...
...
@@ -397,6 +443,7 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
_radioChannel
->
_containerSetRawValue
(
idx
);
//-- Video Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
maxbitrate
\"
:"
))
{
_reqMask
|=
~
static_cast
<
uint32_t
>
(
REQ_VIDEO_SETTINGS
);
int
idx
;
idx
=
_videoMode
->
valueIndex
(
jObj
[
"mode"
].
toString
(
_videoMode
->
enumStringValue
()));
if
(
idx
<
0
)
idx
=
0
;
...
...
@@ -405,5 +452,41 @@ TaisyncManager::_updateSettings(QByteArray jSonData)
if
(
idx
>=
0
)
_videoRate
->
_containerSetRawValue
(
idx
);
idx
=
_videoOutputList
.
indexOf
(
jObj
[
"decode"
].
toString
(
_videoOutput
->
enumStringValue
()));
if
(
idx
>=
0
)
_videoOutput
->
_containerSetRawValue
(
idx
);
//-- IP Address Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
usbEthIp
\"
:"
))
{
QString
value
;
value
=
jObj
[
"ipaddr"
].
toString
(
_localIPAddr
);
if
(
value
!=
_localIPAddr
)
{
_localIPAddr
=
value
;
emit
localIPAddrChanged
();
}
value
=
jObj
[
"netmask"
].
toString
(
_netMask
);
if
(
value
!=
_netMask
)
{
_netMask
=
value
;
emit
netMaskChanged
();
}
value
=
jObj
[
"usbEthIp"
].
toString
(
_remoteIPAddr
);
if
(
value
!=
_remoteIPAddr
)
{
_remoteIPAddr
=
value
;
emit
remoteIPAddrChanged
();
}
//-- RTSP URI Settings?
}
else
if
(
jSonData
.
contains
(
"
\"
rtspURI
\"
:"
))
{
QString
value
;
value
=
jObj
[
"rtspURI"
].
toString
(
_rtspURI
);
if
(
value
!=
_rtspURI
)
{
_rtspURI
=
value
;
emit
rtspURIChanged
();
}
value
=
jObj
[
"account"
].
toString
(
_rtspAccount
);
if
(
value
!=
_rtspAccount
)
{
_rtspAccount
=
value
;
emit
rtspAccountChanged
();
}
value
=
jObj
[
"passwd"
].
toString
(
_rtspPassword
);
if
(
value
!=
_rtspPassword
)
{
_rtspPassword
=
value
;
emit
rtspPasswordChanged
();
}
}
}
src/Taisync/TaisyncManager.h
View file @
df27e66b
...
...
@@ -41,6 +41,15 @@ public:
Q_PROPERTY
(
Fact
*
videoOutput
READ
videoOutput
CONSTANT
)
Q_PROPERTY
(
Fact
*
videoMode
READ
videoMode
CONSTANT
)
Q_PROPERTY
(
Fact
*
videoRate
READ
videoRate
CONSTANT
)
Q_PROPERTY
(
QString
rtspURI
READ
rtspURI
NOTIFY
rtspURIChanged
)
Q_PROPERTY
(
QString
rtspAccount
READ
rtspAccount
NOTIFY
rtspAccountChanged
)
Q_PROPERTY
(
QString
rtspPassword
READ
rtspPassword
NOTIFY
rtspPasswordChanged
)
Q_PROPERTY
(
QString
localIPAddr
READ
localIPAddr
NOTIFY
localIPAddrChanged
)
Q_PROPERTY
(
QString
remoteIPAddr
READ
remoteIPAddr
NOTIFY
remoteIPAddrChanged
)
Q_PROPERTY
(
QString
netMask
READ
netMask
NOTIFY
netMaskChanged
)
Q_INVOKABLE
bool
setRTSPSettings
(
QString
uri
,
QString
account
,
QString
password
);
Q_INVOKABLE
bool
setIPSettings
(
QString
localIP
,
QString
remoteIP
,
QString
netMask
);
explicit
TaisyncManager
(
QGCApplication
*
app
,
QGCToolbox
*
toolbox
);
~
TaisyncManager
()
override
;
...
...
@@ -59,6 +68,12 @@ public:
Fact
*
videoOutput
()
{
return
_videoOutput
;
}
Fact
*
videoMode
()
{
return
_videoMode
;
}
Fact
*
videoRate
()
{
return
_videoRate
;
}
QString
rtspURI
()
{
return
_rtspURI
;
}
QString
rtspAccount
()
{
return
_rtspAccount
;
}
QString
rtspPassword
()
{
return
_rtspPassword
;
}
QString
localIPAddr
()
{
return
_localIPAddr
;
}
QString
remoteIPAddr
()
{
return
_remoteIPAddr
;
}
QString
netMask
()
{
return
_netMask
;
}
signals:
void
linkChanged
();
...
...
@@ -67,6 +82,12 @@ signals:
void
connectedChanged
();
void
decodeIndexChanged
();
void
rateIndexChanged
();
void
rtspURIChanged
();
void
rtspAccountChanged
();
void
rtspPasswordChanged
();
void
localIPAddrChanged
();
void
remoteIPAddrChanged
();
void
netMaskChanged
();
private
slots
:
void
_connected
();
...
...
@@ -90,15 +111,17 @@ private:
private:
enum
{
REQ_LINK_STATUS
,
REQ_DEV_INFO
,
REQ_FREQ_SCAN
,
REQ_VIDEO_SETTINGS
,
REQ_RADIO_SETTINGS
,
REQ_LAST
REQ_LINK_STATUS
=
1
,
REQ_DEV_INFO
=
2
,
REQ_FREQ_SCAN
=
4
,
REQ_VIDEO_SETTINGS
=
8
,
REQ_RADIO_SETTINGS
=
16
,
REQ_RTSP_SETTINGS
=
32
,
REQ_IP_SETTINGS
=
64
,
REQ_ALL
=
0xFFFFFFFF
,
};
int
_
currReq
=
REQ_
LAST
;
u
int
32_t
_
reqMask
=
REQ_
ALL
;
bool
_running
=
false
;
bool
_isConnected
=
false
;
AppSettings
*
_appSettings
=
nullptr
;
...
...
@@ -134,4 +157,10 @@ private:
QStringList
_radioModeList
;
QStringList
_videoOutputList
;
QStringList
_videoRateList
;
QString
_rtspURI
;
QString
_rtspAccount
;
QString
_rtspPassword
;
QString
_localIPAddr
;
QString
_remoteIPAddr
;
QString
_netMask
;
};
src/Taisync/TaisyncSettings.cc
View file @
df27e66b
...
...
@@ -22,6 +22,10 @@ static const char* kPostReq =
static
const
char
*
kGetReq
=
"GET %1 HTTP/1.1
\r\n\r\n
"
;
static
const
char
*
kRadioURI
=
"/v1/radio.json"
;
static
const
char
*
kVideoURI
=
"/v1/video.json"
;
static
const
char
*
kRTSPURI
=
"/v1/rtspuri.json"
;
#if !defined(__ios__) && !defined(__android__)
static
const
char
*
kIPAddrURI
=
"/v1/ipaddr.json"
;
#endif
//-----------------------------------------------------------------------------
TaisyncSettings
::
TaisyncSettings
(
QObject
*
parent
)
...
...
@@ -75,6 +79,22 @@ TaisyncSettings::requestRadioSettings()
return
_request
(
kRadioURI
);
}
//-----------------------------------------------------------------------------
#if !defined(__ios__) && !defined(__android__)
bool
TaisyncSettings
::
requestIPSettings
()
{
return
_request
(
kIPAddrURI
);
}
#endif
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
requestRTSPURISettings
()
{
return
_request
(
kRTSPURI
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
_request
(
const
QString
&
request
)
...
...
@@ -119,6 +139,26 @@ TaisyncSettings::setVideoSettings(const QString& output, const QString& mode, co
return
_post
(
kVideoURI
,
post
);
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings
::
setRTSPSettings
(
const
QString
&
uri
,
const
QString
&
account
,
const
QString
&
password
)
{
static
const
char
*
kRTSPPost
=
"{
\"
rtspURI
\"
:
\"
%1
\"
,
\"
account
\"
:
\"
%2
\"
,
\"
passwd
\"
:
\"
%3
\"
}"
;
QString
post
=
QString
(
kRTSPPost
).
arg
(
uri
).
arg
(
account
).
arg
(
password
);
return
_post
(
kRTSPURI
,
post
);
}
//-----------------------------------------------------------------------------
#if !defined(__ios__) && !defined(__android__)
bool
TaisyncSettings
::
setIPSettings
(
const
QString
&
localIP
,
const
QString
&
remoteIP
,
const
QString
&
netMask
)
{
static
const
char
*
kRTSPPost
=
"{
\"
ipaddr
\"
:
\"
%1
\"
,
\"
netmask
\"
:
\"
%2
\"
,
\"
usbEthIp
\"
:
\"
%3
\"
}"
;
QString
post
=
QString
(
kRTSPPost
).
arg
(
localIP
).
arg
(
netMask
).
arg
(
remoteIP
);
return
_post
(
kIPAddrURI
,
post
);
}
#endif
//-----------------------------------------------------------------------------
void
TaisyncSettings
::
_readBytes
()
...
...
src/Taisync/TaisyncSettings.h
View file @
df27e66b
...
...
@@ -22,8 +22,16 @@ public:
bool
requestFreqScan
();
bool
requestVideoSettings
();
bool
requestRadioSettings
();
#if !defined(__ios__) && !defined(__android__)
bool
requestIPSettings
();
#endif
bool
requestRTSPURISettings
();
bool
setRadioSettings
(
const
QString
&
mode
,
const
QString
&
channel
);
bool
setVideoSettings
(
const
QString
&
output
,
const
QString
&
mode
,
const
QString
&
rate
);
bool
setRTSPSettings
(
const
QString
&
uri
,
const
QString
&
account
,
const
QString
&
password
);
#if !defined(__ios__) && !defined(__android__)
bool
setIPSettings
(
const
QString
&
localIP
,
const
QString
&
remoteIP
,
const
QString
&
netMask
);
#endif
signals:
void
updateSettings
(
QByteArray
jSonData
);
...
...
src/Taisync/TaisyncTelemetry.cc
View file @
df27e66b
...
...
@@ -22,10 +22,6 @@ TaisyncTelemetry::TaisyncTelemetry(QObject* parent)
bool
TaisyncTelemetry
::
close
()
{
if
(
_mavlinkChannel
>=
0
)
{
qgcApp
()
->
toolbox
()
->
linkManager
()
->
_freeMavlinkChannel
(
_mavlinkChannel
);
_mavlinkChannel
=
-
1
;
}
if
(
TaisyncHandler
::
close
())
{
qCDebug
(
TaisyncLog
)
<<
"Close Taisync Telemetry"
;
return
true
;
...
...
@@ -37,15 +33,7 @@ TaisyncTelemetry::close()
bool
TaisyncTelemetry
::
start
()
{
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
;
return
_start
(
TAISYNC_TELEM_PORT
);
}
//-----------------------------------------------------------------------------
...
...
@@ -69,36 +57,8 @@ TaisyncTelemetry::_newConnection()
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 @
df27e66b
...
...
@@ -29,10 +29,5 @@ signals:
private
slots
:
void
_newConnection
()
override
;
void
_readBytes
()
override
;
void
_sendGCSHeartbeat
();
private:
int
_mavlinkChannel
=
-
1
;
QTimer
_heartbeatTimer
;
};
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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