1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-13 20:48:42 +01:00
LibrePilot/ground/src/plugins/hitlnew/simulator.cpp

270 lines
7.9 KiB
C++
Raw Normal View History

/**
******************************************************************************
*
* @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);
QThread* mainThread = QThread::currentThread();
qDebug() << "Simulator Thread: "<< mainThread;
// Get required UAVObjects
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
actDesired = ActuatorDesired::GetInstance(objManager);
altActual = BaroAltitude::GetInstance(objManager);
attActual = AttitudeActual::GetInstance(objManager);
posActual = PositionActual::GetInstance(objManager);
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
while(inSocket->hasPendingDatagrams()) {
// Receive datagram
QByteArray datagram;
datagram.resize(inSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
inSocket->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
QString datastr(datagram);
// Process incomming data
processUpdate(datastr);
}
}
void Simulator::setupObjects()
{
setupInputObject(actDesired, 75);
setupOutputObject(altActual, 250);
setupOutputObject(attActual, 75);
setupOutputObject(posActual, 250);
}
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();
}
}