zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

wip, arducopter hil model and protocol

QGC4.4
Thomas Gubler 12 years ago
parent
5f37d6c999
commit
e390874c55
3 changed files with 602 additions and 505 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 48
      files/flightgear/Aircraft/arducopter/arducopter.xml
  2. 56
      files/flightgear/Protocol/qgroundcontrol-quadrotor.xml
  3. 335
      src/comm/QGCFlightGearLink.cc

48
files/flightgear/Aircraft/arducopter/arducopter.xml
Unescape View File

@ -103,11 +103,11 @@ @@ -103,11 +103,11 @@
</ground_reactions>
<!-- the front and rear motors spin clockwise, and the left and right motors spin counter-clockwise. -->
<propulsion>
<engine file="a2830-12" name="front">
<engine file="a2830-12" name="right">
<location unit="M">
<x> -0.283 </x>
<y> 0.00 </y>
<z> 0.00 </z>
<x> 0.000 </x>
<y> -0.283 </y>
<z> 0.000 </z>
</location>
<orient unit="DEG">
<pitch> 90.00 </pitch>
@ -117,8 +117,8 @@ @@ -117,8 +117,8 @@
<feed>0</feed>
<thruster file="prop10x4.5">
<location unit="M">
<x> -0.283 </x>
<y> 0.00 </y>
<x> 0.000 </x>
<y> -0.283 </y>
<z> 0.125 </z>
</location>
<orient unit="DEG">
@ -130,11 +130,11 @@ @@ -130,11 +130,11 @@
<p_factor> 10 </p_factor>
</thruster>
</engine>
<engine file="a2830-12" name="rear">
<engine file="a2830-12" name="left">
<location unit="M">
<x> 0.283 </x>
<y> 0.000 </y>
<z> 0.000 </z>
<x> 0.00 </x>
<y> 0.283 </y>
<z> 0.00 </z>
</location>
<orient unit="DEG">
<pitch> 90.00 </pitch>
@ -144,8 +144,8 @@ @@ -144,8 +144,8 @@
<feed>0</feed>
<thruster file="prop10x4.5">
<location unit="M">
<x> 0.283 </x>
<y> 0.000 </y>
<x> 0.00 </x>
<y> 0.283 </y>
<z> 0.125 </z>
</location>
<orient unit="DEG">
@ -157,10 +157,11 @@ @@ -157,10 +157,11 @@
<p_factor> 10 </p_factor>
</thruster>
</engine>
<engine file="a2830-12" name="left">
<engine file="a2830-12" name="front">
<location unit="M">
<x> 0.00 </x>
<y> 0.283 </y>
<x> -0.283 </x>
<y> 0.00 </y>
<z> 0.00 </z>
</location>
<orient unit="DEG">
@ -171,8 +172,8 @@ @@ -171,8 +172,8 @@
<feed>0</feed>
<thruster file="prop10x4.5">
<location unit="M">
<x> 0.00 </x>
<y> 0.283 </y>
<x> -0.283 </x>
<y> 0.00 </y>
<z> 0.125 </z>
</location>
<orient unit="DEG">
@ -184,10 +185,10 @@ @@ -184,10 +185,10 @@
<p_factor> 10 </p_factor>
</thruster>
</engine>
<engine file="a2830-12" name="right">
<engine file="a2830-12" name="rear">
<location unit="M">
<x> 0.000 </x>
<y> -0.283 </y>
<x> 0.283 </x>
<y> 0.000 </y>
<z> 0.000 </z>
</location>
<orient unit="DEG">
@ -198,8 +199,8 @@ @@ -198,8 +199,8 @@
<feed>0</feed>
<thruster file="prop10x4.5">
<location unit="M">
<x> 0.000 </x>
<y> -0.283 </y>
<x> 0.283 </x>
<y> 0.000 </y>
<z> 0.125 </z>
</location>
<orient unit="DEG">
@ -211,6 +212,8 @@ @@ -211,6 +212,8 @@
<p_factor> 10 </p_factor>
</thruster>
</engine>
<tank type="FUEL" number="0">
<location unit="M">
<x> 0.00 </x>
@ -222,6 +225,7 @@ @@ -222,6 +225,7 @@
<contents unit="KG"> 0.0 </contents>
</tank>
</propulsion>
<aerodynamics>
<axis name="LIFT">
</axis>

