From e390874c552aaf7502dc63102d1b55e710c1e9e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 10:10:45 +0200 Subject: [PATCH] wip, arducopter hil model and protocol --- .../Aircraft/arducopter/arducopter.xml | 50 +- .../Protocol/qgroundcontrol-quadrotor.xml | 58 +- src/comm/QGCFlightGearLink.cc | 999 +++++++++--------- 3 files changed, 602 insertions(+), 505 deletions(-) diff --git a/files/flightgear/Aircraft/arducopter/arducopter.xml b/files/flightgear/Aircraft/arducopter/arducopter.xml index 5161e7532..52390816f 100755 --- a/files/flightgear/Aircraft/arducopter/arducopter.xml +++ b/files/flightgear/Aircraft/arducopter/arducopter.xml @@ -103,11 +103,11 @@ - + - -0.283 - 0.00 - 0.00 + 0.000 + -0.283 + 0.000 90.00 @@ -117,8 +117,8 @@ 0 - -0.283 - 0.00 + 0.000 + -0.283 0.125 @@ -130,11 +130,11 @@ 10 - + - 0.283 - 0.000 - 0.000 + 0.00 + 0.283 + 0.00 90.00 @@ -144,8 +144,8 @@ 0 - 0.283 - 0.000 + 0.00 + 0.283 0.125 @@ -157,11 +157,12 @@ 10 - + + - 0.00 - 0.283 - 0.00 + -0.283 + 0.00 + 0.00 90.00 @@ -171,8 +172,8 @@ 0 - 0.00 - 0.283 + -0.283 + 0.00 0.125 @@ -184,10 +185,10 @@ 10 - + - 0.000 - -0.283 + 0.283 + 0.000 0.000 @@ -198,8 +199,8 @@ 0 - 0.000 - -0.283 + 0.283 + 0.000 0.125 @@ -211,6 +212,8 @@ 10 + + 0.00 @@ -222,6 +225,7 @@ 0.0 + diff --git a/files/flightgear/Protocol/qgroundcontrol-quadrotor.xml b/files/flightgear/Protocol/qgroundcontrol-quadrotor.xml index 7dd55d4c8..4ef684e43 100644 --- a/files/flightgear/Protocol/qgroundcontrol-quadrotor.xml +++ b/files/flightgear/Protocol/qgroundcontrol-quadrotor.xml @@ -141,6 +141,49 @@ /velocities/airspeed-kt 0.514444444444444 + + + airspeed-indicated-mps + float + %.8f + /instrumentation/indicated-speed-kt + 0.514444444444444 + + + + + Magnetic Variation (rad) + float + %.8f + /environment/magnetic-variation-deg + 0.01745329251994329576 + + + + Magnetic Dip (rad) + float + %.8f + /environment/magnetic-dip-deg + 0.01745329251994329576 + + + + + Temperature (deg C) + float + %.8f + /environment/temperature-degc + 1 + + + + + Pressure (hPa) + float + %.8f + /environment/pressure-inhg + 33.86389 + @@ -152,22 +195,27 @@ throttle0 - double + float /controls/engines/engine[0]/throttle throttle1 - double + float /controls/engines/engine[1]/throttle throttle2 - double + float /controls/engines/engine[2]/throttle + + + running + bool + /engines/engine/running throttle3 - double + float /controls/engines/engine[3]/throttle @@ -176,4 +224,4 @@ - \ No newline at end of file + diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index d8bc240ee..af8702f6d 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -1,25 +1,25 @@ /*===================================================================== -QGroundControl Open Source Ground Control Station + QGroundControl Open Source Ground Control Station -(c) 2009 - 2011 QGROUNDCONTROL PROJECT + (c) 2009 - 2011 QGROUNDCONTROL PROJECT -This file is part of the QGROUNDCONTROL project + This file is part of the QGROUNDCONTROL project - QGROUNDCONTROL 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. + QGROUNDCONTROL 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. - QGROUNDCONTROL 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. + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . -======================================================================*/ + ======================================================================*/ /** * @file @@ -40,173 +40,176 @@ This file is part of the QGROUNDCONTROL project #include #include "MainWindow.h" -QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) : - socket(NULL), - process(NULL), - terraSync(NULL), - flightGearVersion(0), - startupArguments(startupArguments), - _sensorHilEnabled(true) -{ - this->host = host; - this->port = port+mav->getUASID(); - this->connectState = false; - this->currentPort = 49000+mav->getUASID(); - this->mav = mav; - this->name = tr("FlightGear Link (port:%1)").arg(port); - setRemoteHost(remoteHost); +QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, + QString startupArguments, QString remoteHost, QHostAddress host, + quint16 port) : + socket(NULL), process(NULL), terraSync(NULL), flightGearVersion(0), startupArguments( + startupArguments), _sensorHilEnabled(true) { + this->host = host; + this->port = port + mav->getUASID(); + this->connectState = false; + this->currentPort = 49000 + mav->getUASID(); + this->mav = mav; + this->name = tr("FlightGear Link (port:%1)").arg(port); + setRemoteHost(remoteHost); } -QGCFlightGearLink::~QGCFlightGearLink() -{ //do not disconnect unless it is connected. - //disconnectSimulation will delete the memory that was allocated for proces, terraSync and socket - if(connectState){ - disconnectSimulation(); - } +QGCFlightGearLink::~QGCFlightGearLink() { //do not disconnect unless it is connected. +//disconnectSimulation will delete the memory that was allocated for proces, terraSync and socket + if (connectState) { + disconnectSimulation(); + } } /** * @brief Runs the thread * **/ -void QGCFlightGearLink::run() -{ - exec(); +void QGCFlightGearLink::run() { + exec(); } -void QGCFlightGearLink::setPort(int port) -{ - this->port = port; - disconnectSimulation(); - connectSimulation(); +void QGCFlightGearLink::setPort(int port) { + this->port = port; + disconnectSimulation(); + connectSimulation(); } -void QGCFlightGearLink::processError(QProcess::ProcessError err) -{ - switch(err) - { - case QProcess::FailedToStart: - MainWindow::instance()->showCriticalMessage(tr("FlightGear/TerraSync Failed to Start"), tr("Please check if the path and command is correct")); - break; - case QProcess::Crashed: - MainWindow::instance()->showCriticalMessage(tr("FlightGear/TerraSync Crashed"), tr("This is a FlightGear-related problem. Please upgrade FlightGear")); - break; - case QProcess::Timedout: - MainWindow::instance()->showCriticalMessage(tr("FlightGear/TerraSync Start Timed Out"), tr("Please check if the path and command is correct")); - break; - case QProcess::WriteError: - MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with FlightGear/TerraSync"), tr("Please check if the path and command is correct")); - break; - case QProcess::ReadError: - MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with FlightGear/TerraSync"), tr("Please check if the path and command is correct")); - break; - case QProcess::UnknownError: - default: - MainWindow::instance()->showCriticalMessage(tr("FlightGear/TerraSync Error"), tr("Please check if the path and command is correct.")); - break; - } +void QGCFlightGearLink::processError(QProcess::ProcessError err) { + switch (err) { + case QProcess::FailedToStart: + MainWindow::instance()->showCriticalMessage( + tr("FlightGear/TerraSync Failed to Start"), + tr("Please check if the path and command is correct")); + break; + case QProcess::Crashed: + MainWindow::instance()->showCriticalMessage( + tr("FlightGear/TerraSync Crashed"), + tr( + "This is a FlightGear-related problem. Please upgrade FlightGear")); + break; + case QProcess::Timedout: + MainWindow::instance()->showCriticalMessage( + tr("FlightGear/TerraSync Start Timed Out"), + tr("Please check if the path and command is correct")); + break; + case QProcess::WriteError: + MainWindow::instance()->showCriticalMessage( + tr("Could not Communicate with FlightGear/TerraSync"), + tr("Please check if the path and command is correct")); + break; + case QProcess::ReadError: + MainWindow::instance()->showCriticalMessage( + tr("Could not Communicate with FlightGear/TerraSync"), + tr("Please check if the path and command is correct")); + break; + case QProcess::UnknownError: + default: + MainWindow::instance()->showCriticalMessage( + tr("FlightGear/TerraSync Error"), + tr("Please check if the path and command is correct.")); + break; + } } /** * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 */ -void QGCFlightGearLink::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 QGCFlightGearLink::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 QGCFlightGearLink::updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8) -{ - Q_UNUSED(time); - Q_UNUSED(act1); - Q_UNUSED(act2); - Q_UNUSED(act3); - Q_UNUSED(act4); - Q_UNUSED(act5); - Q_UNUSED(act6); - Q_UNUSED(act7); - Q_UNUSED(act8); +void QGCFlightGearLink::updateActuators(uint64_t time, float act1, float act2, + float act3, float act4, float act5, float act6, float act7, + float act8) { + Q_UNUSED(time); + Q_UNUSED(act1); + Q_UNUSED(act2); + Q_UNUSED(act3); + Q_UNUSED(act4); + Q_UNUSED(act5); + Q_UNUSED(act6); + Q_UNUSED(act7); + Q_UNUSED(act8); } -void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode) -{ - // magnetos,aileron,elevator,rudder,throttle\n - - //float magnetos = 3.0f; - Q_UNUSED(time); - Q_UNUSED(systemMode); - Q_UNUSED(navMode); - - if(!isnan(rollAilerons) && !isnan(pitchElevator) && !isnan(yawRudder) && !isnan(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); - writeBytes(state.toAscii().constData(), state.length()); -// qDebug() << "updated controls" << rollAilerons << pitchElevator << yawRudder << throttle; - } - else - { - qDebug() << "HIL: Got NaN values from the hardware: isnan output: roll: " << isnan(rollAilerons) << ", pitch: " << isnan(pitchElevator) << ", yaw: " << isnan(yawRudder) << ", throttle: " << isnan(throttle); - } - //qDebug() << "Updated controls" << state; +void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, + float pitchElevator, float yawRudder, float throttle, + uint8_t systemMode, uint8_t navMode) { + // magnetos,aileron,elevator,rudder,throttle\n + + //float magnetos = 3.0f; + Q_UNUSED(time); + Q_UNUSED(systemMode); + Q_UNUSED(navMode); + + if (!isnan(rollAilerons) && !isnan(pitchElevator) && !isnan(yawRudder) + && !isnan(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); + writeBytes(state.toAscii().constData(), state.length()); + qDebug() << "updated controls" << rollAilerons << pitchElevator + << yawRudder << throttle; + } else { + qDebug() + << "HIL: Got NaN values from the hardware: isnan output: roll: " + << isnan(rollAilerons) << ", pitch: " << isnan(pitchElevator) + << ", yaw: " << isnan(yawRudder) << ", throttle: " + << isnan(throttle); + } + //qDebug() << "Updated controls" << state; } -void QGCFlightGearLink::writeBytes(const char* data, qint64 size) -{ - //#define QGCFlightGearLink_DEBUG +void QGCFlightGearLink::writeBytes(const char* data, qint64 size) { + #define QGCFlightGearLink_DEBUG #ifdef QGCFlightGearLink_DEBUG - QString bytes; - QString ascii; - for (int i=0; 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; + QString bytes; + QString ascii; + for (int i=0; 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, size, currentHost, currentPort); + if (connectState && socket) + socket->writeDatagram(data, size, currentHost, currentPort); } /** @@ -215,107 +218,111 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size) * @param data Pointer to the data byte array to write the bytes to * @param maxLength The maximum number of bytes to write **/ -void QGCFlightGearLink::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); - - QByteArray b(data, s); - - // Print string - QString state(b); +void QGCFlightGearLink::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); + + QByteArray b(data, s); + + // Print string + QString state(b); // qDebug() << "FG LINK GOT:" << state; - QStringList values = state.split("\t"); - - // Check length - if (values.size() != 22) - { - qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size(); - qDebug() << state; - return; - } - - // Parse string - float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; - double lat, lon, alt; - float ind_airspeed; - float true_airspeed; - float vx, vy, vz, xacc, yacc, zacc; - float diff_pressure; - float temperature; - float abs_pressure; - float mag_variation, mag_dip, xmag_ned, ymag_ned, zmag_ned, xmag_body, ymag_body, zmag_body; - - - lat = values.at(1).toDouble(); - lon = values.at(2).toDouble(); - alt = values.at(3).toDouble(); - roll = values.at(4).toFloat(); - pitch = values.at(5).toFloat(); - yaw = values.at(6).toFloat(); - rollspeed = values.at(7).toFloat(); - pitchspeed = values.at(8).toFloat(); - yawspeed = values.at(9).toFloat(); - - xacc = values.at(10).toFloat(); - yacc = values.at(11).toFloat(); - zacc = values.at(12).toFloat(); - - vx = values.at(13).toFloat(); - vy = values.at(14).toFloat(); - vz = values.at(15).toFloat(); - - true_airspeed = values.at(16).toFloat(); - ind_airspeed = values.at(17).toFloat(); - - mag_variation = values.at(18).toFloat(); - mag_dip = values.at(19).toFloat(); - - temperature = values.at(20).toFloat(); - abs_pressure = values.at(21).toFloat(); - - // Send updated state - //qDebug() << "sensorHilEnabled: " << sensorHilEnabled; - if (_sensorHilEnabled) - { - quint16 fields_changed = 0xFFF; //set all 12 used bits - - //calculate differential pressure - const float air_gas_constant = 287.1f; // J/(kg * K) - const float absolute_null_celsius = -273.15f; // °C - float density = abs_pressure / (air_gas_constant * (temperature - absolute_null_celsius)); - diff_pressure = true_airspeed * true_airspeed * density / 2.0f; - - float pressure_alt = alt; - - xmag_ned = cosf(mag_variation); - ymag_ned = sinf(mag_variation); - zmag_ned = sinf(mag_dip); - float tempMagLength = sqrtf(xmag_ned*xmag_ned + ymag_ned*ymag_ned + zmag_ned*zmag_ned); - xmag_ned = xmag_ned / tempMagLength; - ymag_ned = ymag_ned / tempMagLength; - zmag_ned = zmag_ned / tempMagLength; - - //transform magnetic measurement to body frame coordinates - double cosPhi = cos(roll); - double sinPhi = sin(roll); - double cosThe = cos(pitch); - double sinThe = sin(pitch); - double cosPsi = cos(yaw); - double sinPsi = sin(yaw); - - float R_B_N[3][3]; - - R_B_N[0][0] = cosThe * cosPsi; - R_B_N[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - R_B_N[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + QStringList values = state.split("\t"); + + // Check length + if (values.size() != 22) { + qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" + << values.size(); + qDebug() << state; + return; + } + + // Parse string + float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; + double lat, lon, alt; + float ind_airspeed; + float true_airspeed; + float vx, vy, vz, xacc, yacc, zacc; + float diff_pressure; + float temperature; + float abs_pressure; + float mag_variation, mag_dip, xmag_ned, ymag_ned, zmag_ned, xmag_body, + ymag_body, zmag_body; + + lat = values.at(1).toDouble(); + lon = values.at(2).toDouble(); + alt = values.at(3).toDouble(); + roll = values.at(4).toFloat(); + pitch = values.at(5).toFloat(); + yaw = values.at(6).toFloat(); + rollspeed = values.at(7).toFloat(); + pitchspeed = values.at(8).toFloat(); + yawspeed = values.at(9).toFloat(); + + xacc = values.at(10).toFloat(); + yacc = values.at(11).toFloat(); + zacc = values.at(12).toFloat(); + + vx = values.at(13).toFloat(); + vy = values.at(14).toFloat(); + vz = values.at(15).toFloat(); + + true_airspeed = values.at(16).toFloat(); + ind_airspeed = values.at(17).toFloat(); + + mag_variation = values.at(18).toFloat(); + mag_dip = values.at(19).toFloat(); + + temperature = values.at(20).toFloat(); + abs_pressure = values.at(21).toFloat(); + + // Send updated state + //qDebug() << "sensorHilEnabled: " << sensorHilEnabled; + if (_sensorHilEnabled) { + quint16 fields_changed = 0xFFF; //set all 12 used bits + + //calculate differential pressure + const float air_gas_constant = 287.1f; // J/(kg * K) + const float absolute_null_celsius = -273.15f; // °C + float density = abs_pressure + / (air_gas_constant * (temperature - absolute_null_celsius)); + diff_pressure = true_airspeed * true_airspeed * density / 2.0f; + + float pressure_alt = alt; + + xmag_ned = cosf(mag_variation); + ymag_ned = sinf(mag_variation); + zmag_ned = sinf(mag_dip); + float tempMagLength = sqrtf( + xmag_ned * xmag_ned + ymag_ned * ymag_ned + + zmag_ned * zmag_ned); + xmag_ned = xmag_ned / tempMagLength; + ymag_ned = ymag_ned / tempMagLength; + zmag_ned = zmag_ned / tempMagLength; + + //transform magnetic measurement to body frame coordinates + double cosPhi = cos(roll); + double sinPhi = sin(roll); + double cosThe = cos(pitch); + double sinThe = sin(pitch); + double cosPsi = cos(yaw); + double sinPsi = sin(yaw); + + float R_B_N[3][3]; + + R_B_N[0][0] = cosThe * cosPsi; + R_B_N[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + R_B_N[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; R_B_N[1][0] = cosThe * sinPsi; R_B_N[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; @@ -325,59 +332,59 @@ void QGCFlightGearLink::readBytes() R_B_N[2][1] = sinPhi * cosThe; R_B_N[2][2] = cosPhi * cosThe; - Eigen::Matrix3f R_B_N_M = Eigen::Map((float*)R_B_N).eval(); + Eigen::Matrix3f R_B_N_M = + Eigen::Map((float*) R_B_N).eval(); - Eigen::Vector3f mag_ned(xmag_ned, ymag_ned, zmag_ned); + Eigen::Vector3f mag_ned(xmag_ned, ymag_ned, zmag_ned); - Eigen::Vector3f mag_body = R_B_N_M * mag_ned; + Eigen::Vector3f mag_body = R_B_N_M * mag_ned; - xmag_body = mag_body(0); - ymag_body = mag_body(1); - zmag_body = mag_body(2); + xmag_body = mag_body(0); + ymag_body = mag_body(1); + zmag_body = mag_body(2); - emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, - xmag_body, ymag_body, zmag_body, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); + emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, + rollspeed, pitchspeed, yawspeed, xmag_body, ymag_body, + zmag_body, abs_pressure, diff_pressure, pressure_alt, + temperature, fields_changed); // qDebug() << "sensorHilRawImuChanged " << xacc << yacc << zacc << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature; - int gps_fix_type = 3; - float eph = 0.3; - float epv = 0.6; - float vel = sqrt(vx*vx + vy*vy + vz*vz); - float cog = yaw; - int satellites = 8; + int gps_fix_type = 3; + float eph = 0.3; + float epv = 0.6; + float vel = sqrt(vx * vx + vy * vy + vz * vz); + float cog = yaw; + int satellites = 8; - emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); + emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, + gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); // qDebug() << "sensorHilGpsChanged " << lat << lon << alt << vel; - } else { - emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, - pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, - ind_airspeed, true_airspeed, - xacc, yacc, zacc); - //qDebug() << "hilStateChanged " << (int32_t)lat << (int32_t)lon << (int32_t)alt; - } - - // // Echo data for debugging purposes - // std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; - // int i; - // for (i=0; ipendingDatagramSize(); +qint64 QGCFlightGearLink::bytesAvailable() { + return socket->pendingDatagramSize(); } /** @@ -385,39 +392,55 @@ qint64 QGCFlightGearLink::bytesAvailable() * * @return True if connection has been disconnected, false if connection couldn't be disconnected. **/ -bool QGCFlightGearLink::disconnectSimulation() -{ - disconnect(process, SIGNAL(error(QProcess::ProcessError)), - this, SLOT(processError(QProcess::ProcessError))); - disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - disconnect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); - disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); - - if (process) - { - process->close(); - delete process; - process = NULL; - } - if (terraSync) - { - terraSync->close(); - delete terraSync; - terraSync = NULL; - } - if (socket) - { - socket->close(); - delete socket; - socket = NULL; - } - - connectState = false; - - emit simulationDisconnected(); - emit simulationConnected(false); - return !connectState; +bool QGCFlightGearLink::disconnectSimulation() { + disconnect(process, SIGNAL(error(QProcess::ProcessError)), this, + SLOT(processError(QProcess::ProcessError))); + disconnect(mav, + SIGNAL( + hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), + this, + SLOT( + updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); + disconnect(this, + SIGNAL( + hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), + mav, + SLOT( + sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); + disconnect(this, + SIGNAL( + sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), + mav, + SLOT( + sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); + disconnect(this, + SIGNAL( + sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), + mav, + SLOT( + sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); + + if (process) { + process->close(); + delete process; + process = NULL; + } + if (terraSync) { + terraSync->close(); + delete terraSync; + terraSync = NULL; + } + if (socket) { + socket->close(); + delete socket; + socket = NULL; + } + + connectState = false; + + emit simulationDisconnected(); + emit simulationConnected(false); + return !connectState; } /** @@ -425,72 +448,92 @@ bool QGCFlightGearLink::disconnectSimulation() * * @return True if connection has been established, false if connection couldn't be established. **/ -bool QGCFlightGearLink::connectSimulation() -{ - qDebug() << "STARTING FLIGHTGEAR LINK"; - - if (!mav) return false; - socket = new QUdpSocket(this); - connectState = socket->bind(host, port); - - QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); - - process = new QProcess(this); - terraSync = new QProcess(this); - - connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - connect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); - connect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); - connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); - - UAS* uas = dynamic_cast(mav); - if (uas) - { - uas->startHil(); - } - - //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); - // Catch process error - QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), - this, SLOT(processError(QProcess::ProcessError))); - QObject::connect( terraSync, SIGNAL(error(QProcess::ProcessError)), - this, SLOT(processError(QProcess::ProcessError))); - // Start Flightgear - QStringList flightGearArguments; - QString processFgfs; - QString processTerraSync; - QString fgRoot; - QString fgScenery; - QString terraSyncScenery; - QString fgAircraft; +bool QGCFlightGearLink::connectSimulation() { + qDebug() << "STARTING FLIGHTGEAR LINK"; + + if (!mav) + return false; + socket = new QUdpSocket(this); + connectState = socket->bind(host, port); + + QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); + + process = new QProcess(this); + terraSync = new QProcess(this); + + connect(mav, + SIGNAL( + hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), + this, + SLOT( + updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); + connect(this, + SIGNAL( + hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), + mav, + SLOT( + sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); + connect(this, + SIGNAL( + sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), + mav, + SLOT( + sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); + connect(this, + SIGNAL( + sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), + mav, + SLOT( + sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); + + UAS* uas = dynamic_cast(mav); + if (uas) { + uas->startHil(); + } + + //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); + // Catch process error + QObject::connect(process, SIGNAL(error(QProcess::ProcessError)), this, + SLOT(processError(QProcess::ProcessError))); + QObject::connect(terraSync, SIGNAL(error(QProcess::ProcessError)), this, + SLOT(processError(QProcess::ProcessError))); + // Start Flightgear + QStringList flightGearArguments; + QString processFgfs; + QString processTerraSync; + QString fgRoot; + QString fgScenery; + QString terraSyncScenery; + QString fgAircraft; #ifdef Q_OS_MACX - processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs"; - processTerraSync = "/Applications/FlightGear.app/Contents/Resources/terrasync"; - //fgRoot = "/Applications/FlightGear.app/Contents/Resources/data"; - //fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery"; - terraSyncScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync"; - // /Applications/FlightGear.app/Contents/Resources/data/Scenery: + processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs"; + processTerraSync = "/Applications/FlightGear.app/Contents/Resources/terrasync"; + //fgRoot = "/Applications/FlightGear.app/Contents/Resources/data"; + //fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery"; + terraSyncScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync"; + // /Applications/FlightGear.app/Contents/Resources/data/Scenery: #endif #ifdef Q_OS_WIN32 - processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; - //fgRoot = "C:\\Program Files (x86)\\FlightGear\\data"; - terraSyncScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync"; + processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; + //fgRoot = "C:\\Program Files (x86)\\FlightGear\\data"; + terraSyncScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync"; #endif #ifdef Q_OS_LINUX - processFgfs = "fgfs"; - //fgRoot = "/usr/share/games/flightgear"; - //fgScenery = "/usr/share/games/flightgear/Scenery/"; - processTerraSync = "nice"; //according to http://wiki.flightgear.org/TerraSync, run with lower priority - terraSyncScenery = QDir::homePath() + "/.terrasync/Scenery"; //according to http://wiki.flightgear.org/TerraSync a separate directory is used + processFgfs = "fgfs"; + //fgRoot = "/usr/share/games/flightgear"; + //fgScenery = "/usr/share/games/flightgear/Scenery/"; + processTerraSync = "nice"; //according to http://wiki.flightgear.org/TerraSync, run with lower priority + terraSyncScenery = QDir::homePath() + "/.terrasync/Scenery"; //according to http://wiki.flightgear.org/TerraSync a separate directory is used #endif - fgAircraft = QApplication::applicationDirPath() + "/files/flightgear/Aircraft"; + fgAircraft = QApplication::applicationDirPath() + + "/files/flightgear/Aircraft"; - // Sanity checks - bool sane = true; + // Sanity checks + bool sane = true; // QFileInfo executable(processFgfs); // if (!executable.isExecutable()) // { @@ -519,27 +562,36 @@ bool QGCFlightGearLink::connectSimulation() // sane = false; // } - - if (!sane) return false; - - // --atlas=socket,out,1,localhost,5505,udp - // terrasync -p 5505 -S -d /usr/local/share/TerraSync - - /*Prepare FlightGear Arguments */ - //flightGearArguments << QString("--fg-root=%1").arg(fgRoot); - flightGearArguments << QString("--fg-scenery=%1:%2").arg(terraSyncScenery); //according to http://wiki.flightgear.org/TerraSync a separate directory is used - flightGearArguments << QString("--fg-aircraft=%1").arg(fgAircraft); - if (mav->getSystemType() == MAV_TYPE_QUADROTOR) - { - flightGearArguments << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol-quadrotor").arg(port); - flightGearArguments << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol-quadrotor").arg(currentPort); - } - else - { - flightGearArguments << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol-fixed-wing").arg(port); - flightGearArguments << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol-fixed-wing").arg(currentPort); - } - flightGearArguments << "--atlas=socket,out,1,localhost,5505,udp"; + if (!sane) + return false; + + // --atlas=socket,out,1,localhost,5505,udp + // terrasync -p 5505 -S -d /usr/local/share/TerraSync + + /*Prepare FlightGear Arguments */ + //flightGearArguments << QString("--fg-root=%1").arg(fgRoot); + flightGearArguments << QString("--fg-scenery=%1:%2").arg(terraSyncScenery); //according to http://wiki.flightgear.org/TerraSync a separate directory is used + flightGearArguments << QString("--fg-aircraft=%1").arg(fgAircraft); + if (mav->getSystemType() == MAV_TYPE_QUADROTOR) { + flightGearArguments + << QString( + "--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol-quadrotor").arg( + port); + flightGearArguments + << QString( + "--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol-quadrotor").arg( + currentPort); + } else { + flightGearArguments + << QString( + "--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol-fixed-wing").arg( + port); + flightGearArguments + << QString( + "--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol-fixed-wing").arg( + currentPort); + } + flightGearArguments << "--atlas=socket,out,1,localhost,5505,udp"; // flightGearArguments << "--in-air"; // flightGearArguments << "--roll=0"; // flightGearArguments << "--pitch=0"; @@ -567,98 +619,95 @@ bool QGCFlightGearLink::connectSimulation() // //flightGearArguments << "--disable-horizon-effect"; // flightGearArguments << "--disable-clouds"; // flightGearArguments << "--fdm=jsb"; -// flightGearArguments << "--units-meters"; //XXX: check: the protocol xml has already a conversion from feet to m? +// flightGearArguments << "--units-meters"; // flightGearArguments << "--notrim"; - flightGearArguments += startupArguments.split(" "); - if (mav->getSystemType() == MAV_TYPE_QUADROTOR) - { - // Start all engines of the quad - flightGearArguments << "--prop:/engines/engine[0]/running=true"; - flightGearArguments << "--prop:/engines/engine[1]/running=true"; - flightGearArguments << "--prop:/engines/engine[2]/running=true"; - flightGearArguments << "--prop:/engines/engine[3]/running=true"; - } - else - { - flightGearArguments << "--prop:/engines/engine/running=true"; - } - flightGearArguments << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude()); - flightGearArguments << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude()); - flightGearArguments << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude()); - // Add new argument with this: flightGearArguments << ""; - //flightGearArguments << QString("--aircraft=%2").arg(aircraft); - - /*Prepare TerraSync Arguments */ - QStringList terraSyncArguments; + flightGearArguments += startupArguments.split(" "); + if (mav->getSystemType() == MAV_TYPE_QUADROTOR) { + // Start all engines of the quad + flightGearArguments << "--prop:/engines/engine[0]/running=true"; + flightGearArguments << "--prop:/engines/engine[1]/running=true"; + flightGearArguments << "--prop:/engines/engine[2]/running=true"; + flightGearArguments << "--prop:/engines/engine[3]/running=true"; + } else { + flightGearArguments << "--prop:/engines/engine/running=true"; + } + flightGearArguments + << QString("--lat=%1").arg( + UASManager::instance()->getHomeLatitude()); + flightGearArguments + << QString("--lon=%1").arg( + UASManager::instance()->getHomeLongitude()); + flightGearArguments + << QString("--altitude=%1").arg( + UASManager::instance()->getHomeAltitude() + 20); + // Add new argument with this: flightGearArguments << ""; + //flightGearArguments << QString("--aircraft=%2").arg(aircraft); + + /*Prepare TerraSync Arguments */ + QStringList terraSyncArguments; #ifdef Q_OS_LINUX - terraSyncArguments << "terrasync"; + terraSyncArguments << "terrasync"; #endif - terraSyncArguments << "-p"; - terraSyncArguments << "5505"; - terraSyncArguments << "-S"; - terraSyncArguments << "-d"; - terraSyncArguments << terraSyncScenery; //according to http://wiki.flightgear.org/TerraSync a separate directory is used + terraSyncArguments << "-p"; + terraSyncArguments << "5505"; + terraSyncArguments << "-S"; + terraSyncArguments << "-d"; + terraSyncArguments << terraSyncScenery; //according to http://wiki.flightgear.org/TerraSync a separate directory is used #ifdef Q_OS_LINUX - /* Setting environment */ - QProcessEnvironment env = QProcessEnvironment::systemEnvironment(); - process->setProcessEnvironment(env); - terraSync->setProcessEnvironment(env); + /* Setting environment */ + QProcessEnvironment env = QProcessEnvironment::systemEnvironment(); + process->setProcessEnvironment(env); + terraSync->setProcessEnvironment(env); #endif // connect (terraSync, SIGNAL(readyReadStandardOutput()), this, SLOT(printTerraSyncOutput())); // connect (terraSync, SIGNAL(readyReadStandardError()), this, SLOT(printTerraSyncError())); - terraSync->start(processTerraSync, terraSyncArguments); + terraSync->start(processTerraSync, terraSyncArguments); // qDebug() << "STARTING: " << processTerraSync << terraSyncArguments; - process->start(processFgfs, flightGearArguments); + process->start(processFgfs, flightGearArguments); - - - emit simulationConnected(connectState); - if (connectState) { - emit simulationConnected(); - connectionStartTime = QGC::groundTimeUsecs()/1000; - } - qDebug() << "STARTING SIM"; + emit simulationConnected(connectState); + if (connectState) { + emit simulationConnected(); + connectionStartTime = QGC::groundTimeUsecs() / 1000; + } + qDebug() << "STARTING SIM"; // qDebug() << "STARTING: " << processFgfs << flightGearArguments; - - start(LowPriority); - return connectState; + start(LowPriority); + return connectState; } -void QGCFlightGearLink::printTerraSyncOutput() -{ - qDebug() << "TerraSync stdout:"; - QByteArray byteArray = terraSync->readAllStandardOutput(); - QStringList strLines = QString(byteArray).split("\n"); +void QGCFlightGearLink::printTerraSyncOutput() { + qDebug() << "TerraSync stdout:"; + QByteArray byteArray = terraSync->readAllStandardOutput(); + QStringList strLines = QString(byteArray).split("\n"); - foreach (QString line, strLines){ - qDebug() << line; - } + foreach (QString line, strLines){ + qDebug() << line; +} } -void QGCFlightGearLink::printTerraSyncError() -{ - qDebug() << "TerraSync stderr:"; +void QGCFlightGearLink::printTerraSyncError() { + qDebug() << "TerraSync stderr:"; - QByteArray byteArray = terraSync->readAllStandardError(); - QStringList strLines = QString(byteArray).split("\n"); + QByteArray byteArray = terraSync->readAllStandardError(); + QStringList strLines = QString(byteArray).split("\n"); - foreach (QString line, strLines){ - qDebug() << line; - } + foreach (QString line, strLines){ + qDebug() << line; +} } /** * @brief Set the startup arguments used to start flightgear * **/ -void QGCFlightGearLink::setStartupArguments(QString startupArguments) -{ - this->startupArguments = startupArguments; +void QGCFlightGearLink::setStartupArguments(QString startupArguments) { + this->startupArguments = startupArguments; } /** @@ -666,23 +715,19 @@ void QGCFlightGearLink::setStartupArguments(QString startupArguments) * * @return True if link is connected, false otherwise. **/ -bool QGCFlightGearLink::isConnected() -{ - return connectState; +bool QGCFlightGearLink::isConnected() { + return connectState; } -QString QGCFlightGearLink::getName() -{ - return name; +QString QGCFlightGearLink::getName() { + return name; } -QString QGCFlightGearLink::getRemoteHost() -{ - return QString("%1:%2").arg(currentHost.toString(), currentPort); +QString QGCFlightGearLink::getRemoteHost() { + return QString("%1:%2").arg(currentHost.toString(), currentPort); } -void QGCFlightGearLink::setName(QString name) -{ - this->name = name; - // emit nameChanged(this->name); +void QGCFlightGearLink::setName(QString name) { + this->name = name; + // emit nameChanged(this->name); } -- 2.22.0