diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2f2eab3c2a1333dc03f485f72d38919c2b599b52..fcc340dc6b91fb851700adf4868d6a1cb7c426e5 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -751,9 +751,6 @@ HEADERS += \ src/GPS/vehicle_gps_position.h \ src/Joystick/JoystickSDL.h \ src/RunGuard.h \ - src/comm/QGCHilLink.h \ - src/comm/QGCJSBSimLink.h \ - src/comm/QGCXPlaneLink.h \ } iOSBuild { @@ -940,8 +937,6 @@ SOURCES += \ src/GPS/RTCM/RTCMMavlink.cc \ src/Joystick/JoystickSDL.cc \ src/RunGuard.cc \ - src/comm/QGCJSBSimLink.cc \ - src/comm/QGCXPlaneLink.cc \ } # diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 88c0b8569e223468a08287d7c1c6c55976d776cc..9f9036affd5377e9656fac8393b1a4c9fd07ff38 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -66,11 +66,9 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); - if (!_vehicle->hilMode()) { - _sensorsComponent = new SensorsComponent(_vehicle, this); - _sensorsComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); - } + _sensorsComponent = new SensorsComponent(_vehicle, this); + _sensorsComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); _radioComponent = new PX4RadioComponent(_vehicle, this); _radioComponent->setupTriggerSignals(); @@ -151,7 +149,7 @@ QString PX4AutoPilotPlugin::prerequisiteSetup(VehicleComponent* component) const return _airframeComponent->name(); } else if (_radioComponent && !_radioComponent->setupComplete()) { return _radioComponent->name(); - } else if (_sensorsComponent && !_vehicle->hilMode() && !_sensorsComponent->setupComplete()) { + } else if (_sensorsComponent && !_sensorsComponent->setupComplete()) { return _sensorsComponent->name(); } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2c1e26bc589fdbc56e1001f328e3a1270ed738a8..c273a0c0e3b54b47dbe6e4549ae3aef84079d98c 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -788,9 +788,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_WIND_COV: _handleWindCov(message); break; - case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: - _handleHilActuatorControls(message); - break; case MAVLINK_MSG_ID_LOGGING_DATA: _handleMavlinkLoggingData(message); break; @@ -1451,30 +1448,6 @@ QString Vehicle::vehicleUIDStr() return uid; } -void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) -{ - mavlink_hil_actuator_controls_t hil; - mavlink_msg_hil_actuator_controls_decode(&message, &hil); - emit hilActuatorControlsChanged(hil.time_usec, hil.flags, - hil.controls[0], - hil.controls[1], - hil.controls[2], - hil.controls[3], - hil.controls[4], - hil.controls[5], - hil.controls[6], - hil.controls[7], - hil.controls[8], - hil.controls[9], - hil.controls[10], - hil.controls[11], - hil.controls[12], - hil.controls[13], - hil.controls[14], - hil.controls[15], - hil.mode); -} - void Vehicle::_handleCommandLong(mavlink_message_t& message) { #ifdef NO_SERIAL_LINK @@ -2526,30 +2499,6 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName) } } -bool Vehicle::hilMode() -{ - return _base_mode & MAV_MODE_FLAG_HIL_ENABLED; -} - -void Vehicle::setHilMode(bool hilMode) -{ - mavlink_message_t msg; - - uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL; - if (hilMode) { - newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - id(), - newBaseMode, - _custom_mode); - sendMessageOnLink(priorityLink(), msg); -} - void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple) { mavlink_message_t msg; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index eb0d39b8f4f44922e75ce4c3a92b9614f465ebdb..6494e04dfafd5e01fbda11797064cc98314e921f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -543,7 +543,6 @@ public: Q_PROPERTY(QStringList flightModes READ flightModes NOTIFY flightModesChanged) Q_PROPERTY(QStringList extraJoystickFlightModes READ extraJoystickFlightModes NOTIFY flightModesChanged) Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) - Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) Q_PROPERTY(TrajectoryPoints* trajectoryPoints MEMBER _trajectoryPoints CONSTANT) Q_PROPERTY(QmlObjectListModel* cameraTriggerPoints READ cameraTriggerPoints CONSTANT) Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) @@ -866,9 +865,6 @@ public: QVariantList links() const; void setPriorityLinkByName(const QString& priorityLinkName); - bool hilMode(); - void setHilMode(bool hilMode); - bool fixedWing() const; bool multiRotor() const; bool vtol() const; @@ -1135,9 +1131,6 @@ signals: void armedPositionChanged(); void armedChanged (bool armed); void flightModeChanged (const QString& flightMode); - void hilModeChanged (bool hilMode); - /** @brief HIL actuator controls (replaces HIL controls) */ - void hilActuatorControlsChanged (quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode); void connectionLostChanged (bool connectionLost); void connectionLostEnabledChanged (bool connectionLostEnabled); void autoDisconnectChanged (bool autoDisconnectChanged); @@ -1299,7 +1292,6 @@ private: void _handleCommandLong (mavlink_message_t& message); void _handleAutopilotVersion (LinkInterface* link, mavlink_message_t& message); void _handleProtocolVersion (LinkInterface* link, mavlink_message_t& message); - void _handleHilActuatorControls (mavlink_message_t& message); void _handleGpsRawInt (mavlink_message_t& message); void _handleGlobalPositionInt (mavlink_message_t& message); void _handleAltitude (mavlink_message_t& message); diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h deleted file mode 100644 index 3d1ead78b36d1ae38bcfc2c90bc4fb7a7ff9c1ac..0000000000000000000000000000000000000000 --- a/src/comm/QGCHilLink.h +++ /dev/null @@ -1,149 +0,0 @@ -#pragma once - -#include -#include -#include "inttypes.h" - -class QGCHilLink : public QThread -{ - Q_OBJECT -public: - - virtual bool isConnected() = 0; - virtual qint64 bytesAvailable() = 0; - virtual int getPort() const = 0; - - /** - * @brief The human readable port name - */ - virtual QString getName() = 0; - - /** - * @brief Get remote host and port - * @return string in format : - */ - virtual QString getRemoteHost() = 0; - - /** - * @brief Get the application name and version - * @return A string containing a unique application name and compatibility version - */ - virtual QString getVersion() = 0; - - /** - * @brief Get index of currently selected airframe - * @return -1 if default is selected, index else - */ - virtual int getAirFrameIndex() = 0; - - /** - * @brief Check if sensor level HIL is enabled - * @return true if sensor HIL is enabled - */ - virtual bool sensorHilEnabled() = 0; - -public slots: - virtual void setPort(int port) = 0; - /** @brief Add a new host to broadcast messages to */ - virtual void setRemoteHost(const QString& host) = 0; - /** @brief Send new control states to the simulation */ - virtual void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) = 0; - virtual void processError(QProcess::ProcessError err) = 0; - /** @brief Set the simulator version as text string */ - virtual void setVersion(const QString& version) = 0; - /** @brief Enable sensor-level HIL (instead of state-level HIL) */ - virtual void enableSensorHIL(bool enable) = 0; - - virtual void selectAirframe(const QString& airframe) = 0; - - virtual void readBytes() = 0; - /** - * @brief Write a number of bytes to the interface. - * - * @param data Pointer to the data byte array - * @param size The size of the bytes array - **/ - void writeBytesSafe(const char* data, int length) - { - emit _invokeWriteBytes(QByteArray(data, length)); - } - - virtual bool connectSimulation() = 0; - virtual bool disconnectSimulation() = 0; - -private slots: - virtual void _writeBytes(const QByteArray) = 0; - -protected: - virtual void setName(QString name) = 0; - - QGCHilLink() : - QThread() - { - connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::_writeBytes); - } - -signals: - /** - * @brief This signal is emitted instantly when the link is connected - **/ - void simulationConnected(); - - /** - * @brief This signal is emitted instantly when the link is disconnected - **/ - void simulationDisconnected(); - - /** - * @brief Thread safe signal to disconnect simulator from other threads - **/ - void disconnectSim(); - - /** - * @brief This signal is emitted instantly when the link status changes - **/ - void simulationConnected(bool connected); - - /** @brief State update from simulation */ - void hilStateChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); - - void hilGroundTruthChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); - - void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites); - - void sensorHilRawImuChanged(quint64 time_us, float xacc, float yacc, float zacc, - float xgyro, float ygyro, float zgyro, - float xmag, float ymag, float zmag, - float abs_pressure, float diff_pressure, - float pressure_alt, float temperature, - quint32 fields_updated); - - void sensorHilOpticalFlowChanged(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance); - - /** @brief Remote host and port changed */ - void remoteChanged(const QString& hostPort); - - /** @brief Status text message from link */ - void statusMessage(const QString& message); - - /** @brief Airframe changed */ - void airframeChanged(const QString& airframe); - - /** @brief Selected sim version changed */ - void versionChanged(const QString& version); - - /** @brief Selected sim version changed */ - void versionChanged(const int version); - - /** @brief Sensor leve HIL state changed */ - void sensorHilChanged(bool enabled); - - /** @brief Helper signal to force execution on the correct thread */ - void _invokeWriteBytes(QByteArray); -}; - diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc deleted file mode 100644 index 57b41e3772aacdcbf2ba1689a3b7ebbe0c57f1e0..0000000000000000000000000000000000000000 --- a/src/comm/QGCJSBSimLink.cc +++ /dev/null @@ -1,399 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file - * @brief Definition of UDP connection (server) for unmanned vehicles - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include - -#include - -#include "UAS.h" -#include "QGCJSBSimLink.h" -#include "QGC.h" -//-- TODO: #include "QGCMessageBox.h" - -QGCJSBSimLink::QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) - : _vehicle(vehicle) - , socket(nullptr) - , process(nullptr) - , startupArguments(startupArguments) -{ - // We're doing it wrong - because the Qt folks got the API wrong: - // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ - moveToThread(this); - - this->host = host; - this->port = port + _vehicle->id(); - this->connectState = false; - this->currentPort = 49000 + _vehicle->id(); - this->name = tr("JSBSim Link (port:%1)").arg(port); - setRemoteHost(remoteHost); -} - -QGCJSBSimLink::~QGCJSBSimLink() -{ //do not disconnect unless it is connected. - //disconnectSimulation will delete the memory that was allocated for process, terraSync and socket - if(connectState){ - disconnectSimulation(); - } -} - -/** - * @brief Runs the thread - * - **/ -void QGCJSBSimLink::run() -{ - qDebug() << "STARTING FLIGHTGEAR LINK"; - - if (!_vehicle) return; - socket = new QUdpSocket(this); - socket->moveToThread(this); - connectState = socket->bind(host, port, QAbstractSocket::ReuseAddressHint); - - QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCJSBSimLink::readBytes); - - process = new QProcess(this); - - connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCJSBSimLink::updateControls); - connect(this, &QGCJSBSimLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState); - - _vehicle->uas()->startHil(); - - //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); - // Catch process error - connect(process, static_cast(&QProcess::error), - this, &QGCJSBSimLink::processError); - - // Start Flightgear - QStringList arguments; - QString processJSB; - QString rootJSB; - -#ifdef Q_OS_MACX - processJSB = "/usr/local/bin/JSBSim"; - rootJSB = "/Applications/FlightGear.app/Contents/Resources/data"; -#endif - -#ifdef Q_OS_WIN32 - processJSB = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; - rootJSB = "C:\\Program Files (x86)\\FlightGear\\data"; -#endif - -#ifdef Q_OS_LINUX - processJSB = "/usr/games/fgfs"; - rootJSB = "/usr/share/games/flightgear"; -#endif - - // Sanity checks - bool sane = true; - QFileInfo executable(processJSB); - if (!executable.isExecutable()) - { - //-- TODO: QGCMessageBox::critical("JSBSim", tr("JSBSim failed to start. JSBSim was not found at %1").arg(processJSB)); - sane = false; - } - - QFileInfo root(rootJSB); - if (!root.isDir()) - { - //-- TODO: QGCMessageBox::critical("JSBSim", tr("JSBSim failed to start. JSBSim data directory was not found at %1").arg(rootJSB)); - sane = false; - } - - if (!sane) return; - - /*Prepare JSBSim Arguments */ - - if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) - { - arguments << QString("--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB, rootJSB, script); - } - else - { - arguments << QString("JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB, rootJSB, script); - } - - process->start(processJSB, arguments); - - emit simulationConnected(connectState); - if (connectState) { - emit simulationConnected(); - connectionStartTime = QGC::groundTimeUsecs()/1000; - } - qDebug() << "STARTING SIM"; - - exec(); -} - -void QGCJSBSimLink::setPort(int port) -{ - this->port = port; - disconnectSimulation(); - connectSimulation(); -} - -void QGCJSBSimLink::processError(QProcess::ProcessError err) -{ - QString msg; - - switch(err) { - case QProcess::FailedToStart: - msg = tr("JSBSim Failed to start. Please check if the path and command is correct"); - break; - - case QProcess::Crashed: - msg = tr("JSBSim crashed. This is a JSBSim-related problem, check for JSBSim upgrade."); - break; - - case QProcess::Timedout: - msg = tr("JSBSim start timed out. Please check if the path and command is correct"); - break; - - case QProcess::ReadError: - case QProcess::WriteError: - msg = tr("Could not communicate with JSBSim. Please check if the path and command are correct"); - break; - - case QProcess::UnknownError: - default: - msg = tr("JSBSim error occurred. Please check if the path and command is correct."); - break; - } - - //-- TODO: QGCMessageBox::critical("JSBSim HIL", msg); -} - -/** - * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 - */ -void QGCJSBSimLink::setRemoteHost(const QString& host) -{ - if (host.contains(":")) - { - //qDebug() << "HOST: " << host.split(":").first(); - QHostInfo info = QHostInfo::fromName(host.split(":").first()); - if (info.error() == QHostInfo::NoError) - { - // Add host - QList hostAddresses = info.addresses(); - QHostAddress address; - for (int i = 0; i < hostAddresses.size(); i++) - { - // Exclude loopback IPv4 and all IPv6 addresses - if (!hostAddresses.at(i).toString().contains(":")) - { - address = hostAddresses.at(i); - } - } - currentHost = address; - //qDebug() << "Address:" << address.toString(); - // Set port according to user input - currentPort = host.split(":").last().toInt(); - } - } - else - { - QHostInfo info = QHostInfo::fromName(host); - if (info.error() == QHostInfo::NoError) - { - // Add host - currentHost = info.addresses().first(); - } - } - -} - -void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) -{ - // magnetos,aileron,elevator,rudder,throttle\n - - //float magnetos = 3.0f; - Q_UNUSED(time); - Q_UNUSED(systemMode); - Q_UNUSED(navMode); - - if(!qIsNaN(rollAilerons) && !qIsNaN(pitchElevator) && !qIsNaN(yawRudder) && !qIsNaN(throttle)) - { - QString state("%1\t%2\t%3\t%4\t%5\n"); - state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle); - emit _invokeWriteBytes(state.toLatin1()); - } - else - { - qDebug() << "HIL: Got NaN values from the hardware: isnan output: roll: " << qIsNaN(rollAilerons) << ", pitch: " << qIsNaN(pitchElevator) << ", yaw: " << qIsNaN(yawRudder) << ", throttle: " << qIsNaN(throttle); - } - //qDebug() << "Updated controls" << state; -} - -void QGCJSBSimLink::_writeBytes(const QByteArray data) -{ - //#define QGCJSBSimLink_DEBUG -#ifdef QGCJSBSimLink_DEBUG - QString bytes; - QString ascii; - for (int i=0, size = data.size(); i 31 && data[i] < 127) - { - ascii.append(data[i]); - } - else - { - ascii.append(219); - } - } - qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:"; - qDebug() << bytes; - qDebug() << "ASCII:" << ascii; -#endif - if (connectState && socket) socket->writeDatagram(data, currentHost, currentPort); -} - -/** - * @brief Read a number of bytes from the interface. - * - * @param data Pointer to the data byte array to write the bytes to - * @param maxLength The maximum number of bytes to write - **/ -void QGCJSBSimLink::readBytes() -{ - const qint64 maxLength = 65536; - char data[maxLength]; - QHostAddress sender; - quint16 senderPort; - - unsigned int s = socket->pendingDatagramSize(); - if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size" << std::endl; - socket->readDatagram(data, maxLength, &sender, &senderPort); - - /* - // Print string - QByteArray b(data, s); - QString state(b); - - // Parse string - float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; - double lat, lon, alt; - double vx, vy, vz, xacc, yacc, zacc; - - // Send updated state - emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, - pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); - */ - - // Echo data for debugging purposes - std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; - for (unsigned int i=0; ipendingDatagramSize(); -} - -/** - * @brief Disconnect the connection. - * - * @return True if connection has been disconnected, false if connection couldn't be disconnected. - **/ -bool QGCJSBSimLink::disconnectSimulation() -{ - disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCJSBSimLink::updateControls); - disconnect(this, &QGCJSBSimLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState); - disconnect(process, static_cast(&QProcess::error), - this, &QGCJSBSimLink::processError); - - if (process) - { - process->close(); - delete process; - process = nullptr; - } - if (socket) - { - socket->close(); - delete socket; - socket = nullptr; - } - - connectState = false; - - emit simulationDisconnected(); - emit simulationConnected(false); - return !connectState; -} - -/** - * @brief Connect the connection. - * - * @return True if connection has been established, false if connection couldn't be established. - **/ -bool QGCJSBSimLink::connectSimulation() -{ - start(HighPriority); - return true; -} - -/** - * @brief Set the startup arguments used to start flightgear - * - **/ -void QGCJSBSimLink::setStartupArguments(QString startupArguments) -{ - this->startupArguments = startupArguments; -} - -/** - * @brief Check if connection is active. - * - * @return True if link is connected, false otherwise. - **/ -bool QGCJSBSimLink::isConnected() -{ - return connectState; -} - -QString QGCJSBSimLink::getName() -{ - return name; -} - -QString QGCJSBSimLink::getRemoteHost() -{ - return QString("%1:%2").arg(currentHost.toString(), currentPort); -} - -void QGCJSBSimLink::setName(QString name) -{ - this->name = name; -} diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h deleted file mode 100644 index a8d671bef3d1f88da5716a254383e48da6a3a188..0000000000000000000000000000000000000000 --- a/src/comm/QGCJSBSimLink.h +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file - * @brief UDP connection (server) for unmanned vehicles - * @author Lorenz Meier - * - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include "QGCConfig.h" -#include "QGCHilLink.h" -#include "Vehicle.h" - -class QGCJSBSimLink : public QGCHilLink -{ - Q_OBJECT - //Q_INTERFACES(QGCJSBSimLinkInterface:LinkInterface) - -public: - QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); - ~QGCJSBSimLink(); - - bool isConnected(); - qint64 bytesAvailable(); - int getPort() const { - return port; - } - - /** - * @brief The human readable port name - */ - QString getName(); - - /** - * @brief Get remote host and port - * @return string in format : - */ - QString getRemoteHost(); - - QString getVersion() - { - return QString("FlightGear %1").arg(flightGearVersion); - } - - int getAirFrameIndex() - { - return -1; - } - - void run(); - - bool sensorHilEnabled() { - return _sensorHilEnabled; - } - -public slots: -// void setAddress(QString address); - void setPort(int port); - /** @brief Add a new host to broadcast messages to */ - void setRemoteHost(const QString& host); - /** @brief Send new control states to the simulation */ - void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); -// /** @brief Remove a host from broadcasting messages to */ -// void removeHost(const QString& host); - // void readPendingDatagrams(); - void processError(QProcess::ProcessError err); - /** @brief Set the simulator version as text string */ - void setVersion(const QString& version) - { - Q_UNUSED(version); - } - - void selectAirframe(const QString& airframe) - { - script = airframe; - } - - void enableSensorHIL(bool enable) { - if (enable != _sensorHilEnabled) { - _sensorHilEnabled = enable; - emit sensorHilChanged(enable); - } - } - - void readBytes(); - -private slots: - /** - * @brief Write a number of bytes to the interface. - * - * @param data Pointer to the data byte array - * @param size The size of the bytes array - **/ - void _writeBytes(const QByteArray data); - -public slots: - bool connectSimulation(); - bool disconnectSimulation(); - - void setStartupArguments(QString startupArguments); - -private: - Vehicle* _vehicle; - QString name; - QHostAddress host; - QHostAddress currentHost; - quint16 currentPort; - quint16 port; - int id; - QUdpSocket* socket; - bool connectState; - - quint64 bitsSentTotal; - quint64 bitsSentCurrent; - quint64 bitsSentMax; - quint64 bitsReceivedTotal; - quint64 bitsReceivedCurrent; - quint64 bitsReceivedMax; - quint64 connectionStartTime; - QMutex statisticsMutex; - QMutex dataMutex; - QTimer refreshTimer; - QProcess* process; - unsigned int flightGearVersion; - QString startupArguments; - QString script; - bool _sensorHilEnabled; - - void setName(QString name); -}; - diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc deleted file mode 100644 index 341f74729ec0b6a1f7bec85fed0a5386206fce40..0000000000000000000000000000000000000000 --- a/src/comm/QGCXPlaneLink.cc +++ /dev/null @@ -1,1166 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file QGCXPlaneLink.cc - * Implementation of X-Plane interface - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "QGCXPlaneLink.h" -#include "QGC.h" -#include "UAS.h" -#include "UASInterface.h" -//-- TODO: #include "QGCMessageBox.h" - -QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) : - _vehicle(vehicle), - remoteHost(QHostAddress("127.0.0.1")), - remotePort(49000), - socket(nullptr), - process(nullptr), - terraSync(nullptr), - barometerOffsetkPa(-8.0f), - airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN), - xPlaneConnected(false), - xPlaneVersion(0), - simUpdateLast(QGC::groundTimeMilliseconds()), - simUpdateFirst(0), - simUpdateLastText(QGC::groundTimeMilliseconds()), - simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()), - simUpdateHz(0), - _sensorHilEnabled(true), - _useHilActuatorControls(true), - _should_exit(false) -{ - // We're doing it wrong - because the Qt folks got the API wrong: - // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ - moveToThread(this); - - setTerminationEnabled(false); - - this->localHost = localHost; - this->localPort = localPort/*+mav->getUASID()*/; - connectState = false; - - this->name = tr("X-Plane Link (localPort:%1)").arg(localPort); - setRemoteHost(remoteHost); - loadSettings(); -} - -QGCXPlaneLink::~QGCXPlaneLink() -{ - storeSettings(); - // Tell the thread to exit - _should_exit = true; - - if (socket) { - socket->close(); - socket->deleteLater(); - socket = nullptr; - } -} - -void QGCXPlaneLink::loadSettings() -{ - // Load defaults from settings - QSettings settings; - settings.beginGroup("QGC_XPLANE_LINK"); - setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString()); - setVersion(settings.value("XPLANE_VERSION", 10).toInt()); - selectAirframe(settings.value("AIRFRAME", "default").toString()); - _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool(); - _useHilActuatorControls = settings.value("ACTUATOR_HIL", _useHilActuatorControls).toBool(); - settings.endGroup(); -} - -void QGCXPlaneLink::storeSettings() -{ - // Store settings - QSettings settings; - settings.beginGroup("QGC_XPLANE_LINK"); - settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)); - settings.setValue("XPLANE_VERSION", xPlaneVersion); - settings.setValue("AIRFRAME", airframeName); - settings.setValue("SENSOR_HIL", _sensorHilEnabled); - settings.setValue("ACTUATOR_HIL", _useHilActuatorControls); - settings.endGroup(); -} - -void QGCXPlaneLink::setVersion(const QString& version) -{ - unsigned int oldVersion = xPlaneVersion; - if (version.contains("9")) - { - xPlaneVersion = 9; - } - else if (version.contains("10")) - { - xPlaneVersion = 10; - } - else if (version.contains("11")) - { - xPlaneVersion = 11; - } - else if (version.contains("12")) - { - xPlaneVersion = 12; - } - - if (oldVersion != xPlaneVersion) - { - emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); - } -} - -void QGCXPlaneLink::setVersion(unsigned int version) -{ - bool changed = (xPlaneVersion != version); - xPlaneVersion = version; - if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); -} - -void QGCXPlaneLink::enableHilActuatorControls(bool enable) -{ - if (enable != _useHilActuatorControls) { - _useHilActuatorControls = enable; - } - - /* Only use override for new message and specific airframes */ - MAV_TYPE type = _vehicle->vehicleType(); - float value = 0.0f; - if (type == MAV_TYPE_VTOL_RESERVED2) { - value = (enable ? 1.0f : 0.0f); - } - - sendDataRef("sim/operation/override/override_control_surfaces", value); - emit useHilActuatorControlsChanged(enable); -} - - -/** - * @brief Runs the thread - * - **/ -void QGCXPlaneLink::run() -{ - if (!_vehicle) { - emit statusMessage("No MAV present"); - return; - } - - if (connectState) { - emit statusMessage("Already connected"); - return; - } - - socket = new QUdpSocket(this); - socket->moveToThread(this); - connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); - if (!connectState) { - - emit statusMessage("Binding socket failed!"); - - socket->deleteLater(); - socket = nullptr; - return; - } - - emit statusMessage(tr("Waiting for XPlane..")); - - QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes); - - connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection); - connect(_vehicle, &Vehicle::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection); - - connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection); - connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection); - connect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps, Qt::QueuedConnection); - connect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors, Qt::QueuedConnection); - - _vehicle->uas()->startHil(); - -#pragma pack(push, 1) - struct iset_struct - { - char b[5]; - int index; // (0->20 in the list below) - char str_ipad_them[16]; - char str_port_them[6]; - char padding[2]; - int use_ip; - } ip; // to use this option, 0 not to. -#pragma pack(pop) - - ip.b[0] = 'I'; - ip.b[1] = 'S'; - ip.b[2] = 'E'; - ip.b[3] = 'T'; - ip.b[4] = '0'; - - QList hostAddresses = QNetworkInterface::allAddresses(); - - QString localAddrStr; - QString localPortStr = QString("%1").arg(localPort); - - for (int i = 0; i < hostAddresses.size(); i++) - { - // Exclude loopback IPv4 and all IPv6 addresses - if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) - { - localAddrStr = hostAddresses.at(i).toString(); - break; - } - } - - ip.index = 0; - strncpy(ip.str_ipad_them, localAddrStr.toLatin1(), qMin((int)sizeof(ip.str_ipad_them), 16)); - strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6)); - ip.use_ip = 1; - - writeBytesSafe((const char*)&ip, sizeof(ip)); - - /* Call function which makes sure individual control override is enabled/disabled */ - enableHilActuatorControls(_useHilActuatorControls); - - _should_exit = false; - - while(!_should_exit) { - QCoreApplication::processEvents(); - QGC::SLEEP::msleep(5); - } - - disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls); - - disconnect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth); - disconnect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState); - disconnect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps); - disconnect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors); - connectState = false; - - disconnect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes); - - socket->close(); - socket->deleteLater(); - socket = nullptr; - - emit simulationDisconnected(); - emit simulationConnected(false); -} - -void QGCXPlaneLink::setPort(int localPort) -{ - this->localPort = localPort; - disconnectSimulation(); - connectSimulation(); -} - -void QGCXPlaneLink::processError(QProcess::ProcessError err) -{ - QString msg; - - switch(err) { - case QProcess::FailedToStart: - msg = tr("X-Plane Failed to start. Please check if the path and command is correct"); - break; - - case QProcess::Crashed: - msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade."); - break; - - case QProcess::Timedout: - msg = tr("X-Plane start timed out. Please check if the path and command is correct"); - break; - - case QProcess::ReadError: - case QProcess::WriteError: - msg = tr("Could not communicate with X-Plane. Please check if the path and command are correct"); - break; - - case QProcess::UnknownError: - default: - msg = tr("X-Plane error occurred. Please check if the path and command is correct."); - break; - } - - - //-- TODO: QGCMessageBox::critical(tr("X-Plane HIL"), msg); -} - -QString QGCXPlaneLink::getRemoteHost() -{ - return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort); -} - -/** - * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 - */ -void QGCXPlaneLink::setRemoteHost(const QString& newHost) -{ - if (newHost.length() == 0) - return; - - if (newHost.contains(":")) - { - QHostInfo info = QHostInfo::fromName(newHost.split(":").first()); - if (info.error() == QHostInfo::NoError) - { - // Add newHost - QList newHostAddresses = info.addresses(); - QHostAddress address; - for (int i = 0; i < newHostAddresses.size(); i++) - { - // Exclude loopback IPv4 and all IPv6 addresses - if (!newHostAddresses.at(i).toString().contains(":")) - { - address = newHostAddresses.at(i); - } - } - remoteHost = address; - // Set localPort according to user input - remotePort = newHost.split(":").last().toInt(); - } - } - else - { - QHostInfo info = QHostInfo::fromName(newHost); - if (info.error() == QHostInfo::NoError) - { - // Add newHost - remoteHost = info.addresses().first(); - if (remotePort == 0) remotePort = 49000; - } - } - - if (isConnected()) - { - disconnectSimulation(); - connectSimulation(); - } - - emit remoteChanged(QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)); -} - -void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) -{ - /* Only use HIL_CONTROL when the checkbox is unchecked */ - if (_useHilActuatorControls) { - //qDebug() << "received HIL_CONTROL but not using it"; - return; - } - #pragma pack(push, 1) - struct payload { - char b[5]; - int index; - float f[8]; - } p; - #pragma pack(pop) - - p.b[0] = 'D'; - p.b[1] = 'A'; - p.b[2] = 'T'; - p.b[3] = 'A'; - p.b[4] = '\0'; - - Q_UNUSED(time); - Q_UNUSED(systemMode); - Q_UNUSED(navMode); - - if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR - || _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR - || _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR) - { - qDebug() << "MAV_TYPE_QUADROTOR"; - - // Individual effort will be provided directly to the actuators on Xplane quadrotor. - p.f[0] = yawRudder; - p.f[1] = rollAilerons; - p.f[2] = throttle; - p.f[3] = pitchElevator; - - // Direct throttle control - p.index = 25; - writeBytesSafe((const char*)&p, sizeof(p)); - } - else - { - // direct pass-through, normal fixed-wing. - p.f[0] = -pitchElevator; - p.f[1] = rollAilerons; - p.f[2] = yawRudder; - - // Ail / Elevon / Rudder - - // Send to group 12 - p.index = 12; - writeBytesSafe((const char*)&p, sizeof(p)); - - // Send to group 8, which equals manual controls - p.index = 8; - writeBytesSafe((const char*)&p, sizeof(p)); - - // Send throttle to all four motors - p.index = 25; - memset(p.f, 0, sizeof(p.f)); - p.f[0] = throttle; - p.f[1] = throttle; - p.f[2] = throttle; - p.f[3] = throttle; - writeBytesSafe((const char*)&p, sizeof(p)); - } -} - -void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode) -{ - if (!_useHilActuatorControls) { - //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it"; - return; - } - - Q_UNUSED(time); - Q_UNUSED(flags); - Q_UNUSED(mode); - Q_UNUSED(ctl_12); - Q_UNUSED(ctl_13); - Q_UNUSED(ctl_14); - Q_UNUSED(ctl_15); - - #pragma pack(push, 1) - struct payload { - char b[5]; - int index; - float f[8]; - } p; - #pragma pack(pop) - - p.b[0] = 'D'; - p.b[1] = 'A'; - p.b[2] = 'T'; - p.b[3] = 'A'; - p.b[4] = '\0'; - - /* Initialize with zeroes */ - memset(p.f, 0, sizeof(p.f)); - - switch (_vehicle->vehicleType()) { - case MAV_TYPE_QUADROTOR: - case MAV_TYPE_HEXAROTOR: - case MAV_TYPE_OCTOROTOR: - { - p.f[0] = ctl_0; ///< X-Plane Engine 1 - p.f[1] = ctl_1; ///< X-Plane Engine 2 - p.f[2] = ctl_2; ///< X-Plane Engine 3 - p.f[3] = ctl_3; ///< X-Plane Engine 4 - p.f[4] = ctl_4; ///< X-Plane Engine 5 - p.f[5] = ctl_5; ///< X-Plane Engine 6 - p.f[6] = ctl_6; ///< X-Plane Engine 7 - p.f[7] = ctl_7; ///< X-Plane Engine 8 - - /* Direct throttle control */ - p.index = 25; - writeBytesSafe((const char*)&p, sizeof(p)); - break; - } - case MAV_TYPE_VTOL_RESERVED2: - { - /** - * Tailsitter with four control flaps and eight motors. - */ - - /* Throttle channels */ - p.f[0] = ctl_0; - p.f[1] = ctl_1; - p.f[2] = ctl_2; - p.f[3] = ctl_3; - p.f[4] = ctl_4; - p.f[5] = ctl_5; - p.f[6] = ctl_6; - p.f[7] = ctl_7; - p.index = 25; - writeBytesSafe((const char*)&p, sizeof(p)); - - /* Control individual actuators */ - float max_surface_deflection = 30.0f; // Degrees - sendDataRef("sim/flightmodel/controls/wing1l_ail1def", ctl_8 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing1r_ail1def", ctl_9 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing2l_ail1def", ctl_10 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing2r_ail1def", ctl_11 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing1l_ail2def", ctl_12 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing1r_ail2def", ctl_13 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing2l_ail2def", ctl_14 * max_surface_deflection); - sendDataRef("sim/flightmodel/controls/wing2r_ail2def", ctl_15 * max_surface_deflection); - - break; - } - default: - { - /* direct pass-through, normal fixed-wing. */ - p.f[0] = -ctl_1; ///< X-Plane Elevator - p.f[1] = ctl_0; ///< X-Plane Aileron - p.f[2] = ctl_2; ///< X-Plane Rudder - - /* Send to group 8, which equals manual controls */ - p.index = 8; - writeBytesSafe((const char*)&p, sizeof(p)); - - /* Send throttle to all eight motors */ - p.index = 25; - p.f[0] = ctl_3; - p.f[1] = ctl_3; - p.f[2] = ctl_3; - p.f[3] = ctl_3; - p.f[4] = ctl_3; - p.f[5] = ctl_3; - p.f[6] = ctl_3; - p.f[7] = ctl_3; - writeBytesSafe((const char*)&p, sizeof(p)); - - /* Send flap signals, assuming that they are mapped to channel 5 (ctl_4) */ - sendDataRef("sim/flightmodel/controls/flaprqst", ctl_4); - sendDataRef("sim/flightmodel/controls/flap2rqst", ctl_4); - break; - } - - } - -} - -Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) { - double c__ = cos(yaw); - double _c_ = cos(pitch); - double __c = cos(roll); - double s__ = sin(yaw); - double _s_ = sin(pitch); - double __s = sin(roll); - double cc_ = c__ * _c_; - double cs_ = c__ * _s_; - double sc_ = s__ * _c_; - double ss_ = s__ * _s_; - double c_c = c__ * __c; - double c_s = c__ * __s; - double s_c = s__ * __c; - double s_s = s__ * __s; - double _cc = _c_ * __c; - double _cs = _c_ * __s; - double csc = cs_ * __c; - double css = cs_ * __s; - double ssc = ss_ * __c; - double sss = ss_ * __s; - Eigen::Matrix3f wRo; - wRo << - cc_ , css-s_c, csc+s_s, - sc_ , sss+c_c, ssc-c_s, - -_s_ , _cs, _cc; - return wRo; -} - -void QGCXPlaneLink::_writeBytes(const QByteArray data) -{ - if (data.isEmpty()) return; - - // If socket exists and is connected, transmit the data - if (socket && connectState) - { - socket->writeDatagram(data, remoteHost, remotePort); - } -} - -/** - * @brief Read all pending packets from the interface. - **/ -void QGCXPlaneLink::readBytes() -{ - // Only emit updates on attitude message - bool emitUpdate = false; - quint16 fields_changed = 0; - - const qint64 maxLength = 65536; - char data[maxLength]; - QHostAddress sender; - quint16 senderPort; - - unsigned int s = socket->pendingDatagramSize(); - if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl; - socket->readDatagram(data, maxLength, &sender, &senderPort); - if (s > maxLength) { - std::string headStr = std::string(data, data+5); - std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; - } - - // Calculate the number of data segments a 36 bytes - // XPlane always has 5 bytes header: 'DATA@' - unsigned nsegs = (s-5)/36; - - //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs; - - #pragma pack(push, 1) - struct payload { - int index; - float f[8]; - } p; - #pragma pack(pop) - - bool oldConnectionState = xPlaneConnected; - - if (data[0] == 'D' && - data[1] == 'A' && - data[2] == 'T' && - data[3] == 'A') - { - xPlaneConnected = true; - - if (oldConnectionState != xPlaneConnected) { - simUpdateFirst = QGC::groundTimeMilliseconds(); - } - - for (unsigned i = 0; i < nsegs; i++) - { - // Get index - unsigned ioff = (5+i*36);; - memcpy(&(p), data+ioff, sizeof(p)); - - if (p.index == 3) - { - float knotsToMetersPerSecond = 0.514444f; - ind_airspeed = p.f[5] * knotsToMetersPerSecond; - true_airspeed = p.f[6] * knotsToMetersPerSecond; - groundspeed = p.f[7] * knotsToMetersPerSecond; - - //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; - } - if (p.index == 4) - { - // WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested - // with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings - // before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration. - // Instead, we calculate our own accelerations. - if (fabsf(groundspeed)<0.1f && alt_agl<1.0) - { - // TODO: Add centrip. acceleration to the current static acceleration implementation. - Eigen::Vector3f g(0, 0, -9.80665f); - Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll); - Eigen::Vector3f gr = R.transpose().eval() * g; - - xacc = gr[0]; - yacc = gr[1]; - zacc = gr[2]; - - //qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2]; - } - else - { - // Accelerometer readings, directly from X-Plane and including centripetal forces. - const float one_g = 9.80665f; - xacc = p.f[5] * one_g; - yacc = p.f[6] * one_g; - zacc = -p.f[4] * one_g; - - //qDebug() << "X-Plane values" << xacc << yacc << zacc; - } - - fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); - emitUpdate = true; - } - // atmospheric pressure aircraft for XPlane 9 and 10 - else if (p.index == 6) - { - // inHg to hPa (hecto Pascal / millibar) - abs_pressure = p.f[0] * 33.863886666718317f; - temperature = p.f[1]; - fields_changed |= (1 << 9) | (1 << 12); - } - // Forward controls from X-Plane to MAV, not very useful - // better: Connect Joystick to QGroundControl -// else if (p.index == 8) -// { -// //qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7]; -// man_roll = p.f[0]; -// man_pitch = p.f[1]; -// man_yaw = p.f[2]; -// UAS* uas = dynamic_cast(mav); -// if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6); -// } - else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17)) - { - // Cross checked with XPlane flight - pitchspeed = p.f[0]; - rollspeed = p.f[1]; - yawspeed = p.f[2]; - fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); - - emitUpdate = true; - } - else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) - { - //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3]; - pitch = p.f[0] / 180.0f * M_PI; - roll = p.f[1] / 180.0f * M_PI; - yaw = p.f[2] / 180.0f * M_PI; - - // X-Plane expresses yaw as 0..2 PI - if (yaw > M_PI) { - yaw -= 2.0f * static_cast(M_PI); - } - if (yaw < -M_PI) { - yaw += 2.0f * static_cast(M_PI); - } - - float yawmag = p.f[3] / 180.0f * M_PI; - - if (yawmag > M_PI) { - yawmag -= 2.0f * static_cast(M_PI); - } - if (yawmag < -M_PI) { - yawmag += 2.0f * static_cast(M_PI); - } - - // Normal rotation matrix, but since we rotate the - // vector [0.25 0 0.45]', we end up with these relevant - // matrix parts. - - xmag = cos(-yawmag) * 0.25f; - ymag = sin(-yawmag) * 0.25f; - zmag = 0.45f; - fields_changed |= (1 << 6) | (1 << 7) | (1 << 8); - - double cosPhi = cos(roll); - double sinPhi = sin(roll); - double cosThe = cos(pitch); - double sinThe = sin(pitch); - double cosPsi = cos(0.0); - double sinPsi = sin(0.0); - - float dcm[3][3]; - - dcm[0][0] = cosThe * cosPsi; - dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm[1][0] = cosThe * sinPsi; - dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm[2][0] = -sinThe; - dcm[2][1] = sinPhi * cosThe; - dcm[2][2] = cosPhi * cosThe; - - Eigen::Matrix3f m = Eigen::Map((float*)dcm).eval(); - - Eigen::Vector3f mag(xmag, ymag, zmag); - - Eigen::Vector3f magbody = m * mag; - -// qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag; -// qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2); - - xmag = magbody(0); - ymag = magbody(1); - zmag = magbody(2); - - // Rotate the measurement vector into the body frame using roll and pitch - - - emitUpdate = true; - } - -// else if (p.index == 19) -// { -// qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2]; -// } - else if (p.index == 20) - { - //qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2]; - lat = p.f[0]; - lon = p.f[1]; - alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters - alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters - } - else if (p.index == 21) - { - vy = p.f[3]; - vx = -p.f[5]; - // moving 'up' in XPlane is positive, but its negative in NED - // for us. - vz = -p.f[4]; - } - else if (p.index == 12) - { - //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2]; - } - else if (p.index == 25) - { - //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3]; - } - else if (p.index == 0) - { - //qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6]; - } - else if (p.index == 11) - { - //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3]; - } - else - { - //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3]; - } - } - } - else if (data[0] == 'S' && - data[1] == 'N' && - data[2] == 'A' && - data[3] == 'P') - { - - } - else if (data[0] == 'S' && - data[1] == 'T' && - data[2] == 'A' && - data[3] == 'T') - { - - } - else - { - qDebug() << "UNKNOWN PACKET:" << data; - } - - // Wait for 0.5s before actually using the data, so that all fields are filled - if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) { - return; - } - - // Send updated state - if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) - { - simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); - if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) { - emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast(simUpdateHz))); - // Reset lowpass with current value - simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); - // Set state - simUpdateLastText = QGC::groundTimeMilliseconds(); - } - - simUpdateLast = QGC::groundTimeMilliseconds(); - - if (_sensorHilEnabled) - { - diff_pressure = (ind_airspeed * ind_airspeed * 1.225f) / 2.0f; - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = 1013.25 / 10.0; - - /* measured pressure in hPa, plus offset to simulate weather effects / offsets */ - double p = abs_pressure / 10.0 + barometerOffsetkPa; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - pressure_alt = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - - // set pressure alt to changed - fields_changed |= (1 << 11); - - emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, - xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed); - - // XXX make these GUI-configurable and add randomness - int gps_fix_type = 3; - float eph = 0.3f; - float epv = 0.6f; - float vel = sqrt(vx*vx + vy*vy + vz*vz); - float cog = atan2(vy, vx); - int satellites = 8; - - emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); - } else { - emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, - pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); - } - - // Limit ground truth to 25 Hz - if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) { - emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, - pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); - - simUpdateLastGroundTruth = QGC::groundTimeMilliseconds(); - } - } - - if (!oldConnectionState && xPlaneConnected) - { - emit statusMessage(tr("Receiving from XPlane.")); - } - - // // Echo data for debugging purposes - // std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; - // int i; - // for (i=0; ipendingDatagramSize(); -} - -/** - * @brief Disconnect the connection. - * - * @return True if connection has been disconnected, false if connection couldn't be disconnected. - **/ -bool QGCXPlaneLink::disconnectSimulation() -{ - if (connectState) - { - _should_exit = true; - } else { - emit simulationDisconnected(); - emit simulationConnected(false); - } - - return !connectState; -} - -void QGCXPlaneLink::selectAirframe(const QString& plane) -{ - airframeName = plane; - - if (plane.contains("QRO")) - { - if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C) - { - airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C; - emit airframeChanged("QRO_X / MK"); - } - else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE) - { - airframeID = AIRFRAME_QUAD_X_ARDRONE; - emit airframeChanged("QRO_X / ARDRONE"); - } - else - { - bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM); - airframeID = AIRFRAME_QUAD_DJI_F450_PWM; - if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM"); - } - } - else - { - bool changed = (airframeID != AIRFRAME_UNKNOWN); - airframeID = AIRFRAME_UNKNOWN; - if (changed) emit airframeChanged("X Plane default"); - } -} - -void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw) -{ - #pragma pack(push, 1) - struct VEH1_struct - { - char header[5]; - quint32 p; - double lat_lon_ele[3]; - float psi_the_phi[3]; - float gear_flap_vect[3]; - } pos; - #pragma pack(pop) - - pos.header[0] = 'V'; - pos.header[1] = 'E'; - pos.header[2] = 'H'; - pos.header[3] = '1'; - pos.header[4] = '0'; - pos.p = 0; - pos.lat_lon_ele[0] = lat; - pos.lat_lon_ele[1] = lon; - pos.lat_lon_ele[2] = alt; - pos.psi_the_phi[0] = roll; - pos.psi_the_phi[1] = pitch; - pos.psi_the_phi[2] = yaw; - pos.gear_flap_vect[0] = 0.0f; - pos.gear_flap_vect[1] = 0.0f; - pos.gear_flap_vect[2] = 0.0f; - - writeBytesSafe((const char*)&pos, sizeof(pos)); - -// pos.header[0] = 'V'; -// pos.header[1] = 'E'; -// pos.header[2] = 'H'; -// pos.header[3] = '1'; -// pos.header[4] = '0'; -// pos.p = 0; -// pos.lat_lon_ele[0] = -999; -// pos.lat_lon_ele[1] = -999; -// pos.lat_lon_ele[2] = -999; -// pos.psi_the_phi[0] = -999; -// pos.psi_the_phi[1] = -999; -// pos.psi_the_phi[2] = -999; -// pos.gear_flap_vect[0] = -999; -// pos.gear_flap_vect[1] = -999; -// pos.gear_flap_vect[2] = -999; - -// writeBytesSafe((const char*)&pos, sizeof(pos)); -} - -/** - * Sets a random position with an offset of max 1/1000 degree - * and max 100 m altitude - */ -void QGCXPlaneLink::setRandomPosition() -{ - // Initialize generator - srand(0); - - double offLat = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; - double offLon = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; - double offAlt = rand() / static_cast(RAND_MAX) * 200.0 + 100.0; - - if (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0) - { - offAlt *= -1.0; - } - - setPositionAttitude(_vehicle->latitude() + offLat, - _vehicle->longitude() + offLon, - _vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt, - _vehicle->roll()->rawValue().toDouble(), - _vehicle->pitch()->rawValue().toDouble(), - _vehicle->heading()->rawValue().toDouble()); -} - -void QGCXPlaneLink::setRandomAttitude() -{ - // Initialize generator - srand(0); - - double roll = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; - double pitch = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; - double yaw = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; - - setPositionAttitude(_vehicle->latitude(), - _vehicle->longitude(), - _vehicle->altitudeAMSL()->rawValue().toDouble(), - roll, - pitch, - yaw); -} - -/** - * @brief Connect the connection. - * - * @return True if connection has been established, false if connection couldn't be established. - **/ -bool QGCXPlaneLink::connectSimulation() -{ - if (connectState) { - qDebug() << "Simulation already active"; - } else { - qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; - // XXX Hack - storeSettings(); - - start(HighPriority); - } - - return true; -} - -/** - * @brief Check if connection is active. - * - * @return True if link is connected, false otherwise. - **/ -bool QGCXPlaneLink::isConnected() -{ - return connectState; -} - -QString QGCXPlaneLink::getName() -{ - return name; -} - -void QGCXPlaneLink::setName(QString name) -{ - this->name = name; - // emit nameChanged(this->name); -} - -void QGCXPlaneLink::sendDataRef(QString ref, float value) -{ - #pragma pack(push, 1) - struct payload { - char b[5]; - float value; - char name[500]; - } dref; - #pragma pack(pop) - - dref.b[0] = 'D'; - dref.b[1] = 'R'; - dref.b[2] = 'E'; - dref.b[3] = 'F'; - dref.b[4] = '0'; - - /* Set value */ - dref.value = value; - - /* Fill name with zeroes */ - memset(dref.name, 0, sizeof(dref.name)); - - /* Set dref name */ - - /* Send command */ - QByteArray ba = ref.toUtf8(); - if (ba.length() > 500) { - return; - } - - for (int i = 0; i < ba.length(); i++) { - dref.name[i] = ba.at(i); - } - writeBytesSafe((const char*)&dref, sizeof(dref)); -} diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h deleted file mode 100644 index d83f65553e8d0d5971086d220859bb026ceeae8f..0000000000000000000000000000000000000000 --- a/src/comm/QGCXPlaneLink.h +++ /dev/null @@ -1,237 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file QGCXPlaneLink.h - * @brief X-Plane simulation link - * @author Lorenz Meier - * - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include "QGCConfig.h" -#include "QGCHilLink.h" -#include "Vehicle.h" - -class QGCXPlaneLink : public QGCHilLink -{ - Q_OBJECT - //Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface) - -public: - QGCXPlaneLink(Vehicle* vehicle, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); - ~QGCXPlaneLink(); - - /** - * @brief Load X-Plane HIL settings - */ - void loadSettings(); - - /** - * @brief Store X-Plane HIL settings - */ - void storeSettings(); - - bool isConnected(); - qint64 bytesAvailable(); - int getPort() const { - return localPort; - } - - /** - * @brief The human readable port name - */ - QString getName(); - - void run(); - - /** - * @brief Get remote host and port - * @return string in format : - */ - QString getRemoteHost(); - - enum AIRFRAME - { - AIRFRAME_UNKNOWN = 0, - AIRFRAME_QUAD_DJI_F450_PWM, - AIRFRAME_QUAD_X_MK_10INCH_I2C, - AIRFRAME_QUAD_X_ARDRONE, - AIRFRAME_FIXED_WING_BIXLER_II, - AIRFRAME_FIXED_WING_BIXLER_II_AILERONS - }; - - QString getVersion() - { - return QString("X-Plane %1").arg(xPlaneVersion); - } - - int getAirFrameIndex() - { - return (int)airframeID; - } - - bool sensorHilEnabled() { - return _sensorHilEnabled; - } - - bool useHilActuatorControls() { - return _useHilActuatorControls; - } - -signals: - /** @brief Sensor leve HIL state changed */ - void useHilActuatorControlsChanged(bool enabled); - -public slots: -// void setAddress(QString address); - void setPort(int port); - /** @brief Add a new host to broadcast messages to */ - void setRemoteHost(const QString& host); - /** @brief Send new control states to the simulation */ - void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); - /** @brief Send new control commands to the simulation */ - void updateActuatorControls(quint64 time, quint64 flags, - float ctl_0, - float ctl_1, - float ctl_2, - float ctl_3, - float ctl_4, - float ctl_5, - float ctl_6, - float ctl_7, - float ctl_8, - float ctl_9, - float ctl_10, - float ctl_11, - float ctl_12, - float ctl_13, - float ctl_14, - float ctl_15, - quint8 mode); - /** @brief Set the simulator version as text string */ - void setVersion(const QString& version); - /** @brief Set the simulator version as integer */ - void setVersion(unsigned int version); - - void enableSensorHIL(bool enable) { - if (enable != _sensorHilEnabled) { - _sensorHilEnabled = enable; - emit sensorHilChanged(enable); - } - } - - void enableHilActuatorControls(bool enable); - - void processError(QProcess::ProcessError err); - - void readBytes(); - -private slots: - /** - * @brief Write a number of bytes to the interface. - * - * @param data Pointer to the data byte array - * @param size The size of the bytes array - **/ - void _writeBytes(const QByteArray data); - -public slots: - bool connectSimulation(); - bool disconnectSimulation(); - /** - * @brief Select airplane model - * @param plane the name of the airplane - */ - void selectAirframe(const QString& airframe); - /** - * @brief Set the airplane position and attitude - * @param lat - * @param lon - * @param alt - * @param roll - * @param pitch - * @param yaw - */ - void setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw); - - /** - * @brief Set a random position - */ - void setRandomPosition(); - - /** - * @brief Set a random attitude - */ - void setRandomAttitude(); - -protected: - Vehicle* _vehicle; - QString name; - QHostAddress localHost; - quint16 localPort; - QHostAddress remoteHost; - quint16 remotePort; - int id; - QUdpSocket* socket; - bool connectState; - - quint64 bitsSentTotal; - quint64 bitsSentCurrent; - quint64 bitsSentMax; - quint64 bitsReceivedTotal; - quint64 bitsReceivedCurrent; - quint64 bitsReceivedMax; - quint64 connectionStartTime; - QMutex statisticsMutex; - QMutex dataMutex; - QTimer refreshTimer; - QProcess* process; - QProcess* terraSync; - - bool gpsReceived; - bool attitudeReceived; - - float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; - double lat, lon, alt, alt_agl; - float vx, vy, vz, xacc, yacc, zacc; - float ind_airspeed; - float true_airspeed; - float groundspeed; - float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature; - float barometerOffsetkPa; - - float man_roll, man_pitch, man_yaw; - QString airframeName; - enum AIRFRAME airframeID; - bool xPlaneConnected; - unsigned int xPlaneVersion; - quint64 simUpdateLast; - quint64 simUpdateFirst; - quint64 simUpdateLastText; - quint64 simUpdateLastGroundTruth; - float simUpdateHz; - bool _sensorHilEnabled; - bool _useHilActuatorControls; - bool _should_exit; - - void setName(QString name); - void sendDataRef(QString ref, float value); -}; - diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b230d9753a45b3ceb61e6ca30e7e2dbdf32ea7d7..4a263d6d4d3cf7f345665142e28c437bbe018889 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -104,20 +104,11 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi temperature_var(0.0f), */ -#ifndef __mobile__ - simulation(0), -#endif - // The protected members. connectionLost(false), lastVoltageWarning(0), lastNonNullTime(0), onboardTimeOffsetInvalidCount(0), - hilEnabled(false), - sensorHil(false), - lastSendTimeGPS(0), - lastSendTimeSensors(0), - lastSendTimeOpticalFlow(0), _vehicle(vehicle), _firmwarePluginManager(firmwarePluginManager) { @@ -229,13 +220,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); } break; - case MAVLINK_MSG_ID_HIL_CONTROLS: - { - mavlink_hil_controls_t hil; - mavlink_msg_hil_controls_decode(&message, &hil); - emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); - } - break; case MAVLINK_MSG_ID_PARAM_VALUE: { @@ -1002,384 +986,6 @@ void UAS::pairRX(int rxType, int rxSubType) } } -/** -* If enabled, connect the JSBSim link. -*/ -#ifndef __mobile__ -void UAS::enableHilJSBSim(bool enable, QString options) -{ - auto* link = qobject_cast(simulation); - if (!link) { - // Delete wrong sim - if (simulation) { - stopHil(); - delete simulation; - } - simulation = new QGCJSBSimLink(_vehicle, options); - } - // Connect Flight Gear Link - link = qobject_cast(simulation); - link->setStartupArguments(options); - if (enable) - { - startHil(); - } - else - { - stopHil(); - } -} -#endif - -/** -* If enabled, connect the X-plane gear link. -*/ -#ifndef __mobile__ -void UAS::enableHilXPlane(bool enable) -{ - auto* link = qobject_cast(simulation); - if (!link) { - if (simulation) { - stopHil(); - delete simulation; - } - simulation = new QGCXPlaneLink(_vehicle); - - float noise_scaler = 0.0001f; - xacc_var = noise_scaler * 0.2914f; - yacc_var = noise_scaler * 0.2914f; - zacc_var = noise_scaler * 0.9577f; - rollspeed_var = noise_scaler * 0.8126f; - pitchspeed_var = noise_scaler * 0.6145f; - yawspeed_var = noise_scaler * 0.5852f; - xmag_var = noise_scaler * 0.0786f; - ymag_var = noise_scaler * 0.0566f; - zmag_var = noise_scaler * 0.0333f; - abs_pressure_var = noise_scaler * 0.5604f; - diff_pressure_var = noise_scaler * 0.2604f; - pressure_alt_var = noise_scaler * 0.5604f; - temperature_var = noise_scaler * 0.7290f; - } - // Connect X-Plane Link - if (enable) - { - startHil(); - } - else - { - stopHil(); - } -} -#endif - -/** -* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) -* @param roll Roll angle (rad) -* @param pitch Pitch angle (rad) -* @param yaw Yaw angle (rad) -* @param rollspeed Roll angular speed (rad/s) -* @param pitchspeed Pitch angular speed (rad/s) -* @param yawspeed Yaw angular speed (rad/s) -* @param lat Latitude, expressed as * 1E7 -* @param lon Longitude, expressed as * 1E7 -* @param alt Altitude in meters, expressed as * 1000 (millimeters) -* @param vx Ground X Speed (Latitude), expressed as m/s * 100 -* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 -* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 -* @param xacc X acceleration (mg) -* @param yacc Y acceleration (mg) -* @param zacc Z acceleration (mg) -*/ -#ifndef __mobile__ -void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) -{ - Q_UNUSED(time_us); - Q_UNUSED(xacc); - Q_UNUSED(yacc); - Q_UNUSED(zacc); - - // Emit attitude for cross-check - emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); - emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); - emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); - - emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); - emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); - emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); - - emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime()); - emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime()); - emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime()); - - emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime()); - emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime()); - emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime()); - - emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime()); - emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); -} -#endif - -/** -* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) -* @param roll Roll angle (rad) -* @param pitch Pitch angle (rad) -* @param yaw Yaw angle (rad) -* @param rollspeed Roll angular speed (rad/s) -* @param pitchspeed Pitch angular speed (rad/s) -* @param yawspeed Yaw angular speed (rad/s) -* @param lat Latitude, expressed as * 1E7 -* @param lon Longitude, expressed as * 1E7 -* @param alt Altitude in meters, expressed as * 1000 (millimeters) -* @param vx Ground X Speed (Latitude), expressed as m/s * 100 -* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 -* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 -* @param xacc X acceleration (mg) -* @param yacc Y acceleration (mg) -* @param zacc Z acceleration (mg) -*/ -#ifndef __mobile__ -void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) -{ - if (!_vehicle) { - return; - } - - if (_vehicle->hilMode()) - { - float q[4]; - - double cosPhi_2 = cos(double(roll) / 2.0); - double sinPhi_2 = sin(double(roll) / 2.0); - double cosTheta_2 = cos(double(pitch) / 2.0); - double sinTheta_2 = sin(double(pitch) / 2.0); - double cosPsi_2 = cos(double(yaw) / 2.0); - double sinPsi_2 = sin(double(yaw) / 2.0); - q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); - - mavlink_message_t msg; - mavlink_msg_hil_state_quaternion_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - time_us, q, rollspeed, pitchspeed, yawspeed, - lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); - } - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -#ifndef __mobile__ -float UAS::addZeroMeanNoise(float truth_meas, float noise_var) -{ - /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var. Calculated according to - Box-Muller transform */ - static const float epsilon = std::numeric_limits::min(); //used to ensure non-zero uniform numbers - static float z0; //calculated normal distribution random variables with mu = 0, var = 1; - float u1, u2; //random variables generated from c++ rand(); - - /*Generate random variables in range (0 1] */ - do - { - //TODO seed rand() with srand(time) but srand(time should be called once on startup) - //currently this will generate repeatable random noise - u1 = rand() * (1.0 / RAND_MAX); - u2 = rand() * (1.0 / RAND_MAX); - } - while ( u1 <= epsilon ); //Have a catch to ensure non-zero for log() - - z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1 - - //TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these - //as well - float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2 - - //Finally guard against any case where the noise is not real - if(std::isfinite(noise)) { - return truth_meas + noise; - } else { - return truth_meas; - } -} -#endif - -/* -* @param abs_pressure Absolute Pressure (hPa) -* @param diff_pressure Differential Pressure (hPa) -*/ -#ifndef __mobile__ -void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) -{ - if (!_vehicle) { - return; - } - - if (_vehicle->hilMode()) - { - float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var); - float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var); - float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var); - float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var); - float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var); - float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var); - float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var); - float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var); - float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var); - float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var); - float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var); - float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var); - float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var); - - mavlink_message_t msg; - mavlink_msg_hil_sensor_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt, - yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, - diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); - lastSendTimeSensors = QGC::groundTimeMilliseconds(); - } - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -#ifndef __mobile__ -void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance) -{ - if (!_vehicle) { - return; - } - - // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api - - Q_UNUSED(time_us); - Q_UNUSED(flow_x); - Q_UNUSED(flow_y); - Q_UNUSED(flow_comp_m_x); - Q_UNUSED(flow_comp_m_y); - Q_UNUSED(quality); - Q_UNUSED(ground_distance); - - if (_vehicle->hilMode()) - { -#if 0 - mavlink_message_t msg; - mavlink_msg_hil_optical_flow_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance); - - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); - lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); -#endif - } - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } - -} -#endif - -#ifndef __mobile__ -void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) -{ - if (!_vehicle) { - return; - } - - // Only send at 10 Hz max rate - if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) - return; - - if (_vehicle->hilMode()) - { - float course = cog; - // map to 0..2pi - if (course < 0) - course += 2.0f * static_cast(M_PI); - // scale from radians to degrees - course = (course / M_PI) * 180.0f; - - mavlink_message_t msg; - mavlink_msg_hil_gps_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); - lastSendTimeGPS = QGC::groundTimeMilliseconds(); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); - } - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -/** -* Connect flight gear link. -**/ -#ifndef __mobile__ -void UAS::startHil() -{ - if (hilEnabled) return; - hilEnabled = true; - sensorHil = false; - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - // Connect HIL simulation link - simulation->connectSimulation(); -} -#endif - -/** -* disable flight gear link. -*/ -#ifndef __mobile__ -void UAS::stopHil() -{ - if (simulation && simulation->isConnected()) { - simulation->disconnectSimulation(); - _vehicle->setHilMode(false); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; - } - hilEnabled = false; - sensorHil = false; -} -#endif - void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) { if (!_vehicle) { @@ -1444,14 +1050,5 @@ void UAS::unsetRCToParameterMap() void UAS::shutdownVehicle(void) { -#ifndef __mobile__ - stopHil(); - if (simulation) { - // wait for the simulator to exit - simulation->wait(); - simulation->disconnectSimulation(); - simulation->deleteLater(); - } -#endif _vehicle = nullptr; } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d8b676b29c581ee2e6d138987ab7f6d303ca746c..e9faa24adfa2701bc89460cbe52fd65f103aaf8e 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -22,9 +22,6 @@ #ifndef __mobile__ #include "FileManager.h" -#include "QGCHilLink.h" -#include "QGCJSBSimLink.h" -#include "QGCXPlaneLink.h" #endif Q_DECLARE_LOGGING_CATEGORY(UASLog) @@ -181,11 +178,6 @@ protected: float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa) float temperature_var; ///< variance of temperature noise for HIL sim (C) - /// SIMULATION -#ifndef __mobile__ - QGCHilLink* simulation; ///< Hardware in the loop simulation link -#endif - public: /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); @@ -194,13 +186,6 @@ public: virtual FileManager* getFileManager() { return &fileManager; } #endif - /** @brief Get the HIL simulation */ -#ifndef __mobile__ - QGCHilLink* getHILSimulation() const { - return simulation; - } -#endif - QImage getImage(); void requestImage(); @@ -208,52 +193,6 @@ public slots: /** @brief Order the robot to pair its receiver **/ void pairRX(int rxType, int rxSubType); - /** @brief Enable / disable HIL */ -#ifndef __mobile__ - void enableHilJSBSim(bool enable, QString options); - void enableHilXPlane(bool enable); - - /** @brief Send the full HIL state to the MAV */ - void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, - float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); - - void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, - float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); - - /** @brief RAW sensors for sensor HIL */ - void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed); - - /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ - void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance); - - float addZeroMeanNoise(float truth_meas, float noise_var); - - /** - * @param time_us - * @param lat - * @param lon - * @param alt - * @param fix_type - * @param eph - * @param epv - * @param vel - * @param cog course over ground, in radians, -pi..pi - * @param satellites - */ - void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites); - - - /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ - void startHil(); - - /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/ - void stopHil(); -#endif - /** @brief Set the values for the manual control of the vehicle */ void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); @@ -280,8 +219,6 @@ signals: void imageStarted(quint64 timestamp); /** @brief A new camera image has arrived */ void imageReady(UASInterface* uas); - /** @brief HIL controls have changed */ - void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); void rollChanged(double val,QString name); void pitchChanged(double val,QString name); @@ -305,11 +242,6 @@ protected: quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong - bool hilEnabled; - bool sensorHil; ///< True if sensor HIL is enabled - quint64 lastSendTimeGPS; ///< Last HIL GPS message sent - quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent - quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent private: Vehicle* _vehicle; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0c8511e11e86cf68281902366cfbbfe0910de355..c2aee79c53fec16805eb94ad5f77de597c99c822 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -82,24 +82,6 @@ public slots: /** @brief Order the robot to pair its receiver **/ virtual void pairRX(int rxType, int rxSubType) = 0; - /** @brief Send the full HIL state to the MAV */ -#ifndef __mobile__ - virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) = 0; - - /** @brief RAW sensors for sensor HIL */ - virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) = 0; - - /** @brief Send raw GPS for sensor HIL */ - virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) = 0; - - /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ - virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance) = 0; -#endif - /** @brief Send command to map a RC channel to a parameter */ virtual void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) = 0;