56
files/flightgear/Protocol/qgroundcontrol-quadrotor.xml
Unescape View File

@ -142,6 +142,49 @@ @@ -142,6 +142,49 @@
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk>
<chunk>
<name>airspeed-indicated-mps</name>
<type>float</type>
<format>%.8f</format>
<node>/instrumentation/indicated-speed-kt</node>
<factor>0.514444444444444</factor> <!-- knots to mps -->
</chunk>
<!-- Magnetometer -->
<chunk>
<name>Magnetic Variation (rad)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/magnetic-variation-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<chunk>
<name>Magnetic Dip (rad)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/magnetic-dip-deg</node>
<factor>0.01745329251994329576</factor> <!-- degrees to radians -->
</chunk>
<!-- Temperature -->
<chunk>
<name>Temperature (deg C)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/temperature-degc</node>
<factor>1</factor>
</chunk>
<!-- Pressure -->
<chunk>
<name>Pressure (hPa)</name>
<type>float</type>
<format>%.8f</format>
<node>/environment/pressure-inhg</node>
<factor>33.86389</factor> <!-- inhg to hpa -->
</chunk>
</output>
<input>
@ -152,22 +195,27 @@ @@ -152,22 +195,27 @@
<!-- motors, in range 0.0 to 1.0 -->
<chunk>
<name>throttle0</name>
<type>double</type>
<type>float</type>
<node>/controls/engines/engine[0]/throttle</node>
</chunk>
<chunk>
<name>throttle1</name>
<type>double</type>
<type>float</type>
<node>/controls/engines/engine[1]/throttle</node>
</chunk>
<chunk>
<name>throttle2</name>
<type>double</type>
<type>float</type>
<node>/controls/engines/engine[2]/throttle</node>
</chunk>
<chunk>
<name>running</name>
<type>bool</type>
<node>/engines/engine/running</node>
</chunk>
<chunk>
<name>throttle3</name>
<type>double</type>
<type>float</type>
<node>/controls/engines/engine[3]/throttle</node>
</chunk>

