2010-08-31 12:17:14 +02:00
|
|
|
/**
|
|
|
|
******************************************************************************
|
|
|
|
*
|
|
|
|
* @file simulator.cpp
|
|
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
|
|
* @addtogroup GCSPlugins GCS Plugins
|
|
|
|
* @{
|
|
|
|
* @addtogroup HITLPlugin HITL Plugin
|
|
|
|
* @{
|
|
|
|
* @brief The Hardware In The Loop plugin
|
|
|
|
*****************************************************************************/
|
|
|
|
/*
|
|
|
|
* This program is free software; you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License as published by
|
|
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This program is distributed in the hope that it will be useful, but
|
|
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
|
|
* for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License along
|
|
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
#include "simulator.h"
|
|
|
|
#include "qxtlogger.h"
|
|
|
|
#include "extensionsystem/pluginmanager.h"
|
|
|
|
#include "coreplugin/icore.h"
|
|
|
|
#include "coreplugin/threadmanager.h"
|
|
|
|
|
|
|
|
volatile bool Simulator::isStarted = false;
|
|
|
|
|
|
|
|
Simulator::Simulator(const SimulatorSettings& params) :
|
|
|
|
simProcess(NULL),
|
|
|
|
time(NULL),
|
|
|
|
inSocket(NULL),
|
|
|
|
outSocket(NULL),
|
|
|
|
settings(params),
|
|
|
|
updatePeriod(50),
|
|
|
|
simTimeout(2000),
|
|
|
|
autopilotConnectionStatus(false),
|
|
|
|
simConnectionStatus(false),
|
|
|
|
txTimer(NULL),
|
|
|
|
simTimer(NULL),
|
|
|
|
name("")
|
|
|
|
{
|
|
|
|
// move to thread
|
|
|
|
moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
|
|
|
|
connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection);
|
|
|
|
emit myStart();
|
|
|
|
}
|
|
|
|
|
|
|
|
Simulator::~Simulator()
|
|
|
|
{
|
|
|
|
if(inSocket)
|
|
|
|
{
|
|
|
|
delete inSocket;
|
|
|
|
inSocket = NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(outSocket)
|
|
|
|
{
|
|
|
|
delete outSocket;
|
|
|
|
outSocket = NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(txTimer)
|
|
|
|
{
|
|
|
|
delete txTimer;
|
|
|
|
txTimer = NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(simTimer)
|
|
|
|
{
|
|
|
|
delete simTimer;
|
|
|
|
simTimer = NULL;
|
|
|
|
}
|
|
|
|
// NOTE: Does not currently work, may need to send control+c to through the terminal
|
|
|
|
if (simProcess != NULL)
|
|
|
|
{
|
|
|
|
//connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus)));
|
|
|
|
|
|
|
|
simProcess->disconnect();
|
|
|
|
if(simProcess->state() == QProcess::Running)
|
|
|
|
simProcess->kill();
|
|
|
|
//if(simProcess->waitForFinished())
|
|
|
|
//emit deleteSimProcess();
|
|
|
|
delete simProcess;
|
|
|
|
simProcess = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::onDeleteSimulator(void)
|
|
|
|
{
|
|
|
|
// [1]
|
|
|
|
Simulator::setStarted(false);
|
|
|
|
// [2]
|
|
|
|
Simulator::Instances().removeOne(simulatorId);
|
|
|
|
|
|
|
|
disconnect(this);
|
|
|
|
delete this;
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::onStart()
|
|
|
|
{
|
|
|
|
QMutexLocker locker(&lock);
|
|
|
|
|
2010-08-31 14:21:06 +02:00
|
|
|
QThread* mainThread = QThread::currentThread();
|
2010-08-31 12:17:14 +02:00
|
|
|
qDebug() << "Simulator Thread: "<< mainThread;
|
|
|
|
|
|
|
|
// Get required UAVObjects
|
|
|
|
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
|
|
|
|
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
|
|
|
|
actDesired = ActuatorDesired::GetInstance(objManager);
|
2010-08-31 14:21:06 +02:00
|
|
|
altActual = BaroAltitude::GetInstance(objManager);
|
2010-08-31 12:17:14 +02:00
|
|
|
attActual = AttitudeActual::GetInstance(objManager);
|
2010-09-26 05:06:27 +02:00
|
|
|
gpsPos = GPSPosition::GetInstance(objManager);
|
2010-08-31 12:17:14 +02:00
|
|
|
telStats = GCSTelemetryStats::GetInstance(objManager);
|
|
|
|
|
|
|
|
// Listen to autopilot connection events
|
|
|
|
TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
|
|
|
|
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
|
|
|
|
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
|
|
|
|
//connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*)));
|
|
|
|
|
|
|
|
// If already connect setup autopilot
|
|
|
|
GCSTelemetryStats::DataFields stats = telStats->getData();
|
|
|
|
if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED )
|
|
|
|
onAutopilotConnect();
|
|
|
|
|
|
|
|
inSocket = new QUdpSocket();
|
|
|
|
outSocket = new QUdpSocket();
|
|
|
|
setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort);
|
|
|
|
|
|
|
|
emit processOutput("\nHost: " + settings.hostAddress + "\n" + \
|
|
|
|
"inputPort: " + QString::number(settings.inPort) + "\n" + \
|
|
|
|
"outpurPort: " + QString::number(settings.outPort) + "\n");
|
|
|
|
|
|
|
|
qxtLog->info("\nHost: " + settings.hostAddress + "\n" + \
|
|
|
|
"inputPort: " + QString::number(settings.inPort) + "\n" + \
|
|
|
|
"outpurPort: " + QString::number(settings.outPort) + "\n");
|
|
|
|
|
|
|
|
// if(!inSocket->waitForConnected(5000))
|
|
|
|
// processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort));
|
|
|
|
//outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG
|
|
|
|
//if(!outSocket->waitForConnected(5000))
|
|
|
|
// processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort));
|
|
|
|
|
|
|
|
|
|
|
|
connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()),Qt::DirectConnection);
|
|
|
|
|
|
|
|
// Setup transmit timer
|
|
|
|
txTimer = new QTimer();
|
|
|
|
connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()),Qt::DirectConnection);
|
|
|
|
txTimer->setInterval(updatePeriod);
|
|
|
|
txTimer->start();
|
|
|
|
// Setup simulator connection timer
|
|
|
|
simTimer = new QTimer();
|
|
|
|
connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()),Qt::DirectConnection);
|
|
|
|
simTimer->setInterval(simTimeout);
|
|
|
|
simTimer->start();
|
|
|
|
|
|
|
|
// setup time
|
|
|
|
time = new QTime();
|
|
|
|
time->start();
|
|
|
|
current.T=0;
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::receiveUpdate()
|
|
|
|
{
|
|
|
|
// Update connection timer and status
|
|
|
|
simTimer->setInterval(simTimeout);
|
|
|
|
simTimer->stop();
|
|
|
|
simTimer->start();
|
|
|
|
if ( !simConnectionStatus )
|
|
|
|
{
|
|
|
|
simConnectionStatus = true;
|
|
|
|
emit simulatorConnected();
|
|
|
|
}
|
|
|
|
|
|
|
|
// Process data
|
2010-09-19 14:37:41 +02:00
|
|
|
while(inSocket->hasPendingDatagrams()) {
|
2010-08-31 12:17:14 +02:00
|
|
|
// Receive datagram
|
|
|
|
QByteArray datagram;
|
|
|
|
datagram.resize(inSocket->pendingDatagramSize());
|
|
|
|
QHostAddress sender;
|
|
|
|
quint16 senderPort;
|
|
|
|
inSocket->readDatagram(datagram.data(), datagram.size(),
|
|
|
|
&sender, &senderPort);
|
2010-09-23 17:16:45 +02:00
|
|
|
//QString datastr(datagram);
|
2010-08-31 12:17:14 +02:00
|
|
|
// Process incomming data
|
2010-09-23 17:16:45 +02:00
|
|
|
processUpdate(datagram);
|
2010-08-31 12:17:14 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::setupObjects()
|
|
|
|
{
|
|
|
|
setupInputObject(actDesired, 75);
|
|
|
|
setupOutputObject(altActual, 250);
|
|
|
|
setupOutputObject(attActual, 75);
|
2010-09-26 05:06:27 +02:00
|
|
|
setupOutputObject(gpsPos, 250);
|
2010-08-31 12:17:14 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::setupInputObject(UAVObject* obj, int updatePeriod)
|
|
|
|
{
|
|
|
|
UAVObject::Metadata mdata;
|
|
|
|
mdata = obj->getDefaultMetadata();
|
|
|
|
mdata.flightAccess = UAVObject::ACCESS_READWRITE;
|
|
|
|
mdata.gcsAccess = UAVObject::ACCESS_READWRITE;
|
|
|
|
mdata.flightTelemetryAcked = false;
|
|
|
|
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
|
|
|
|
mdata.flightTelemetryUpdatePeriod = updatePeriod;
|
|
|
|
mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
|
|
|
|
obj->setMetadata(mdata);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::setupOutputObject(UAVObject* obj, int updatePeriod)
|
|
|
|
{
|
|
|
|
UAVObject::Metadata mdata;
|
|
|
|
mdata = obj->getDefaultMetadata();
|
|
|
|
mdata.flightAccess = UAVObject::ACCESS_READONLY;
|
|
|
|
mdata.gcsAccess = UAVObject::ACCESS_READWRITE;
|
|
|
|
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_NEVER;
|
|
|
|
mdata.gcsTelemetryAcked = false;
|
|
|
|
mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
|
|
|
|
mdata.gcsTelemetryUpdatePeriod = updatePeriod;
|
|
|
|
obj->setMetadata(mdata);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::onAutopilotConnect()
|
|
|
|
{
|
|
|
|
autopilotConnectionStatus = true;
|
|
|
|
setupObjects();
|
|
|
|
emit autopilotConnected();
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::onAutopilotDisconnect()
|
|
|
|
{
|
|
|
|
autopilotConnectionStatus = false;
|
|
|
|
emit autopilotDisconnected();
|
|
|
|
}
|
|
|
|
|
|
|
|
void Simulator::onSimulatorConnectionTimeout()
|
|
|
|
{
|
|
|
|
if ( simConnectionStatus )
|
|
|
|
{
|
|
|
|
simConnectionStatus = false;
|
|
|
|
emit simulatorDisconnected();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void Simulator::telStatsUpdated(UAVObject* obj)
|
|
|
|
{
|
|
|
|
GCSTelemetryStats::DataFields stats = telStats->getData();
|
|
|
|
if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED )
|
|
|
|
{
|
|
|
|
onAutopilotConnect();
|
|
|
|
}
|
|
|
|
else if ( autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED )
|
|
|
|
{
|
|
|
|
onAutopilotDisconnect();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|