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
19eb13dc
Commit
19eb13dc
authored
Apr 21, 2014
by
Lorenz Meier
Browse files
Proper multithreading on all comm threads
parent
85a1e606
Changes
5
Expand all
Hide whitespace changes
Inline
Side-by-side
src/comm/QGCFlightGearLink.cc
View file @
19eb13dc
This diff is collapsed.
Click to expand it.
src/comm/QGCJSBSimLink.cc
View file @
19eb13dc
...
...
@@ -71,6 +71,89 @@ QGCJSBSimLink::~QGCJSBSimLink()
**/
void
QGCJSBSimLink
::
run
()
{
qDebug
()
<<
"STARTING FLIGHTGEAR LINK"
;
if
(
!
mav
)
return
;
socket
=
new
QUdpSocket
(
this
);
connectState
=
socket
->
bind
(
host
,
port
);
QObject
::
connect
(
socket
,
SIGNAL
(
readyRead
()),
this
,
SLOT
(
readBytes
()));
process
=
new
QProcess
(
this
);
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
{
uas
->
startHil
();
}
//connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
// Catch process error
QObject
::
connect
(
process
,
SIGNAL
(
error
(
QProcess
::
ProcessError
)),
this
,
SLOT
(
processError
(
QProcess
::
ProcessError
)));
// Start Flightgear
QStringList
arguments
;
QString
processJSB
;
QString
rootJSB
;
#ifdef Q_OS_MACX
processJSB
=
"/usr/local/bin/JSBSim"
;
rootJSB
=
"/Applications/FlightGear.app/Contents/Resources/data"
;
#endif
#ifdef Q_OS_WIN32
processJSB
=
"C:
\\
Program Files (x86)
\\
FlightGear
\\
bin
\\
Win32
\\
fgfs"
;
rootJSB
=
"C:
\\
Program Files (x86)
\\
FlightGear
\\
data"
;
#endif
#ifdef Q_OS_LINUX
processJSB
=
"/usr/games/fgfs"
;
rootJSB
=
"/usr/share/games/flightgear"
;
#endif
// Sanity checks
bool
sane
=
true
;
QFileInfo
executable
(
processJSB
);
if
(
!
executable
.
isExecutable
())
{
MainWindow
::
instance
()
->
showCriticalMessage
(
tr
(
"JSBSim Failed to Start"
),
tr
(
"JSBSim was not found at %1"
).
arg
(
processJSB
));
sane
=
false
;
}
QFileInfo
root
(
rootJSB
);
if
(
!
root
.
isDir
())
{
MainWindow
::
instance
()
->
showCriticalMessage
(
tr
(
"JSBSim Failed to Start"
),
tr
(
"JSBSim data directory was not found at %1"
).
arg
(
rootJSB
));
sane
=
false
;
}
if
(
!
sane
)
return
;
/*Prepare JSBSim Arguments */
if
(
mav
->
getSystemType
()
==
MAV_TYPE_QUADROTOR
)
{
arguments
<<
QString
(
"--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s"
).
arg
(
rootJSB
).
arg
(
rootJSB
).
arg
(
script
);
}
else
{
arguments
<<
QString
(
"JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s"
).
arg
(
rootJSB
).
arg
(
rootJSB
).
arg
(
script
);
}
process
->
start
(
processJSB
,
arguments
);
emit
simulationConnected
(
connectState
);
if
(
connectState
)
{
emit
simulationConnected
();
connectionStartTime
=
QGC
::
groundTimeUsecs
()
/
1000
;
}
qDebug
()
<<
"STARTING SIM"
;
exec
();
}
...
...
@@ -303,91 +386,8 @@ bool QGCJSBSimLink::disconnectSimulation()
**/
bool
QGCJSBSimLink
::
connectSimulation
()
{
qDebug
()
<<
"STARTING FLIGHTGEAR LINK"
;
if
(
!
mav
)
return
false
;
socket
=
new
QUdpSocket
(
this
);
connectState
=
socket
->
bind
(
host
,
port
);
QObject
::
connect
(
socket
,
SIGNAL
(
readyRead
()),
this
,
SLOT
(
readBytes
()));
process
=
new
QProcess
(
this
);
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
{
uas
->
startHil
();
}
//connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
// Catch process error
QObject
::
connect
(
process
,
SIGNAL
(
error
(
QProcess
::
ProcessError
)),
this
,
SLOT
(
processError
(
QProcess
::
ProcessError
)));
// Start Flightgear
QStringList
arguments
;
QString
processJSB
;
QString
rootJSB
;
#ifdef Q_OS_MACX
processJSB
=
"/usr/local/bin/JSBSim"
;
rootJSB
=
"/Applications/FlightGear.app/Contents/Resources/data"
;
#endif
#ifdef Q_OS_WIN32
processJSB
=
"C:
\\
Program Files (x86)
\\
FlightGear
\\
bin
\\
Win32
\\
fgfs"
;
rootJSB
=
"C:
\\
Program Files (x86)
\\
FlightGear
\\
data"
;
#endif
#ifdef Q_OS_LINUX
processJSB
=
"/usr/games/fgfs"
;
rootJSB
=
"/usr/share/games/flightgear"
;
#endif
// Sanity checks
bool
sane
=
true
;
QFileInfo
executable
(
processJSB
);
if
(
!
executable
.
isExecutable
())
{
MainWindow
::
instance
()
->
showCriticalMessage
(
tr
(
"JSBSim Failed to Start"
),
tr
(
"JSBSim was not found at %1"
).
arg
(
processJSB
));
sane
=
false
;
}
QFileInfo
root
(
rootJSB
);
if
(
!
root
.
isDir
())
{
MainWindow
::
instance
()
->
showCriticalMessage
(
tr
(
"JSBSim Failed to Start"
),
tr
(
"JSBSim data directory was not found at %1"
).
arg
(
rootJSB
));
sane
=
false
;
}
if
(
!
sane
)
return
false
;
/*Prepare JSBSim Arguments */
if
(
mav
->
getSystemType
()
==
MAV_TYPE_QUADROTOR
)
{
arguments
<<
QString
(
"--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s"
).
arg
(
rootJSB
).
arg
(
rootJSB
).
arg
(
script
);
}
else
{
arguments
<<
QString
(
"JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s"
).
arg
(
rootJSB
).
arg
(
rootJSB
).
arg
(
script
);
}
process
->
start
(
processJSB
,
arguments
);
emit
simulationConnected
(
connectState
);
if
(
connectState
)
{
emit
simulationConnected
();
connectionStartTime
=
QGC
::
groundTimeUsecs
()
/
1000
;
}
qDebug
()
<<
"STARTING SIM"
;
start
(
LowPriority
);
return
connectState
;
start
(
HighPriority
);
return
true
;
}
/**
...
...
src/comm/QGCXPlaneLink.cc
View file @
19eb13dc
...
...
@@ -144,9 +144,72 @@ void QGCXPlaneLink::setVersion(unsigned int version)
**/
void
QGCXPlaneLink
::
run
()
{
forever
{
QGC
::
SLEEP
::
usleep
(
500000
);
if
(
!
mav
)
return
;
if
(
connectState
)
return
;
socket
=
new
QUdpSocket
(
this
);
connectState
=
socket
->
bind
(
localHost
,
localPort
);
if
(
!
connectState
)
return
;
QObject
::
connect
(
socket
,
SIGNAL
(
readyRead
()),
this
,
SLOT
(
readBytes
()));
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
mav
,
SIGNAL
(
hilActuatorsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateActuators
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilGroundTruthChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilGroundTruth
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)));
connect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
{
uas
->
startHil
();
}
#pragma pack(push, 1)
struct
iset_struct
{
char
b
[
5
];
int
index
;
// (0->20 in the lsit below)
char
str_ipad_them
[
16
];
char
str_port_them
[
6
];
char
padding
[
2
];
int
use_ip
;
}
ip
;
// to use this option, 0 not to.
#pragma pack(pop)
ip
.
b
[
0
]
=
'I'
;
ip
.
b
[
1
]
=
'S'
;
ip
.
b
[
2
]
=
'E'
;
ip
.
b
[
3
]
=
'T'
;
ip
.
b
[
4
]
=
'0'
;
QList
<
QHostAddress
>
hostAddresses
=
QNetworkInterface
::
allAddresses
();
QString
localAddrStr
;
QString
localPortStr
=
QString
(
"%1"
).
arg
(
localPort
);
for
(
int
i
=
0
;
i
<
hostAddresses
.
size
();
i
++
)
{
// Exclude loopback IPv4 and all IPv6 addresses
if
(
hostAddresses
.
at
(
i
)
!=
QHostAddress
(
"127.0.0.1"
)
&&
!
hostAddresses
.
at
(
i
).
toString
().
contains
(
":"
))
{
localAddrStr
=
hostAddresses
.
at
(
i
).
toString
();
break
;
}
}
//qDebug() << "REQ SEND TO:" << localAddrStr << localPortStr;
ip
.
index
=
0
;
strncpy
(
ip
.
str_ipad_them
,
localAddrStr
.
toAscii
(),
qMin
((
int
)
sizeof
(
ip
.
str_ipad_them
),
16
));
strncpy
(
ip
.
str_port_them
,
localPortStr
.
toAscii
(),
qMin
((
int
)
sizeof
(
ip
.
str_port_them
),
6
));
ip
.
use_ip
=
1
;
writeBytes
((
const
char
*
)
&
ip
,
sizeof
(
ip
));
exec
();
}
void
QGCXPlaneLink
::
setPort
(
int
localPort
)
...
...
@@ -945,73 +1008,9 @@ bool QGCXPlaneLink::connectSimulation()
// XXX Hack
storeSettings
();
start
(
LowPriority
);
if
(
!
mav
)
return
false
;
if
(
connectState
)
return
false
;
start
(
HighPriority
);
socket
=
new
QUdpSocket
(
this
);
connectState
=
socket
->
bind
(
localHost
,
localPort
);
if
(
!
connectState
)
return
false
;
QObject
::
connect
(
socket
,
SIGNAL
(
readyRead
()),
this
,
SLOT
(
readBytes
()));
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
mav
,
SIGNAL
(
hilActuatorsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateActuators
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilGroundTruthChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilGroundTruth
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)));
connect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
{
uas
->
startHil
();
}
#pragma pack(push, 1)
struct
iset_struct
{
char
b
[
5
];
int
index
;
// (0->20 in the lsit below)
char
str_ipad_them
[
16
];
char
str_port_them
[
6
];
char
padding
[
2
];
int
use_ip
;
}
ip
;
// to use this option, 0 not to.
#pragma pack(pop)
ip
.
b
[
0
]
=
'I'
;
ip
.
b
[
1
]
=
'S'
;
ip
.
b
[
2
]
=
'E'
;
ip
.
b
[
3
]
=
'T'
;
ip
.
b
[
4
]
=
'0'
;
QList
<
QHostAddress
>
hostAddresses
=
QNetworkInterface
::
allAddresses
();
QString
localAddrStr
;
QString
localPortStr
=
QString
(
"%1"
).
arg
(
localPort
);
for
(
int
i
=
0
;
i
<
hostAddresses
.
size
();
i
++
)
{
// Exclude loopback IPv4 and all IPv6 addresses
if
(
hostAddresses
.
at
(
i
)
!=
QHostAddress
(
"127.0.0.1"
)
&&
!
hostAddresses
.
at
(
i
).
toString
().
contains
(
":"
))
{
localAddrStr
=
hostAddresses
.
at
(
i
).
toString
();
break
;
}
}
//qDebug() << "REQ SEND TO:" << localAddrStr << localPortStr;
ip
.
index
=
0
;
strncpy
(
ip
.
str_ipad_them
,
localAddrStr
.
toAscii
(),
qMin
((
int
)
sizeof
(
ip
.
str_ipad_them
),
16
));
strncpy
(
ip
.
str_port_them
,
localPortStr
.
toAscii
(),
qMin
((
int
)
sizeof
(
ip
.
str_port_them
),
6
));
ip
.
use_ip
=
1
;
writeBytes
((
const
char
*
)
&
ip
,
sizeof
(
ip
));
return
connectState
;
return
true
;
}
/**
...
...
src/comm/TCPLink.cc
View file @
19eb13dc
...
...
@@ -61,6 +61,8 @@ TCPLink::~TCPLink()
void
TCPLink
::
run
()
{
_hardwareConnect
();
exec
();
}
...
...
@@ -214,11 +216,10 @@ bool TCPLink::connect()
quit
();
wait
();
}
bool
connected
=
_hardwareConnect
();
if
(
connected
)
{
start
(
HighPriority
);
}
return
connected
;
start
(
HighPriority
);
return
true
;
}
bool
TCPLink
::
_hardwareConnect
(
void
)
...
...
src/comm/UDPLink.cc
View file @
19eb13dc
...
...
@@ -69,9 +69,9 @@ UDPLink::~UDPLink()
**/
void
UDPLink
::
run
()
{
forever
{
QGC
::
SLEEP
::
usleep
(
500000
);
}
hardwareConnect
();
exec
();
}
void
UDPLink
::
setAddress
(
QHostAddress
host
)
...
...
@@ -307,7 +307,7 @@ bool UDPLink::connect()
this
->
quit
();
this
->
wait
();
}
bool
connected
=
t
his
->
hardwareConnect
()
;
bool
connected
=
t
rue
;
start
(
HighPriority
);
return
connected
;
}
...
...
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