335
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -40,14 +40,11 @@ This file is part of the QGROUNDCONTROL project @@ -40,14 +40,11 @@ This file is part of the QGROUNDCONTROL project
#include <QHostInfo>
#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)
{
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;
@ -57,8 +54,7 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments @@ -57,8 +54,7 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments
setRemoteHost(remoteHost);
}
QGCFlightGearLink::~QGCFlightGearLink()
{ //do not disconnect unless it is connected.
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();
@ -69,40 +65,49 @@ QGCFlightGearLink::~QGCFlightGearLink() @@ -69,40 +65,49 @@ QGCFlightGearLink::~QGCFlightGearLink()
* @brief Runs the thread
*
**/
void QGCFlightGearLink::run()
{
void QGCFlightGearLink::run() {
exec();
}
void QGCFlightGearLink::setPort(int port)
{
void QGCFlightGearLink::setPort(int port) {
this->port = port;
disconnectSimulation();
connectSimulation();
}
void QGCFlightGearLink::processError(QProcess::ProcessError err)
{
switch(err)
{
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"));
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"));
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"));
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"));
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"));
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."));
MainWindow::instance()->showCriticalMessage(
tr("FlightGear/TerraSync Error"),
tr("Please check if the path and command is correct."));
break;
}
}
@ -110,22 +115,17 @@ void QGCFlightGearLink::processError(QProcess::ProcessError err) @@ -110,22 +115,17 @@ void QGCFlightGearLink::processError(QProcess::ProcessError err)
/**
* @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(":"))
{
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)
{
if (info.error() == QHostInfo::NoError) {
// Add host
QList<QHostAddress> hostAddresses = info.addresses();
QHostAddress address;
for (int i = 0; i < hostAddresses.size(); i++)
{
for (int i = 0; i < hostAddresses.size(); i++) {
// Exclude loopback IPv4 and all IPv6 addresses
if (!hostAddresses.at(i).toString().contains(":"))
{
if (!hostAddresses.at(i).toString().contains(":")) {
address = hostAddresses.at(i);
}
}
@ -134,12 +134,9 @@ void QGCFlightGearLink::setRemoteHost(const QString& host) @@ -134,12 +134,9 @@ void QGCFlightGearLink::setRemoteHost(const QString& host)
// Set port according to user input
currentPort = host.split(":").last().toInt();
}
}
else
{
} else {
QHostInfo info = QHostInfo::fromName(host);
if (info.error() == QHostInfo::NoError)
{
if (info.error() == QHostInfo::NoError) {
// Add host
currentHost = info.addresses().first();
}
@ -147,8 +144,9 @@ void QGCFlightGearLink::setRemoteHost(const QString& host) @@ -147,8 +144,9 @@ void QGCFlightGearLink::setRemoteHost(const QString& host)
}
void QGCFlightGearLink::updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float 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);
@ -160,8 +158,9 @@ void QGCFlightGearLink::updateActuators(uint64_t time, float act1, float act2, f @@ -160,8 +158,9 @@ void QGCFlightGearLink::updateActuators(uint64_t time, float act1, float act2, f
Q_UNUSED(act8);
}
void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode)
{
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;
@ -169,23 +168,26 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float @@ -169,23 +168,26 @@ void QGCFlightGearLink::updateControls(uint64_t time, float rollAilerons, float
Q_UNUSED(systemMode);
Q_UNUSED(navMode);
if(!isnan(rollAilerons) && !isnan(pitchElevator) && !isnan(yawRudder) && !isnan(throttle))
{
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);
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" << 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;
@ -206,7 +208,8 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size) @@ -206,7 +208,8 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
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,15 +218,17 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size) @@ -215,15 +218,17 @@ 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()
{
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;
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);
@ -235,9 +240,9 @@ void QGCFlightGearLink::readBytes() @@ -235,9 +240,9 @@ void QGCFlightGearLink::readBytes()
QStringList values = state.split("\t");
// Check length
if (values.size() != 22)
{
qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size();
if (values.size() != 22) {
qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT"
<< values.size();
qDebug() << state;
return;
}
@ -251,8 +256,8 @@ void QGCFlightGearLink::readBytes() @@ -251,8 +256,8 @@ void QGCFlightGearLink::readBytes()
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;
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();
@ -283,14 +288,14 @@ void QGCFlightGearLink::readBytes() @@ -283,14 +288,14 @@ void QGCFlightGearLink::readBytes()
// Send updated state
//qDebug() << "sensorHilEnabled: " << sensorHilEnabled;
if (_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));
float density = abs_pressure
/ (air_gas_constant * (temperature - absolute_null_celsius));
diff_pressure = true_airspeed * true_airspeed * density / 2.0f;
float pressure_alt = alt;
@ -298,7 +303,9 @@ void QGCFlightGearLink::readBytes() @@ -298,7 +303,9 @@ void QGCFlightGearLink::readBytes()
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);
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;
@ -325,7 +332,8 @@ void QGCFlightGearLink::readBytes() @@ -325,7 +332,8 @@ 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<Eigen::Matrix3f>((float*)R_B_N).eval();
Eigen::Matrix3f R_B_N_M =
Eigen::Map<Eigen::Matrix3f>((float*) R_B_N).eval();
Eigen::Vector3f mag_ned(xmag_ned, ymag_ned, zmag_ned);
@ -335,8 +343,10 @@ void QGCFlightGearLink::readBytes() @@ -335,8 +343,10 @@ void QGCFlightGearLink::readBytes()
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;
@ -346,15 +356,14 @@ void QGCFlightGearLink::readBytes() @@ -346,15 +356,14 @@ void QGCFlightGearLink::readBytes()
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);
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;
}
@ -369,14 +378,12 @@ void QGCFlightGearLink::readBytes() @@ -369,14 +378,12 @@ void QGCFlightGearLink::readBytes()
// std::cerr << std::endl;
}
/**
* @brief Get the number of bytes to read.
*
* @return The number of bytes to read
**/
qint64 QGCFlightGearLink::bytesAvailable()
{
qint64 QGCFlightGearLink::bytesAvailable() {
return socket->pendingDatagramSize();
}
@ -385,29 +392,45 @@ qint64 QGCFlightGearLink::bytesAvailable() @@ -385,29 +392,45 @@ 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)
{
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)
{
if (terraSync) {
terraSync->close();
delete terraSync;
terraSync = NULL;
}
if (socket)
{
if (socket) {
socket->close();
delete socket;
socket = NULL;
@ -425,11 +448,11 @@ bool QGCFlightGearLink::disconnectSimulation() @@ -425,11 +448,11 @@ bool QGCFlightGearLink::disconnectSimulation()
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCFlightGearLink::connectSimulation()
{
bool QGCFlightGearLink::connectSimulation() {
qDebug() << "STARTING FLIGHTGEAR LINK";
if (!mav) return false;
if (!mav)
return false;
socket = new QUdpSocket(this);
connectState = socket->bind(host, port);
@ -438,23 +461,42 @@ bool QGCFlightGearLink::connectSimulation() @@ -438,23 +461,42 @@ bool QGCFlightGearLink::connectSimulation()
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)));
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<UAS*>(mav);
if (uas)
{
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)));
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;
@ -487,7 +529,8 @@ bool QGCFlightGearLink::connectSimulation() @@ -487,7 +529,8 @@ bool QGCFlightGearLink::connectSimulation()
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;
@ -519,8 +562,8 @@ bool QGCFlightGearLink::connectSimulation() @@ -519,8 +562,8 @@ bool QGCFlightGearLink::connectSimulation()
// sane = false;
// }
if (!sane) return false;
if (!sane)
return false;
// --atlas=socket,out,1,localhost,5505,udp
// terrasync -p 5505 -S -d /usr/local/share/TerraSync
@ -529,15 +572,24 @@ bool QGCFlightGearLink::connectSimulation() @@ -529,15 +572,24 @@ bool QGCFlightGearLink::connectSimulation()
//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);
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";
@ -567,25 +619,28 @@ bool QGCFlightGearLink::connectSimulation() @@ -567,25 +619,28 @@ 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)
{
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
{
} 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());
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);
@ -613,8 +668,6 @@ bool QGCFlightGearLink::connectSimulation() @@ -613,8 +668,6 @@ bool QGCFlightGearLink::connectSimulation()
process->start(processFgfs, flightGearArguments);
emit simulationConnected(connectState);
if (connectState) {
emit simulationConnected();
@ -624,13 +677,11 @@ bool QGCFlightGearLink::connectSimulation() @@ -624,13 +677,11 @@ bool QGCFlightGearLink::connectSimulation()
// qDebug() << "STARTING: " << processFgfs << flightGearArguments;
start(LowPriority);
return connectState;
}
void QGCFlightGearLink::printTerraSyncOutput()
{
void QGCFlightGearLink::printTerraSyncOutput() {
qDebug() << "TerraSync stdout:";
QByteArray byteArray = terraSync->readAllStandardOutput();
QStringList strLines = QString(byteArray).split("\n");
@ -640,8 +691,7 @@ void QGCFlightGearLink::printTerraSyncOutput() @@ -640,8 +691,7 @@ void QGCFlightGearLink::printTerraSyncOutput()
}
}
void QGCFlightGearLink::printTerraSyncError()
{
void QGCFlightGearLink::printTerraSyncError() {
qDebug() << "TerraSync stderr:";
QByteArray byteArray = terraSync->readAllStandardError();
@ -656,8 +706,7 @@ void QGCFlightGearLink::printTerraSyncError() @@ -656,8 +706,7 @@ void QGCFlightGearLink::printTerraSyncError()
* @brief Set the startup arguments used to start flightgear
*
**/
void QGCFlightGearLink::setStartupArguments(QString startupArguments)
{
void QGCFlightGearLink::setStartupArguments(QString startupArguments) {
this->startupArguments = startupArguments;
}
@ -666,23 +715,19 @@ void QGCFlightGearLink::setStartupArguments(QString startupArguments) @@ -666,23 +715,19 @@ void QGCFlightGearLink::setStartupArguments(QString startupArguments)
*
* @return True if link is connected, false otherwise.
**/
bool QGCFlightGearLink::isConnected()
{
bool QGCFlightGearLink::isConnected() {
return connectState;
}
QString QGCFlightGearLink::getName()
{
QString QGCFlightGearLink::getName() {
return name;
}
QString QGCFlightGearLink::getRemoteHost()
{
QString QGCFlightGearLink::getRemoteHost() {
return QString("%1:%2").arg(currentHost.toString(), currentPort);
}
void QGCFlightGearLink::setName(QString name)
{
void QGCFlightGearLink::setName(QString name) {
this->name = name;
// emit nameChanged(this->name);
}

Write Preview
Loading…
Cancel
Save