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
d4bff450
Unverified
Commit
d4bff450
authored
Oct 19, 2020
by
Don Gagne
Committed by
GitHub
Oct 19, 2020
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Param crash (#9124)
* Remove unneeded mutex * Cleanup LinkInterface, fix connect/disconnect bug
parent
e7d29a87
Changes
17
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
31 additions
and
69 deletions
+31
-69
ADSBVehicleManager.cc
src/ADSB/ADSBVehicleManager.cc
+1
-0
ParameterManager.cc
src/FactSystem/ParameterManager.cc
+0
-14
ParameterManager.h
src/FactSystem/ParameterManager.h
+0
-2
MultiVehicleManager.cc
src/Vehicle/MultiVehicleManager.cc
+2
-2
VehicleLinkManager.cc
src/Vehicle/VehicleLinkManager.cc
+1
-1
BluetoothLink.cc
src/comm/BluetoothLink.cc
+1
-6
BluetoothLink.h
src/comm/BluetoothLink.h
+2
-3
LinkInterface.h
src/comm/LinkInterface.h
+0
-2
LinkManager.cc
src/comm/LinkManager.cc
+6
-1
LogReplayLink.h
src/comm/LogReplayLink.h
+3
-4
MockLink.h
src/comm/MockLink.h
+2
-3
SerialLink.cc
src/comm/SerialLink.cc
+4
-9
SerialLink.h
src/comm/SerialLink.h
+2
-3
TCPLink.cc
src/comm/TCPLink.cc
+3
-8
TCPLink.h
src/comm/TCPLink.h
+2
-3
UDPLink.cc
src/comm/UDPLink.cc
+0
-5
UDPLink.h
src/comm/UDPLink.h
+2
-3
No files found.
src/ADSB/ADSBVehicleManager.cc
View file @
d4bff450
...
...
@@ -83,6 +83,7 @@ ADSBTCPLink::ADSBTCPLink(const QString& hostAddress, int port, QObject* parent)
ADSBTCPLink
::~
ADSBTCPLink
(
void
)
{
if
(
_socket
)
{
QObject
::
disconnect
(
_socket
,
&
QTcpSocket
::
readyRead
,
this
,
&
ADSBTCPLink
::
_readBytes
);
_socket
->
disconnectFromHost
();
_socket
->
deleteLater
();
_socket
=
nullptr
;
...
...
src/FactSystem/ParameterManager.cc
View file @
d4bff450
...
...
@@ -390,8 +390,6 @@ void ParameterManager::_handleParamValue(int componentId, QString parameterName,
emit
factAdded
(
componentId
,
fact
);
}
_dataMutex
.
unlock
();
fact
->
_containerSetRawValue
(
parameterValue
);
// Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params
...
...
@@ -416,8 +414,6 @@ void ParameterManager::_handleParamValue(int componentId, QString parameterName,
/// Writes the parameter update to mavlink, sets up for write wait
void
ParameterManager
::
_factRawValueUpdateWorker
(
int
componentId
,
const
QString
&
name
,
FactMetaData
::
ValueType_t
valueType
,
const
QVariant
&
rawValue
)
{
_dataMutex
.
lock
();
if
(
_waitingWriteParamNameMap
.
contains
(
componentId
))
{
if
(
_waitingWriteParamNameMap
[
componentId
].
contains
(
name
))
{
_waitingWriteParamNameMap
[
componentId
].
remove
(
name
);
...
...
@@ -432,8 +428,6 @@ void ParameterManager::_factRawValueUpdateWorker(int componentId, const QString&
qWarning
()
<<
"Internal error ParameterManager::_factValueUpdateWorker: component id not found"
<<
componentId
;
}
_dataMutex
.
unlock
();
_sendParamSetToVehicle
(
componentId
,
name
,
valueType
,
rawValue
);
qCDebug
(
ParameterManagerLog
)
<<
_logVehiclePrefix
(
componentId
)
<<
"Update parameter (_waitingParamTimeoutTimer started) - compId:name:rawValue"
<<
componentId
<<
name
<<
rawValue
;
}
...
...
@@ -465,8 +459,6 @@ void ParameterManager::refreshAllParameters(uint8_t componentId)
emit
missingParametersChanged
(
_missingParameters
);
}
_dataMutex
.
lock
();
if
(
!
_initialLoadComplete
)
{
_initialRequestTimeoutTimer
.
start
();
}
...
...
@@ -482,8 +474,6 @@ void ParameterManager::refreshAllParameters(uint8_t componentId)
}
}
_dataMutex
.
unlock
();
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
mavlink_message_t
msg
;
...
...
@@ -517,8 +507,6 @@ void ParameterManager::refreshParameter(int componentId, const QString& paramNam
componentId
=
_actualComponentId
(
componentId
);
qCDebug
(
ParameterManagerLog
)
<<
_logVehiclePrefix
(
componentId
)
<<
"refreshParameter - name:"
<<
paramName
<<
")"
;
_dataMutex
.
lock
();
if
(
_waitingReadParamNameMap
.
contains
(
componentId
))
{
QString
mappedParamName
=
_remapParamNameToVersion
(
paramName
);
...
...
@@ -535,8 +523,6 @@ void ParameterManager::refreshParameter(int componentId, const QString& paramNam
qWarning
()
<<
"Internal error"
;
}
_dataMutex
.
unlock
();
_readParameterRaw
(
componentId
,
paramName
,
-
1
);
}
...
...
src/FactSystem/ParameterManager.h
View file @
d4bff450
...
...
@@ -181,8 +181,6 @@ private:
QTimer
_initialRequestTimeoutTimer
;
QTimer
_waitingParamTimeoutTimer
;
QMutex
_dataMutex
;
Fact
_defaultFact
;
///< Used to return default fact, when parameter not found
static
const
char
*
_jsonParametersKey
;
...
...
src/Vehicle/MultiVehicleManager.cc
View file @
d4bff450
...
...
@@ -83,7 +83,7 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
if
(
vehicleId
!=
81
||
componentId
!=
50
)
{
// Don't create vehicles for components other than the autopilot
qCDebug
(
MultiVehicleManagerLog
())
<<
"Ignoring heartbeat from unknown component port:vehicleId:componentId:fwType:vehicleType"
<<
link
->
getN
ame
()
<<
link
->
linkConfiguration
()
->
n
ame
()
<<
vehicleId
<<
componentId
<<
vehicleFirmwareType
...
...
@@ -112,7 +112,7 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
}
qCDebug
(
MultiVehicleManagerLog
())
<<
"Adding new vehicle link:vehicleId:componentId:vehicleFirmwareType:vehicleType "
<<
link
->
getN
ame
()
<<
link
->
linkConfiguration
()
->
n
ame
()
<<
vehicleId
<<
componentId
<<
vehicleFirmwareType
...
...
src/Vehicle/VehicleLinkManager.cc
View file @
d4bff450
...
...
@@ -171,7 +171,7 @@ void VehicleLinkManager::_addLink(LinkInterface* link)
qCWarning
(
VehicleLinkManagerLog
)
<<
"_addLink call with link which is already in the list"
;
return
;
}
else
{
qCDebug
(
VehicleLinkManagerLog
)
<<
"_addLink:"
<<
link
->
getN
ame
()
<<
QString
(
"%1"
).
arg
((
qulonglong
)
link
,
0
,
16
);
qCDebug
(
VehicleLinkManagerLog
)
<<
"_addLink:"
<<
link
->
linkConfiguration
()
->
n
ame
()
<<
QString
(
"%1"
).
arg
((
qulonglong
)
link
,
0
,
16
);
link
->
addVehicleReference
();
...
...
src/comm/BluetoothLink.cc
View file @
d4bff450
...
...
@@ -47,11 +47,6 @@ void BluetoothLink::run()
}
QString
BluetoothLink
::
getName
()
const
{
return
_config
->
name
();
}
void
BluetoothLink
::
_writeBytes
(
const
QByteArray
bytes
)
{
if
(
_targetSocket
)
{
...
...
@@ -87,7 +82,7 @@ void BluetoothLink::disconnect(void)
#endif
if
(
_targetSocket
)
{
// This prevents stale signals from calling the link after it has been deleted
QObject
::
connect
(
_targetSocket
,
&
QBluetoothSocket
::
readyRead
,
this
,
&
BluetoothLink
::
readBytes
);
QObject
::
dis
connect
(
_targetSocket
,
&
QBluetoothSocket
::
readyRead
,
this
,
&
BluetoothLink
::
readBytes
);
_targetSocket
->
deleteLater
();
_targetSocket
=
nullptr
;
emit
disconnected
();
...
...
src/comm/BluetoothLink.h
View file @
d4bff450
...
...
@@ -129,9 +129,8 @@ public:
void
run
(
void
)
override
;
// LinkConfiguration overrides
bool
isConnected
(
void
)
const
override
;
QString
getName
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
public
slots
:
void
readBytes
(
void
);
...
...
src/comm/LinkInterface.h
View file @
d4bff450
...
...
@@ -52,7 +52,6 @@ public:
SharedLinkConfigurationPtr
linkConfiguration
(
void
)
{
return
_config
;
}
Q_INVOKABLE
virtual
QString
getName
(
void
)
const
=
0
;
Q_INVOKABLE
virtual
void
disconnect
(
void
)
=
0
;
virtual
bool
isConnected
(
void
)
const
=
0
;
...
...
@@ -70,7 +69,6 @@ signals:
void
bytesSent
(
LinkInterface
*
link
,
QByteArray
data
);
void
connected
(
void
);
void
disconnected
(
void
);
void
nameChanged
(
QString
name
);
void
communicationError
(
const
QString
&
title
,
const
QString
&
error
);
protected:
...
...
src/comm/LinkManager.cc
View file @
d4bff450
...
...
@@ -198,10 +198,15 @@ void LinkManager::_linkDisconnected(void)
return
;
}
disconnect
(
link
,
&
LinkInterface
::
communicationError
,
_app
,
&
QGCApplication
::
criticalMessageBoxOnMainThread
);
disconnect
(
link
,
&
LinkInterface
::
bytesReceived
,
_mavlinkProtocol
,
&
MAVLinkProtocol
::
receiveBytes
);
disconnect
(
link
,
&
LinkInterface
::
bytesSent
,
_mavlinkProtocol
,
&
MAVLinkProtocol
::
logSentBytes
);
disconnect
(
link
,
&
LinkInterface
::
disconnected
,
this
,
&
LinkManager
::
_linkDisconnected
);
_freeMavlinkChannel
(
link
->
mavlinkChannel
());
for
(
int
i
=
0
;
i
<
_rgLinks
.
count
();
i
++
)
{
if
(
_rgLinks
[
i
].
get
()
==
link
)
{
qCDebug
(
LinkManagerLog
)
<<
"LinkManager::_linkDisconnected"
<<
_rgLinks
[
i
]
->
getN
ame
()
<<
_rgLinks
[
i
].
use_count
();
qCDebug
(
LinkManagerLog
)
<<
"LinkManager::_linkDisconnected"
<<
_rgLinks
[
i
]
->
linkConfiguration
()
->
n
ame
()
<<
_rgLinks
[
i
].
use_count
();
_rgLinks
.
removeAt
(
i
);
return
;
}
...
...
src/comm/LogReplayLink.h
View file @
d4bff450
...
...
@@ -65,10 +65,9 @@ public:
void
movePlayhead
(
qreal
percentComplete
);
// overrides from LinkInterface
QString
getName
(
void
)
const
override
{
return
_config
->
name
();
}
bool
isConnected
(
void
)
const
override
{
return
_connected
;
}
bool
isLogReplay
(
void
)
override
{
return
true
;
}
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
{
return
_connected
;
}
bool
isLogReplay
(
void
)
override
{
return
true
;
}
void
disconnect
(
void
)
override
;
public
slots
:
/// Sets the acceleration factor: -100: 0.01X, 0: 1.0X, 100: 100.0X
...
...
src/comm/MockLink.h
View file @
d4bff450
...
...
@@ -117,9 +117,8 @@ public:
MockLinkFTP
*
mockLinkFTP
(
void
)
{
return
_mockLinkFTP
;
}
// Overrides from LinkInterface
QString
getName
(
void
)
const
override
{
return
_name
;
}
bool
isConnected
(
void
)
const
override
{
return
_connected
;
}
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
{
return
_connected
;
}
void
disconnect
(
void
)
override
;
/// Sets a failure mode for unit testingqgcm
/// @param failureMode Type of failure to simulate
...
...
src/comm/SerialLink.cc
View file @
d4bff450
...
...
@@ -72,7 +72,7 @@ void SerialLink::_writeBytes(const QByteArray data)
}
else
{
// Error occurred
qWarning
()
<<
"Serial port not writeable"
;
_emitLinkError
(
tr
(
"Could not send data - link %1 is disconnected!"
).
arg
(
getN
ame
()));
_emitLinkError
(
tr
(
"Could not send data - link %1 is disconnected!"
).
arg
(
_config
->
n
ame
()));
}
}
...
...
@@ -202,7 +202,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
}
#endif
if
(
!
_port
->
isOpen
()
)
{
qDebug
()
<<
"open failed"
<<
_port
->
errorString
()
<<
_port
->
error
()
<<
getN
ame
()
<<
"autconnect:"
<<
_config
->
isAutoConnect
();
qDebug
()
<<
"open failed"
<<
_port
->
errorString
()
<<
_port
->
error
()
<<
_config
->
n
ame
()
<<
"autconnect:"
<<
_config
->
isAutoConnect
();
error
=
_port
->
error
();
errorString
=
_port
->
errorString
();
_port
->
close
();
...
...
@@ -241,7 +241,7 @@ void SerialLink::_readBytes(void)
}
else
{
// Error occurred
qWarning
()
<<
"Serial port not readable"
;
_emitLinkError
(
tr
(
"Could not read data - link %1 is disconnected!"
).
arg
(
getN
ame
()));
_emitLinkError
(
tr
(
"Could not read data - link %1 is disconnected!"
).
arg
(
_config
->
n
ame
()));
}
}
...
...
@@ -275,16 +275,11 @@ bool SerialLink::isConnected() const
return
isConnected
;
}
QString
SerialLink
::
getName
()
const
{
return
_serialConfig
->
name
();
}
void
SerialLink
::
_emitLinkError
(
const
QString
&
errorMsg
)
{
QString
msg
(
"Error on link %1. %2"
);
qDebug
()
<<
errorMsg
;
emit
communicationError
(
tr
(
"Link Error"
),
msg
.
arg
(
getN
ame
()).
arg
(
errorMsg
));
emit
communicationError
(
tr
(
"Link Error"
),
msg
.
arg
(
_config
->
n
ame
()).
arg
(
errorMsg
));
}
//--------------------------------------------------------------------------
...
...
src/comm/SerialLink.h
View file @
d4bff450
...
...
@@ -115,9 +115,8 @@ class SerialLink : public LinkInterface
public:
// LinkInterface overrides
QString
getName
(
void
)
const
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
/// Don't even think of calling this method!
QSerialPort
*
_hackAccessToPort
(
void
)
{
return
_port
;
}
...
...
src/comm/TCPLink.cc
View file @
d4bff450
...
...
@@ -107,7 +107,7 @@ void TCPLink::disconnect(void)
wait
();
if
(
_socket
)
{
// This prevents stale signal from calling the link after it has been deleted
QObject
::
connect
(
_socket
,
&
QTcpSocket
::
readyRead
,
this
,
&
TCPLink
::
readBytes
);
QObject
::
dis
connect
(
_socket
,
&
QTcpSocket
::
readyRead
,
this
,
&
TCPLink
::
readBytes
);
_socketIsConnected
=
false
;
_socket
->
disconnectFromHost
();
// Disconnect tcp
_socket
->
waitForDisconnected
();
...
...
@@ -155,7 +155,7 @@ bool TCPLink::_hardwareConnect()
// Whether a failed connection emits an error signal or not is platform specific.
// So in cases where it is not emitted, we emit one ourselves.
if
(
errorSpy
.
count
()
==
0
)
{
emit
communicationError
(
tr
(
"Link Error"
),
tr
(
"Error on link %1. Connection failed"
).
arg
(
getN
ame
()));
emit
communicationError
(
tr
(
"Link Error"
),
tr
(
"Error on link %1. Connection failed"
).
arg
(
_config
->
n
ame
()));
}
delete
_socket
;
_socket
=
nullptr
;
...
...
@@ -169,7 +169,7 @@ bool TCPLink::_hardwareConnect()
void
TCPLink
::
_socketError
(
QAbstractSocket
::
SocketError
socketError
)
{
Q_UNUSED
(
socketError
);
emit
communicationError
(
tr
(
"Link Error"
),
tr
(
"Error on link %1. Error on socket: %2."
).
arg
(
getN
ame
()).
arg
(
_socket
->
errorString
()));
emit
communicationError
(
tr
(
"Link Error"
),
tr
(
"Error on link %1. Error on socket: %2."
).
arg
(
_config
->
n
ame
()).
arg
(
_socket
->
errorString
()));
}
/**
...
...
@@ -182,11 +182,6 @@ bool TCPLink::isConnected() const
return
_socketIsConnected
;
}
QString
TCPLink
::
getName
()
const
{
return
_tcpConfig
->
name
();
}
void
TCPLink
::
waitForBytesWritten
(
int
msecs
)
{
Q_ASSERT
(
_socket
);
...
...
src/comm/TCPLink.h
View file @
d4bff450
...
...
@@ -82,9 +82,8 @@ public:
void
signalBytesWritten
(
void
);
// LinkInterface overrides
QString
getName
(
void
)
const
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
public
slots
:
void
waitForBytesWritten
(
int
msecs
);
...
...
src/comm/UDPLink.cc
View file @
d4bff450
...
...
@@ -112,11 +112,6 @@ void UDPLink::run()
}
}
QString
UDPLink
::
getName
()
const
{
return
_udpConfig
->
name
();
}
bool
UDPLink
::
_isIpLocal
(
const
QHostAddress
&
add
)
{
// In simulation and testing setups the vehicle and the GCS can be
...
...
src/comm/UDPLink.h
View file @
d4bff450
...
...
@@ -105,9 +105,8 @@ public:
~
UDPLink
();
// LinkInterface overrides
bool
isConnected
(
void
)
const
override
;
QString
getName
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
bool
isConnected
(
void
)
const
override
;
void
disconnect
(
void
)
override
;
// QThread overrides
void
run
(
void
)
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