From c68af6de78100377d6093f4b8461763862701687 Mon Sep 17 00:00:00 2001 From: Dmitry Zaitsev Date: Sat, 10 Mar 2012 05:56:07 +0400 Subject: [PATCH] HITL: initial commit plugin for simulator: https://github.com/hhrhhr/Aerosim-plugin-for-CopterControl --- .../src/plugins/hitlv2/aerosimrc.cpp | 444 ++++++++++++++ .../src/plugins/hitlv2/aerosimrc.h | 46 ++ .../src/plugins/hitlv2/hitlv2.pluginspec | 12 + .../src/plugins/hitlv2/hitlv2.pro | 27 + .../plugins/hitlv2/hitlv2_dependencies.pri | 4 + .../plugins/hitlv2/hitlv2configuration.cpp | 162 +++++ .../src/plugins/hitlv2/hitlv2configuration.h | 61 ++ .../src/plugins/hitlv2/hitlv2factory.cpp | 58 ++ .../src/plugins/hitlv2/hitlv2factory.h | 52 ++ .../src/plugins/hitlv2/hitlv2gadget.cpp | 49 ++ .../src/plugins/hitlv2/hitlv2gadget.h | 60 ++ .../src/plugins/hitlv2/hitlv2optionspage.cpp | 138 +++++ .../src/plugins/hitlv2/hitlv2optionspage.h | 61 ++ .../src/plugins/hitlv2/hitlv2optionspage.ui | 553 ++++++++++++++++++ .../src/plugins/hitlv2/hitlv2plugin.cpp | 70 +++ .../src/plugins/hitlv2/hitlv2plugin.h | 67 +++ .../src/plugins/hitlv2/hitlv2widget.cpp | 187 ++++++ .../src/plugins/hitlv2/hitlv2widget.h | 72 +++ .../src/plugins/hitlv2/hitlv2widget.ui | 314 ++++++++++ .../src/plugins/hitlv2/simulatorv2.cpp | 328 +++++++++++ .../src/plugins/hitlv2/simulatorv2.h | 214 +++++++ 21 files changed, 2979 insertions(+) create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp new file mode 100644 index 000000000..1bc5c3b06 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp @@ -0,0 +1,444 @@ +#include "aerosimrc.h" +#include +#include +#include + +AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) + : Simulator(params) +{ + udpCounterASrecv = 0; +} + +AeroSimRCSimulator::~AeroSimRCSimulator() +{ +} + +bool AeroSimRCSimulator::setupProcess() +{ + QMutexLocker locker(&lock); + return true; +} + +void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) +{ + Q_UNUSED(outPort) + if (inSocket->bind(QHostAddress(host), inPort)) + emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); + else + emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n"); +} + +void AeroSimRCSimulator::transmitUpdate() +{ + // read actuator output + ActuatorCommand::DataFields actCmdData; + actCmdData = actCommand->getData(); + float channels[10]; + for (int i = 0; i < 10; ++i) { + qint16 ch = actCmdData.Channel[i]; + float out = -1.0; + if (ch >= 1000 && ch <= 2000) { + ch -= 1000; + out = ((float)ch / 500.0) - 1.0; + } + channels[i] = out; + } + + // read flight status + FlightStatus::DataFields flightData; + flightData = flightStatus->getData(); + quint8 armed; + quint8 mode; + armed = flightData.Armed; + mode = flightData.FlightMode; + + QByteArray data; + // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) + data.resize(50); + QDataStream stream(&data, QIODevice::WriteOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + stream << quint32(0x52434D44); // magic header, "RCMD" + for (int i = 0; i < 10; ++i) + stream << channels[i]; // channels + stream << armed << mode; // flight status + stream << udpCounterASrecv; // packet counter + + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { + qDebug() << "write failed: " << outSocket->errorString(); + } + +#ifdef DBG_TIMERS + static int cntTX = 0; + if (cntTX >= 100) { + qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart(); + cntTX = 0; + } else { + ++cntTX; + } +#endif +} + +void AeroSimRCSimulator::processUpdate(const QByteArray &data) +{ + // check size + if (data.size() > 188) { + qDebug() << "!!! big datagram: " << data.size(); + return; + } + + QByteArray buf = data; + QDataStream stream(&buf, QIODevice::ReadOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + + // check magic header + quint32 magic; + stream >> magic; + if (magic != 0x4153494D) { // "AERO" + qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); + return; + } + + float timeStep, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, // world + velX, velY, velZ, // world + angX, angY, angZ, // model + accX, accY, accZ, // model + lat, lon, agl, // world + yaw, pitch, roll, // model + volt, curr, + rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix + ch[8]; + + stream >> timeStep; + stream >> homeX >> homeY >> homeZ; + stream >> WpHX >> WpHY >> WpLat >> WpLon; + stream >> posX >> posY >> posZ; + stream >> velX >> velY >> velZ; + stream >> angX >> angY >> angZ; + stream >> accX >> accY >> accZ; + stream >> lat >> lon >> agl; + stream >> yaw >> pitch >> roll; + stream >> volt >> curr; + stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz; + stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7]; + stream >> udpCounterASrecv; + + /**********************************************************************************************/ + QTime currentTime = QTime::currentTime(); + /**********************************************************************************************/ + static bool firstRun = true; + if (settings.homeLocation) { + if (firstRun) { + HomeLocation::DataFields homeData; + homeData = posHome->getData(); + + homeData.Latitude = WpLat * 10e6; + homeData.Longitude = WpLon * 10e6; + homeData.Altitude = homeZ; + homeData.Set = HomeLocation::SET_TRUE; + + posHome->setData(homeData); + + firstRun = false; + } + if (settings.homeLocRate > 0) { + static QTime homeLocTime = currentTime; + if (homeLocTime.secsTo(currentTime) >= settings.homeLocRate) { + firstRun = true; + homeLocTime = currentTime; + } + } + } + /**********************************************************************************************/ + if (settings.attRaw || settings.attActual) { + QMatrix4x4 mat; + mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); + mat.optimize(); + + QQuaternion quat; // model quat + asMatrix2Quat(mat, quat); + + // rotate gravity + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D gee = QVector3D(0.0, 0.0, -GEE); + QQuaternion qWorld = quat.conjugate(); + gee = qWorld.rotatedVector(gee); + acc += gee; + + /*************************************************************************************/ + if (settings.attRaw) { + AttitudeRaw::DataFields attRawData; + attRawData = attRaw->getData(); + + attRawData.gyros[0] = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + attRawData.gyros[1] = angX * RAD2DEG; + attRawData.gyros[2] = angZ * -RAD2DEG; + attRawData.accels[0] = acc.x(); + attRawData.accels[1] = acc.y(); + attRawData.accels[2] = acc.z(); + + attRaw->setData(attRawData); + } + /*************************************************************************************/ + if (settings.attActHW) { + // do nothing + /*****************************************/ + } else if (settings.attActSim) { + // take all data from simulator + AttitudeActual::DataFields attActData; + attActData = attActual->getData(); + + QVector3D rpy; // model roll, pitch, yaw + asMatrix2RPY(mat, rpy); + + attActData.Roll = rpy.x(); + attActData.Pitch = rpy.y(); + attActData.Yaw = rpy.z(); + attActData.q1 = quat.scalar(); + attActData.q2 = quat.x(); + attActData.q3 = quat.y(); + attActData.q4 = quat.z(); + + attActual->setData(attActData); + /*****************************************/ + } else if (settings.attActCalc) { + // calculate RPY with code from Attitude module + AttitudeActual::DataFields attActData; + attActData = attActual->getData(); + + static float q[4] = {1, 0, 0, 0}; + static float gyro_correct_int2 = 0; + + float dT = timeStep; + + AttitudeSettings::DataFields attSettData = attSettings->getData(); + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; + float yawBiasRate = attSettData.YawBiasRate; + + // calibrate sensors on arming + if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { + accelKp = 2.0; + accelKi = 0.9; + } + + float gyro[3] = {angY * RAD2DEG, angX * RAD2DEG, angZ * -RAD2DEG}; + float attRawAcc[3] = {acc.x(), acc.y(), acc.z()}; + + // code from Attitude module begin /////////////////////////////// + float *accels = attRawAcc; + float grot[3]; + float accel_err[3]; + + // Rotate gravity to body frame and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + + // CrossProduct + { + accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; + accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; + accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + } + + // Account for accel magnitude + float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int2 += -gyro[2] * yawBiasRate; + + // Correct rates based on error, integral component dealt with in updateSensors + gyro[0] += accel_err[0] * accelKp / dT; + gyro[1] += accel_err[1] * accelKp / dT; + gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; + + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; + + if(q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + + // Renomalize + float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + q[0] /= qmag; + q[1] /= qmag; + q[2] /= qmag; + q[3] /= qmag; + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } + + float rpy2[3]; + // Quaternion2RPY + { + float q0s, q1s, q2s, q3s; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; + + float R13, R11, R12, R23, R33; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; + + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[2] = RAD2DEG * atan2f(R12, R11); + rpy2[0] = RAD2DEG * atan2f(R23, R33); + } + + attActData.Roll = rpy2[0]; + attActData.Pitch = rpy2[1]; + attActData.Yaw = rpy2[2]; + attActData.q1 = q[0]; + attActData.q2 = q[1]; + attActData.q3 = q[2]; + attActData.q4 = q[3]; + attActual->setData(attActData); + /*****************************************/ + } + } + /**********************************************************************************************/ + if (settings.gcsReciever) { + static QTime gcsRcvrTime = currentTime; + if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) { + GCSReceiver::DataFields gcsRcvrData; + gcsRcvrData = gcsReceiver->getData(); + + for (int i = 0; i < 8; ++i) + gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500); + + gcsReceiver->setData(gcsRcvrData); + if (settings.manualOutput) + gcsRcvrTime = currentTime; + } + } else if (settings.manualControl) { + // not implemented yet + } + /**********************************************************************************************/ + if (settings.gpsPosition) { + static QTime gpsPosTime = currentTime; + if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { + GPSPosition::DataFields gpsPosData; + gpsPosData = gpsPosition->getData(); + + gpsPosData.Altitude = posZ; + gpsPosData.Heading = yaw * RAD2DEG; + gpsPosData.Latitude = lat * 10e6; + gpsPosData.Longitude = lon * 10e6; + gpsPosData.Groundspeed = qSqrt(velX * velX + velY * velY); + gpsPosData.GeoidSeparation = 0.0; + gpsPosData.Satellites = 8; + gpsPosData.PDOP = 3.0; + gpsPosData.Status = GPSPosition::STATUS_FIX3D; + + gpsPosition->setData(gpsPosData); + gpsPosTime = currentTime; + } + } + /**********************************************************************************************/ +/* + BaroAltitude::DataFields altActData; + altActData = altActual->getData(); + altActData.Altitude = posZ; + altActual->setData(altActData); + + PositionActual::DataFields posActData; + posActData = posActual->getData(); + posActData.North = posY * 100; + posActData.East = posX * 100; + posActData.Down = posZ * -100; + posActual->setData(posActData); + + VelocityActual::DataFields velActData; + velActData = velActual->getData(); + velActData.North = velY * 100; + velActData.East = velX * 100; + velActData.Down = velZ * 100; + velActual->setData(velActData); +*/ + +#ifdef DBG_TIMERS + static int cntRX = 0; + if (cntRX >= 100) { + qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart(); + cntRX = 0; + } else { + ++cntRX; + } +#endif +} + +// transfomations + +void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q) +{ + qreal w, x, y, z; + + // w always >= 0 + w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0; + x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0; + y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0; + z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0; + + x = copysign(x, (m(1, 2) - m(2, 1))); + y = copysign(y, (m(2, 0) - m(0, 2))); + z = copysign(z, (m(0, 1) - m(1, 0))); + + q.setScalar(w); + q.setX(x); + q.setY(y); + q.setZ(z); +} + +void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) +{ + qreal roll, pitch, yaw; + + if (qFabs(m(0, 2)) > 0.998) { + // ~86.3°, gimbal lock + roll = 0.0; + pitch = copysign(M_PI_2, -m(0, 2)); + yaw = qAtan2(-m(1, 0), m(1, 1)); + } else { + roll = qAtan2(m(1, 2), m(2, 2)); + pitch = qAsin(-m(0, 2)); + yaw = qAtan2(m(0, 1), m(0, 0)); + } + + rpy.setX(roll * RAD2DEG); + rpy.setY(pitch * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h new file mode 100644 index 000000000..19dae3bce --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h @@ -0,0 +1,46 @@ +#ifndef AEROSIMRC_H +#define AEROSIMRC_H + +#include +#include +#include +#include +#include "simulatorv2.h" + +class AeroSimRCSimulator: public Simulator +{ + Q_OBJECT + +public: + AeroSimRCSimulator(const SimulatorSettings ¶ms); + ~AeroSimRCSimulator(); + + bool setupProcess(); + void setupUdpPorts(const QString& host, int inPort, int outPort); + +private slots: + void transmitUpdate(); + +private: + quint32 udpCounterASrecv; //keeps track of udp packets received by ASim + + void processUpdate(const QByteArray &data); + + void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q); + void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy); +}; + +class AeroSimRCSimulatorCreator : public SimulatorCreator +{ +public: + AeroSimRCSimulatorCreator(const QString &classId, const QString &description) + : SimulatorCreator (classId, description) + {} + + Simulator* createSimulator(const SimulatorSettings ¶ms) + { + return new AeroSimRCSimulator(params); + } +}; + +#endif // AEROSIMRC_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec new file mode 100644 index 000000000..26fede385 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec @@ -0,0 +1,12 @@ + + The OpenPilot Project + (C) 2011 OpenPilot Project + The GNU Public License (GPL) Version 3 + Hardware In The Loop Simulation (v2) + http://www.openpilot.org + + + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro new file mode 100644 index 000000000..f567fc401 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro @@ -0,0 +1,27 @@ +TEMPLATE = lib +TARGET = HITLV2 +QT += network +include(../../openpilotgcsplugin.pri) +include(hitlv2_dependencies.pri) +HEADERS += \ + aerosimrc.h \ + hitlv2configuration.h \ + hitlv2factory.h \ + hitlv2gadget.h \ + hitlv2optionspage.h \ + hitlv2plugin.h \ + hitlv2widget.h \ + simulatorv2.h +SOURCES += \ + aerosimrc.cpp \ + hitlv2configuration.cpp \ + hitlv2factory.cpp \ + hitlv2gadget.cpp \ + hitlv2optionspage.cpp \ + hitlv2plugin.cpp \ + hitlv2widget.cpp \ + simulatorv2.cpp +OTHER_FILES += hitlv2.pluginspec +FORMS += \ + hitlv2optionspage.ui \ + hitlv2widget.ui diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri new file mode 100644 index 000000000..50268face --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri @@ -0,0 +1,4 @@ +include(../../plugins/uavobjects/uavobjects.pri) +include(../../plugins/uavtalk/uavtalk.pri) +#include(../../plugins/coreplugin/coreplugin.pri) +#include(../../libs/utils/utils.pri) diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp new file mode 100644 index 000000000..90c385038 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp @@ -0,0 +1,162 @@ +/** + ****************************************************************************** + * + * @file hitlconfiguration.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 "hitlv2configuration.h" + +HITLConfiguration::HITLConfiguration(QString classId, + QSettings* qSettings, + QObject *parent) + : IUAVGadgetConfiguration(classId, parent) +{ +// qDebug() << "HITLV2Configuration"; + // default values + QString simulatorId = ""; + QString hostAddress = "127.0.0.1"; + int inPort = 0; + QString remoteAddress = "127.0.0.1"; + int outPort = 0; + QString binPath = ""; + QString dataPath = ""; + + bool homeLocation = true; + quint16 homeLocRate = 0; + + bool attRaw = true; + quint8 attRawRate = 20; + + bool attActual = true; + bool attActHW = false; + bool attActSim = true; + bool attActCalc = false; + + bool gpsPosition = true; + quint16 gpsPosRate = 200; + + bool inputCommand = true; + bool gcsReciever = true; + bool manualControl = false; + + bool manualOutput = false; + quint8 outputRate = 20; + + // if a saved configuration exists load it + if (qSettings != 0) { + settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString(); + settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString(); + settings.inPort = qSettings->value("inPort", inPort).toInt(); + settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString(); + settings.outPort = qSettings->value("outPort", outPort).toInt(); + settings.binPath = qSettings->value("binPath", binPath).toString(); + settings.dataPath = qSettings->value("dataPath", dataPath).toString(); + + settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool(); + settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt(); + + settings.attRaw = qSettings->value("attRaw", attRaw).toBool(); + settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt(); + + settings.attActual = qSettings->value("attActual", attActual).toBool(); + settings.attActHW = qSettings->value("attActHW", attActHW).toBool(); + settings.attActSim = qSettings->value("attActSim", attActSim).toBool(); + settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool(); + + settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt(); + + settings.inputCommand = qSettings->value("inputCommand", inputCommand).toBool(); + settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool(); + settings.manualControl = qSettings->value("manualControl", manualControl).toBool(); + settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool(); + settings.outputRate = qSettings->value("outputRate", outputRate).toInt(); + } else { + settings.simulatorId = simulatorId; + settings.hostAddress = hostAddress; + settings.inPort = inPort; + settings.remoteAddress = remoteAddress; + settings.outPort = outPort; + settings.binPath = binPath; + settings.dataPath = dataPath; + + settings.homeLocation = homeLocation; + settings.homeLocRate = homeLocRate; + + settings.attRaw = attRaw; + settings.attRawRate = attRawRate; + + settings.attActual = attActual; + settings.attActHW = attActHW; + settings.attActSim = attActSim; + settings.attActCalc = attActCalc; + + settings.gpsPosition = gpsPosition; + settings.gpsPosRate = gpsPosRate; + + settings.inputCommand = inputCommand; + settings.gcsReciever = gcsReciever; + settings.manualControl = manualControl; + settings.manualOutput = manualOutput; + settings.outputRate = outputRate; + } +} + +IUAVGadgetConfiguration *HITLConfiguration::clone() +{ + HITLConfiguration *m = new HITLConfiguration(this->classId()); + m->settings = settings; + return m; +} + +void HITLConfiguration::saveConfig(QSettings* qSettings) const +{ + qSettings->setValue("simulatorId", settings.simulatorId); + qSettings->setValue("hostAddress", settings.hostAddress); + qSettings->setValue("inPort", settings.inPort); + qSettings->setValue("remoteAddress", settings.remoteAddress); + qSettings->setValue("outPort", settings.outPort); + qSettings->setValue("binPath", settings.binPath); + qSettings->setValue("dataPath", settings.dataPath); + + qSettings->setValue("homeLocation", settings.homeLocation); + qSettings->setValue("homeLocRate", settings.homeLocRate); + + qSettings->setValue("attRaw", settings.attRaw); + qSettings->setValue("attRawRate", settings.attRawRate); + + qSettings->setValue("attActual", settings.attActual); + qSettings->setValue("attActHW", settings.attActHW); + qSettings->setValue("attActSim", settings.attActSim); + qSettings->setValue("attActCalc", settings.attActCalc); + + qSettings->setValue("gpsPosition", settings.gpsPosition); + qSettings->setValue("gpsPosRate", settings.gpsPosRate); + + qSettings->setValue("inputCommand", settings.inputCommand); + qSettings->setValue("gcsReciever", settings.gcsReciever); + qSettings->setValue("manualControl", settings.manualControl); + qSettings->setValue("manualOutput", settings.manualOutput); + qSettings->setValue("outputRate", settings.outputRate); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h new file mode 100644 index 000000000..dfac80ddf --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file hitlconfiguration.h + * @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 + */ + +#ifndef HITLV2CONFIGURATION_H +#define HITLV2CONFIGURATION_H + +#include +#include +#include +#include + +using namespace Core; + +class HITLConfiguration : public IUAVGadgetConfiguration +{ + + Q_OBJECT + + Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) + +public: + explicit HITLConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + + void saveConfig(QSettings* settings) const; + IUAVGadgetConfiguration *clone(); + + SimulatorSettings Settings() const { return settings; } + +public slots: + void setSimulatorSettings (const SimulatorSettings& params ) { settings = params; } + + +private: + SimulatorSettings settings; +}; + +#endif // HITLV2CONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp new file mode 100644 index 000000000..6e6f1a42e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp @@ -0,0 +1,58 @@ +/** + ****************************************************************************** + * + * @file hitlfactory.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 "hitlv2factory.h" +#include "hitlv2widget.h" +#include "hitlv2gadget.h" +#include "hitlv2configuration.h" +#include "hitlv2optionspage.h" +#include + +HITLFactory::HITLFactory(QObject *parent) + : IUAVGadgetFactory(QString("HITLv2"), tr("HITL Simulation (v2)"), parent) +{ +} + +HITLFactory::~HITLFactory() +{ +} + +Core::IUAVGadget* HITLFactory::createGadget(QWidget *parent) +{ + HITLWidget* gadgetWidget = new HITLWidget(parent); + return new HITLGadget(QString("HITLv2"), gadgetWidget, parent); +} + +IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings* qSettings) +{ + return new HITLConfiguration(QString("HITLv2"), qSettings); +} + +IOptionsPage *HITLFactory::createOptionsPage(IUAVGadgetConfiguration *config) +{ + return new HITLOptionsPage(qobject_cast(config)); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h new file mode 100644 index 000000000..b4142eeed --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * + * @file hitlfactory.h + * @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 + */ + +#ifndef HITLV2FACTORY_H +#define HITLV2FACTORY_H + +#include + +namespace Core { +class IUAVGadget; +class IUAVGadgetFactory; +} + +using namespace Core; + +class HITLFactory : public Core::IUAVGadgetFactory +{ + Q_OBJECT +public: + HITLFactory(QObject *parent = 0); + ~HITLFactory(); + + IUAVGadget *createGadget(QWidget *parent); + IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); +}; + +#endif // HITLV2FACTORY_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp new file mode 100644 index 000000000..de22d121b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file hitl.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 "hitlv2gadget.h" +#include "hitlv2widget.h" +#include "hitlv2configuration.h" +#include "simulatorv2.h" + +HITLGadget::HITLGadget(QString classId, HITLWidget *widget, QWidget *parent) : + IUAVGadget(classId, parent), + m_widget(widget) +{ + connect(this, SIGNAL(changeConfiguration(void)), m_widget, SLOT(stopButtonClicked(void))); +} + +HITLGadget::~HITLGadget() +{ + delete m_widget; +} + +void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) +{ + HITLConfiguration *m = qobject_cast(config); + emit changeConfiguration(); + m_widget->setSettingParameters(m->Settings()); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h new file mode 100644 index 000000000..0e4fd1a80 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file hitl.h + * @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 + */ + +#ifndef HITLV2_H +#define HITLV2_H + +#include +#include "hitlv2widget.h" + +class IUAVGadget; +class QWidget; +class QString; +class Simulator; + +using namespace Core; + +class HITLGadget : public Core::IUAVGadget +{ + Q_OBJECT +public: + HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); + ~HITLGadget(); + + QWidget *widget() { return m_widget; } + void loadConfiguration(IUAVGadgetConfiguration* config); + +signals: + void changeConfiguration(); + +private: + HITLWidget* m_widget; + Simulator* simulator; +}; + + +#endif // HITLV2_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp new file mode 100644 index 000000000..cf89aae18 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp @@ -0,0 +1,138 @@ +/** + ****************************************************************************** + * + * @file hitloptionspage.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 "hitlv2optionspage.h" +#include "hitlv2configuration.h" +#include "ui_hitlv2optionspage.h" +#include "hitlv2plugin.h" + +#include +#include +#include +#include "simulatorv2.h" + +HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : + IOptionsPage(parent), + config(conf) +{ +} + +QWidget *HITLOptionsPage::createPage(QWidget *parent) +{ +// qDebug() << "HITLOptionsPage::createPage"; + // Create page + m_optionsPage = new Ui::HITLOptionsPage(); + QWidget* optionsPageWidget = new QWidget; + m_optionsPage->setupUi(optionsPageWidget); + int index = 0; + foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) + m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(), creator->ClassId()); + + m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); + m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); + m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); + m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); + + // Restore the contents from the settings: + foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) { + QString id = config->Settings().simulatorId; + if (creator->ClassId() == id) + m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } + + m_optionsPage->hostAddress->setText(config->Settings().hostAddress); + m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); + m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress); + m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); + m_optionsPage->executablePath->setPath(config->Settings().binPath); + m_optionsPage->dataPath->setPath(config->Settings().dataPath); + + m_optionsPage->homeLocation->setChecked(config->Settings().homeLocation); + m_optionsPage->homeLocRate->setValue(config->Settings().homeLocRate); + + m_optionsPage->attRaw->setChecked(config->Settings().attRaw); + m_optionsPage->attRawRate->setValue(config->Settings().attRawRate); + + m_optionsPage->attActual->setChecked(config->Settings().attActual); + m_optionsPage->attActHW->setChecked(config->Settings().attActHW); + m_optionsPage->attActSim->setChecked(config->Settings().attActSim); + m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); + + m_optionsPage->gpsPosition->setChecked(config->Settings().gpsPosition); + m_optionsPage->gpsPosRate->setValue(config->Settings().gpsPosRate); + + m_optionsPage->inputCommand->setChecked(config->Settings().inputCommand); + m_optionsPage->gcsReciever->setChecked(config->Settings().gcsReciever); + m_optionsPage->manualControl->setChecked(config->Settings().manualControl); + + m_optionsPage->manualOutput->setChecked(config->Settings().manualOutput); + m_optionsPage->outputRate->setValue(config->Settings().outputRate); + + return optionsPageWidget; +} + +void HITLOptionsPage::apply() +{ + SimulatorSettings settings; + int i = m_optionsPage->chooseFlightSimulator->currentIndex(); + + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.inPort = m_optionsPage->inputPort->text().toInt(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + + settings.homeLocation = m_optionsPage->homeLocation->isChecked(); + settings.homeLocRate = m_optionsPage->homeLocRate->value(); + + settings.attRaw = m_optionsPage->attRaw->isChecked(); + settings.attRawRate = m_optionsPage->attRawRate->value(); + + settings.attActual = m_optionsPage->attActual->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + + settings.gpsPosition = m_optionsPage->gpsPosition->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRate->value(); + + settings.inputCommand = m_optionsPage->inputCommand->isChecked(); + settings.gcsReciever = m_optionsPage->gcsReciever->isChecked(); + settings.manualControl = m_optionsPage->manualControl->isChecked(); + + settings.manualOutput = m_optionsPage->manualOutput->isChecked(); + settings.outputRate = m_optionsPage->outputRate->value(); + + config->setSimulatorSettings(settings); +} + +void HITLOptionsPage::finish() +{ + delete m_optionsPage; +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h new file mode 100644 index 000000000..5c8d33f59 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file hitloptionspage.h + * @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 + */ + +#ifndef HITLV2OPTIONSPAGE_H +#define HITLV2OPTIONSPAGE_H + +#include + +namespace Core { +class IUAVGadgetConfiguration; +} + +class HITLConfiguration; + +using namespace Core; + +namespace Ui { +class HITLOptionsPage; +} + +class HITLOptionsPage : public IOptionsPage +{ + Q_OBJECT +public: + explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); + + QWidget *createPage(QWidget *parent); + void apply(); + void finish(); + bool isDecorated() const { return true; } + +private: + HITLConfiguration* config; + Ui::HITLOptionsPage* m_optionsPage; +}; + +#endif // HITLV2OPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui new file mode 100644 index 000000000..4bbfe93bb --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui @@ -0,0 +1,553 @@ + + + HITLOptionsPage + + + + 0 + 0 + 403 + 400 + + + + Form + + + + 3 + + + 0 + + + 0 + + + 0 + + + 3 + + + + + + + Choose flight simulator: + + + + + + + + + + + + + + Local interface (IP): + + + + + + + For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. + + + + + + + Remote interface (IP): + + + + + + + Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. + + + + + + + Input Port: + + + + + + + For receiving data from sim + + + + + + + Output Port: + + + + + + + For sending data to sim + + + + + + + + + QFormLayout::AllNonFixedFieldsGrow + + + + + + 0 + 0 + + + + Path executable: + + + + + + + + + + + 0 + 0 + + + + Data directory: + + + + + + + + + + + + + + Attitude data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + AttitudeRaw (gyro, accels) + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + ms + + + 10 + + + 100 + + + 20 + + + + + + + + + + AttitudeActual + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + send raw data to board + + + + + + + + 75 + true + + + + + + + use values from simulator + + + true + + + + + + + + + + calculate from AttitudeRaw + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Other data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + HomeLocation + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + 0 - update once, or every N seconds + + + sec + + + 10 + + + + + + + + + + GPSPosition + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate + + + + + + + ms + + + 100 + + + 2000 + + + 500 + + + + + + + + + + Map command from simulator + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + + 75 + true + + + + to GCSReciver + + + true + + + + + + + false + + + to ManualCtrll (not implemented) + + + + + + + + + + 6 + + + + + Maximum output rate + + + + + + + ms + + + 10 + + + 100 + + + 5 + + + 15 + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Utils::PathChooser + QWidget +
utils/pathchooser.h
+ 1 + + + + chooseFlightSimulator + hostAddress + inputPort + remoteAddress + outputPort + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp new file mode 100644 index 000000000..f0ee2d07a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * + * @file mapplugin.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 "hitlv2plugin.h" +#include "hitlv2factory.h" +#include +#include +#include + +#include "aerosimrc.h" + +QList HITLPlugin::typeSimulators; + +HITLPlugin::HITLPlugin() +{ + // Do nothing +} + +HITLPlugin::~HITLPlugin() +{ + // Do nothing +} + +bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new HITLFactory(this); + + addAutoReleasedObject(mf); + + addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); + + return true; +} + +void HITLPlugin::extensionsInitialized() +{ + // Do nothing +} + +void HITLPlugin::shutdown() +{ + // Do nothing +} +Q_EXPORT_PLUGIN(HITLPlugin) + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h new file mode 100644 index 000000000..e68129bd4 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h @@ -0,0 +1,67 @@ +/** + ****************************************************************************** + * + * @file browserplugin.h + * @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 + */ + +#ifndef HITLV2PLUGIN_H +#define HITLV2PLUGIN_H + +#include +#include + +#include + +class HITLFactory; + +class HITLPlugin : public ExtensionSystem::IPlugin +{ +public: + HITLPlugin(); + ~HITLPlugin(); + + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString * errorString); + void shutdown(); + + static void addSimulator(SimulatorCreator* creator) + { + HITLPlugin::typeSimulators.append(creator); + } + + static SimulatorCreator* getSimulatorCreator(const QString classId) + { + foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) { + if(classId == creator->ClassId()) + return creator; + } + return 0; + } + + static QList typeSimulators; + +private: + HITLFactory *mf; +}; +#endif /* HITLV2PLUGIN_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp new file mode 100644 index 000000000..55d14f735 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp @@ -0,0 +1,187 @@ +/** + ****************************************************************************** + * + * @file hitlwidget.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 "hitlv2widget.h" +#include "ui_hitlv2widget.h" +#include +#include +#include +#include +#include + +#include "hitlv2plugin.h" +#include "simulatorv2.h" +#include "uavobjectmanager.h" +#include +#include + +QStringList Simulator::instances; + +HITLWidget::HITLWidget(QWidget *parent) + : QWidget(parent), + simulator(0) +{ + widget = new Ui_HITLWidget(); + widget->setupUi(this); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); + + strAutopilotDisconnected = " AP OFF "; + strSimulatorDisconnected = " Sim OFF "; + strAutopilotConnected = " AP ON "; +// strSimulatorConnected = " Sim ON "; + strStyleEnable = "QFrame{background-color: green; color: white}"; + strStyleDisable = "QFrame{background-color: red; color: grey}"; + + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); + + connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); + connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); + connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); +} + +HITLWidget::~HITLWidget() +{ + delete widget; +} + +void HITLWidget::startButtonClicked() +{ + // allow only one instance per simulator + if (Simulator::Instances().indexOf(settings.simulatorId) != -1) { + widget->textBrowser->append(settings.simulatorId + " alreary started!"); + return; + } + + if (!HITLPlugin::typeSimulators.size()) { + widget->textBrowser->append("There is no registered simulators, add through HITLPlugin::addSimulator"); + return; + } + + // Stop running process if one is active + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } + + if (settings.hostAddress == "" || settings.inPort == 0) { + widget->textBrowser->append("Before start, set UDP parameters in options page!"); + return; + } + + SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); + simulator = creator->createSimulator(settings); + simulator->setName(creator->Description()); + simulator->setSimulatorId(creator->ClassId()); + + connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); + + // Setup process + onProcessOutput(QString("[%1] Starting %2... ") + .arg(QTime::currentTime().toString("hh:mm:ss")) + .arg(creator->Description())); + + // Start bridge + bool ret = QMetaObject::invokeMethod(simulator, "setupProcess", Qt::QueuedConnection); + if (ret) { + Simulator::setInstance(settings.simulatorId); + + connect(this, SIGNAL(deleteSimulator()), simulator, SLOT(onDeleteSimulator()), Qt::QueuedConnection); + + widget->startButton->setEnabled(false); + widget->stopButton->setEnabled(true); + + connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()), Qt::QueuedConnection); + + // Initialize connection status + if (simulator->isAutopilotConnected()) + onAutopilotConnect(); + else + onAutopilotDisconnect(); + + if (simulator->isSimulatorConnected()) + onSimulatorConnect(); + else + onSimulatorDisconnect(); + } +} + +void HITLWidget::stopButtonClicked() +{ + if (simulator) + widget->textBrowser->append(QString("[%1] Terminate %2 ") + .arg(QTime::currentTime().toString("hh:mm:ss")) + .arg(simulator->Name())); + + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); + widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } +} + +void HITLWidget::buttonClearLogClicked() +{ + widget->textBrowser->clear(); +} + +void HITLWidget::onProcessOutput(QString text) +{ + widget->textBrowser->append(text); +} + +void HITLWidget::onAutopilotConnect() +{ + widget->apLabel->setStyleSheet(strStyleEnable); + widget->apLabel->setText(strAutopilotConnected); +} + +void HITLWidget::onAutopilotDisconnect() +{ + widget->apLabel->setStyleSheet(strStyleDisable); + widget->apLabel->setText(strAutopilotDisconnected); +} + +void HITLWidget::onSimulatorConnect() +{ + widget->simLabel->setStyleSheet(strStyleEnable); + widget->simLabel->setText(" " + simulator->Name() + " ON "); +} + +void HITLWidget::onSimulatorDisconnect() +{ + widget->simLabel->setStyleSheet(strStyleDisable); + widget->simLabel->setText(" " + simulator->Name() + " OFF "); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h new file mode 100644 index 000000000..78f3e9405 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * + * @file hitlwidget.h + * @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 + */ + +#ifndef HITLV2WIDGET_H +#define HITLV2WIDGET_H + +#include +#include +#include "simulatorv2.h" + +class Ui_HITLWidget; + +class HITLWidget : public QWidget +{ + Q_OBJECT + +public: + HITLWidget(QWidget *parent = 0); + ~HITLWidget(); + + void setSettingParameters(const SimulatorSettings& params) { settings = params; } + +signals: + void deleteSimulator(); + +private slots: + void startButtonClicked(); + void stopButtonClicked(); + void buttonClearLogClicked(); + void onProcessOutput(QString text); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onSimulatorConnect(); + void onSimulatorDisconnect(); + +private: + Ui_HITLWidget* widget; + Simulator* simulator; + SimulatorSettings settings; + + QString strAutopilotDisconnected; + QString strSimulatorDisconnected; + QString strAutopilotConnected; +// QString strSimulatorConnected; + QString strStyleEnable; + QString strStyleDisable; +}; +#endif /* HITLV2WIDGET_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui new file mode 100644 index 000000000..48d2c2db2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui @@ -0,0 +1,314 @@ + + + HITLWidget + + + + 0 + 0 + 477 + 300 + + + + + 0 + 0 + + + + Form + + + + +QScrollBar:vertical { + border: 1px solid grey; + background: grey; + margin: 22px 0 22px 0; + } + +QScrollBar:vertical:disabled { + border: 1px solid grey; + + background-color: grey; + margin: 22px 0 22px 0; + } + + QScrollBar::handle:vertical { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 255, 255), stop:1 rgba(80, 80, 160, 255)); + min-height: 20px; + } + +QScrollBar::handle:vertical:disabled{ +background-color: grey; + min-height: 20px; + } + + + QScrollBar::handle:vertical:pressed { + + background-color: rgb(85, 85, 255); +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(170, 170, 255, 255), stop:1 rgba(80, 80, 160, 255)); + + min-height: 20px; + } + + QScrollBar::add-line:vertical { + border: 1px solid black; +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); + height: 20px; + subcontrol-position: bottom; + subcontrol-origin: margin; + } + + QScrollBar::add-line:vertical:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); + border: 1px solid grey; + + } + + QScrollBar::sub-line:vertical:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); + border: 1px solid grey; + + } + + QScrollBar::add-line:vertical:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); + } + + QScrollBar::sub-line:vertical:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); + } + + QScrollBar::sub-line:vertical { + border: 1px solid black; +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); + height: 20px; + subcontrol-position: top; + subcontrol-origin: margin; + } + QScrollBar::down-arrow:vertical { + + image: url(:/hitlnew/images/arrow-down2.png); + } + + QScrollBar::up-arrow:vertical { + image: url(:/hitlnew/images/arrow-up2.png); + } + + QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { + background: none; + } + + +QPushButton { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); + + color: rgb(255, 255, 255); +border: 1px solid black; +width: 66px; +height: 20px; +} + +QPushButton:disabled { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 120, 255)); + color: rgb(194, 194, 194); +border: 1px solid gray; +width: 66px; +height: 20px; +} + +QPushButton:hover { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); +color: rgb(255, 255, 255); +border: 0px; +} +QPushButton:pressed { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); +color: rgb(255, 255, 255); +border: 0px; +} + +QPushButton:checked { +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); +color: rgb(255, 255, 255); +border: 0px; +} + + + + 0 + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + +QFrame{ +background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); +color: rgba(0, 0, 0, 128); +} + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + QLayout::SetMaximumSize + + + + + QLayout::SetDefaultConstraint + + + + + Request update + + + + + + Start + + + + + + + false + + + Send update + + + Stop + + + + + + + + 0 + 22 + + + + + 50 + false + + + + Qt::LeftToRight + + + false + + + color: rgb(255, 255, 255); + + + AP OFF + + + Qt::AlignCenter + + + + + + + + 0 + 22 + + + + color: rgb(255, 255, 255); + + + Sim OFF + + + Qt::AlignCenter + + + + + + + Clear Log + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + false + + + QTextEdit { + background-color: white; + color: rgb(0, 0, 0); +} + + + QFrame::NoFrame + + + QFrame::Plain + + + Qt::ScrollBarAlwaysOn + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp new file mode 100644 index 000000000..a940c0c4a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp @@ -0,0 +1,328 @@ +/** + ****************************************************************************** + * + * @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 "simulatorv2.h" +#include +#include +#include + +volatile bool Simulator::isStarted = false; + +const float Simulator::GEE = 9.81; +const float Simulator::FT2M = 0.3048; +const float Simulator::KT2MPS = 0.514444444; +const float Simulator::INHG2KPA = 3.386; +const float Simulator::FPS2CMPS = 30.48; +const float Simulator::DEG2RAD = (M_PI/180.0); +const float Simulator::RAD2DEG = (180.0/M_PI); + + +Simulator::Simulator(const SimulatorSettings& params) : + 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() +{ +// qDebug() << "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; + } +} + +void Simulator::onDeleteSimulator(void) +{ +// qDebug() << "Simulator::onDeleteSimulator"; + resetAllObjects(); + + Simulator::setStarted(false); + Simulator::Instances().removeOne(simulatorId); + + disconnect(this); + delete this; +} + +void Simulator::onStart() +{ +// qDebug() << "Simulator::onStart"; + QMutexLocker locker(&lock); + + // Get required UAVObjects + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager* objManager = pm->getObject(); + +// actDesired = ActuatorDesired::GetInstance(objManager); +// manCtrlCommand = ManualControlCommand::GetInstance(objManager); +// velActual = VelocityActual::GetInstance(objManager); +// posActual = PositionActual::GetInstance(objManager); +// altActual = BaroAltitude::GetInstance(objManager); +// camDesired = CameraDesired::GetInstance(objManager); +// acsDesired = AccessoryDesired::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + attRaw = AttitudeRaw::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + gpsPosition = GPSPosition::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + gcsReceiver = GCSReceiver::GetInstance(objManager); + actCommand = ActuatorCommand::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + + telStats = GCSTelemetryStats::GetInstance(objManager); + + // Listen to autopilot connection events + TelemetryManager* telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + + // If already connect setup autopilot + GCSTelemetryStats::DataFields stats = telStats->getData(); + if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) + onAutopilotConnect(); + + emit processOutput("Local interface: " + settings.hostAddress + ":" + \ + QString::number(settings.inPort) + "\n" + \ + "Remote interface: " + settings.remoteAddress + ":" + \ + QString::number(settings.outPort) + "\n"); + + inSocket = new QUdpSocket(); + outSocket = new QUdpSocket(); + setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort); + + connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate())/*, Qt::DirectConnection*/); + + // Setup transmit timer + if (settings.manualOutput) { + txTimer = new QTimer(); + connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate())/*, Qt::DirectConnection*/); + txTimer->setInterval(settings.outputRate); + txTimer->start(); + } + + // Setup simulator connection timer + simTimer = new QTimer(); + connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout())/*, Qt::DirectConnection*/); + simTimer->setInterval(simTimeout); + simTimer->start(); + +#ifdef DBG_TIMERS + timeRX = QTime(); + timeRX.start(); + timeTX = QTime(); + timeTX.start(); +#endif + + setupObjects(); +} + +void Simulator::receiveUpdate() +{ + // Update connection timer and status + 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); + // Process incomming data + processUpdate(datagram); + } + if (!settings.manualOutput) + transmitUpdate(); +} + +void Simulator::setupObjects() +{ + if (settings.gcsReciever) { + setupInputObject(actCommand, settings.outputRate); + setupOutputObject(gcsReceiver); + } else if (settings.manualControl) { +// setupInputObject(actDesired); +// setupInputObject(camDesired); +// setupInputObject(acsDesired); +// setupOutputObject(manCtrlCommand); + qDebug() << "ManualControlCommand not implemented yet"; + } + + if (settings.homeLocation) + setupOutputObject(posHome); + + if (settings.gpsPosition) + setupOutputObject(gpsPosition); + + if (settings.attRaw || settings.attActual) + setupOutputObject(attRaw); + + if (settings.attActual && !settings.attActHW) + setupOutputObject(attActual); + else + setupWatchedObject(attActual); +} + +void Simulator::resetAllObjects() +{ + setupDefaultObject(posHome); + setupDefaultObject(attRaw); + setupDefaultObject(attActual); + setupDefaultObject(gpsPosition); + setupDefaultObject(gcsReceiver); + setupDefaultObject(actCommand); +// setupDefaultObject(manCtrlCommand); +// setupDefaultObject(actDesired); +// setupDefaultObject(camDesired); +// setupDefaultObject(acsDesired); +// setupDefaultObject(altActual); +// setupDefaultObject(posActual); +// setupDefaultObject(velActual); +} + +void Simulator::setupInputObject(UAVObject* obj, quint32 updateRate) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + mdata.gcsAccess = UAVObject::ACCESS_READONLY; + mdata.gcsTelemetryAcked = false; + mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_NEVER; + mdata.gcsTelemetryUpdatePeriod = 0; + mdata.flightAccess = UAVObject::ACCESS_READWRITE; + mdata.flightTelemetryAcked = false; + if (settings.manualOutput) { + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = updateRate; + } else { + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; + mdata.flightTelemetryUpdatePeriod = 0; + } + + obj->setMetadata(mdata); +} + +void Simulator::setupWatchedObject(UAVObject *obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + mdata.gcsAccess = UAVObject::ACCESS_READONLY; + mdata.gcsTelemetryAcked = false; + mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL; + mdata.gcsTelemetryUpdatePeriod = 0; + mdata.flightAccess = UAVObject::ACCESS_READWRITE; + mdata.flightTelemetryAcked = false; + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 100; + + obj->setMetadata(mdata); +} + +void Simulator::setupOutputObject(UAVObject* obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + mdata.flightAccess = UAVObject::ACCESS_READONLY; + mdata.flightTelemetryAcked = false; + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_NEVER; + mdata.flightTelemetryUpdatePeriod = 0; + mdata.gcsAccess = UAVObject::ACCESS_READWRITE; + mdata.gcsTelemetryAcked = false; + mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; + mdata.gcsTelemetryUpdatePeriod = 0; + + obj->setMetadata(mdata); +} + +void Simulator::setupDefaultObject(UAVObject *obj) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + obj->setMetadata(mdata); +} + +void Simulator::onAutopilotConnect() +{ + autopilotConnectionStatus = true; + 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(); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h new file mode 100644 index 000000000..0ddd93a9f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h @@ -0,0 +1,214 @@ +/** + ****************************************************************************** + * + * @file simulator.h + * @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 + */ + +#ifndef ISIMULATORV2_H +#define ISIMULATORV2_H + +#include +#include +#include +#include +#include +#include +#include "uavtalk/telemetrymanager.h" +#include "uavobjectmanager.h" +#include "homelocation.h" +#include "attituderaw.h" +#include "attitudeactual.h" +#include "gpsposition.h" +#include "flightstatus.h" +#include "gcsreceiver.h" +#include "actuatorcommand.h" +#include "gcstelemetrystats.h" +#include "attitudesettings.h" + +//#define DBG_TIMERS +#undef DBG_TIMERS + +/** + * just imagine this was a class without methods and all public properties + */ + +typedef struct _CONNECTION +{ + QString simulatorId; + QString hostAddress; + int inPort; + QString remoteAddress; + int outPort; + QString binPath; + QString dataPath; + + bool homeLocation; + quint16 homeLocRate; + + bool attRaw; + quint8 attRawRate; + + bool attActual; + bool attActHW; + bool attActSim; + bool attActCalc; + + bool gpsPosition; + quint16 gpsPosRate; + + bool inputCommand; + bool gcsReciever; + bool manualControl; + bool manualOutput; + quint8 outputRate; + +} SimulatorSettings; + +class Simulator : public QObject +{ + Q_OBJECT + +public: + Simulator(const SimulatorSettings& params); + virtual ~Simulator(); + + bool isAutopilotConnected() const { return autopilotConnectionStatus; } + bool isSimulatorConnected() const { return simConnectionStatus; } + + QString Name() const { return name; } + void setName(QString str) { name = str; } + + QString SimulatorId() const { return simulatorId; } + void setSimulatorId(QString str) { simulatorId = str; } + + static bool IsStarted() { return isStarted; } + static void setStarted(bool val) { isStarted = val; } + + static QStringList& Instances() { return Simulator::instances; } + static void setInstance(const QString& str) { Simulator::instances.append(str); } + + virtual void stopProcess() {} + virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} + +signals: + void processOutput(QString str); + void autopilotConnected(); + void autopilotDisconnected(); + void simulatorConnected(); + void simulatorDisconnected(); + void myStart(); + +public slots: + Q_INVOKABLE virtual bool setupProcess() { return true; } + +private slots: + void onStart(); + void receiveUpdate(); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onSimulatorConnectionTimeout(); + void telStatsUpdated(UAVObject* obj); + Q_INVOKABLE void onDeleteSimulator(void); + + virtual void transmitUpdate() = 0; + virtual void processUpdate(const QByteArray& data) = 0; + +protected: + static const float GEE; + static const float FT2M; + static const float KT2MPS; + static const float INHG2KPA; + static const float FPS2CMPS; + static const float DEG2RAD; + static const float RAD2DEG; + +#ifdef DBG_TIMERS + QTime timeRX; + QTime timeTX; +#endif + + QUdpSocket* inSocket; + QUdpSocket* outSocket; + +// ActuatorDesired* actDesired; +// ManualControlCommand* manCtrlCommand; +// VelocityActual* velActual; +// PositionActual* posActual; +// BaroAltitude* altActual; +// CameraDesired *camDesired; +// AccessoryDesired *acsDesired; + AttitudeRaw* attRaw; + AttitudeActual* attActual; + HomeLocation* posHome; + FlightStatus* flightStatus; + GPSPosition* gpsPosition; + GCSReceiver *gcsReceiver; + ActuatorCommand *actCommand; + AttitudeSettings *attSettings; + + GCSTelemetryStats* telStats; + SimulatorSettings settings; + + QMutex lock; + +private: + int updatePeriod; + int simTimeout; + volatile bool autopilotConnectionStatus; + volatile bool simConnectionStatus; + QTimer* txTimer; + QTimer* simTimer; + QString name; + QString simulatorId; + volatile static bool isStarted; + static QStringList instances; + + void setupObjects(); + void resetAllObjects(); + void setupInputObject(UAVObject* obj, quint32 updateRate); + void setupOutputObject(UAVObject* obj); + void setupWatchedObject(UAVObject *obj); + void setupDefaultObject(UAVObject *obj); +}; + +class SimulatorCreator +{ +public: + SimulatorCreator(QString id, QString descr) : + classId(id), + description(descr) + {} + virtual ~SimulatorCreator() {} + + QString ClassId() const { return classId; } + QString Description() const { return description; } + + virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; + +private: + QString classId; + QString description; +}; + +#endif // ISIMULATORV2_H