Skip to content

GitLab

Commit 19eb13dc authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Proper multithreading on all comm threads

parent 85a1e606
Expand all Hide whitespace changes
Inline Side-by-side
src/comm/QGCFlightGearLink.cc
View file @ 19eb13dc
This diff is collapsed.
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 = this->hardwareConnect();
bool connected = true;
start(HighPriority);
return connected;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment