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

Added FlightGear interface framework, not yet compiled and not yet fully operational

QGC4.4
lm 14 years ago
parent
1dd786356d
commit
5ab17db6ea
3 changed files with 574 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 161
      flightgear/connector_flightgear_uav.xml
  2. 301
      src/comm/QGCFlightGearLink.cc
  3. 112
      src/comm/QGCFlightGearLink.h

161
flightgear/connector_flightgear_uav.xml
Unescape View File

@ -0,0 +1,161 @@
<PropertyList>
<generic>
<output>
<line_separator>newline</line_separator>
<var_separator>,</var_separator>
<chunk>
<name>lat</name>
<type>float</type>
<format>%+1.8f</format>
<node>/position/latitude-deg</node>
</chunk>
<chunk>
<name>lon</name>
<type>float</type>
<format>%+1.8f</format>
<node>/position/longitude-deg</node>
</chunk>
<chunk>
<name>alt</name>
<type>float</type>
<format>%+1.4f</format>
<node>/position/altitude-ft</node>
</chunk>
<chunk>
<name>speed</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/groundspeed-kt</node>
</chunk>
<chunk>
<name>airspeed</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/airspeed-kt</node>
</chunk>
<chunk>
<name>pitch</name>
<type>float</type>
<format>%+1.3f</format>
<node>/orientation/pitch-deg</node>
</chunk>
<chunk>
<name>roll</name>
<type>float</type>
<format>%+1.3f</format>
<node>/orientation/roll-deg</node>
</chunk>
<chunk>
<name>heading</name>
<type>float</type>
<format>%+1.3f</format>
<node>/orientation/heading-deg</node>
</chunk>
<chunk>
<name>v_n</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-north-fps</node>
</chunk>
<chunk>
<name>v_e</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-east-fps</node>
</chunk>
<chunk>
<name>v_d</name>
<type>float</type>
<format>%2.3f</format>
<node>/velocities/speed-down-fps</node>
</chunk>
<chunk>
<name>rate_phi</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/roll-rate-degps</node>
</chunk>
<chunk>
<name>rate_theta</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/pitch-rate-degps</node>
</chunk>
<chunk>
<name>rate_psi</name>
<type>float</type>
<format>%2.3f</format>
<node>/orientation/yaw-rate-degps</node>
</chunk>
</output>
<input>
<line_separator>newline</line_separator>
<var_separator>,</var_separator>
<chunk>
<name>pitch</name>
<type>float</type>
<format>%+1.3f</format>
<node>/autopilot/settings/target-pitch-deg</node>
</chunk>
<chunk>
<name>roll</name>
<type>float</type>
<format>%+1.3f</format>
<node>/autopilot/settings/target-roll-deg</node>
</chunk>
<chunk>
<name>throttle</name>
<type>float</type>
<format>%+1.3f</format>
<node>/controls/engines/engine/throttle</node>
</chunk>
<chunk>
<name>lat</name>
<type>float</type>
<format>%+1.8f</format>
<node>/sim/view/target/latitude-deg</node>
</chunk>
<chunk>
<name>lon</name>
<type>float</type>
<format>%+1.8f</format>
<node>/sim/view/target/longitude-deg</node>
</chunk>
<chunk>
<name>alt</name>
<type>float</type>
<format>%+1.4f</format>
<node>/sim/view/target/alt</node>
</chunk>
</input>
</generic>
</PropertyList>

301
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -0,0 +1,301 @@
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of UDP connection (server) for unmanned vehicles
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <iostream>
#include "QGCFlightGearLink.h"
#include "LinkManager.h"
#include "QGC.h"
#include <QHostInfo>
//#include <netinet/in.h>
QGCFlightGearLink::QGCFlightGearLink(QHostAddress host, quint16 port)
{
this->host = host;
this->port = port;
this->connectState = false;
// Set unique ID and add link to the list of links
this->name = tr("FlightGear Link (port:%1)").arg(5401);
LinkManager::instance()->add(this);
}
QGCFlightGearLink::~QGCFlightGearLink()
{
disconnect();
}
/**
* @brief Runs the thread
*
**/
void QGCFlightGearLink::run()
{
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
exec();
}
void QGCFlightGearLink::setPort(int port)
{
this->port = port;
disconnect();
connect();
}
/**
* @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
*/
void QGCFlightGearLink::setRemoteHost(const QString& host)
{
//qDebug() << "UDP:" << "ADDING HOST:" << host;
if (host.contains(":"))
{
//qDebug() << "HOST: " << host.split(":").first();
QHostInfo info = QHostInfo::fromName(host.split(":").first());
if (info.error() == QHostInfo::NoError)
{
// Add host
QList<QHostAddress> 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(":");
}
}
else
{
QHostInfo info = QHostInfo::fromName(host);
if (info.error() == QHostInfo::NoError)
{
// Add host
currentHost = info.addresses().first();
}
}
}
void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
{
// Broadcast to all connected systems
for (int h = 0; h < hosts.size(); h++)
{
quint16 currentPort = ports.at(h);
//#define QGCFlightGearLink_DEBUG
#ifdef QGCFlightGearLink_DEBUG
QString bytes;
QString ascii;
for (int i=0; i<size; i++)
{
unsigned char v = data[i];
bytes.append(QString().sprintf("%02x ", v));
if (data[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
socket->writeDatagram(data, size, 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 QGCFlightGearLink::readBytes()
{
const qint64 maxLength = 65536;
static 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);
// FIXME TODO Check if this method is better than retrieving the data by individual processes
QByteArray b(data, s);
emit bytesReceived(this, b);
// // Echo data for debugging purposes
// std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
// int i;
// for (i=0; i<s; i++)
// {
// unsigned int v=data[i];
// fprintf(stderr,"%02x ", v);
// }
// std::cerr << std::endl;
// Add host to broadcast list if not yet present
if (!hosts.contains(sender))
{
hosts.append(sender);
ports.append(senderPort);
// ports->insert(sender, senderPort);
}
else
{
int index = hosts.indexOf(sender);
ports.replace(index, senderPort);
}
}
/**
* @brief Get the number of bytes to read.
*
* @return The number of bytes to read
**/
qint64 QGCFlightGearLink::bytesAvailable()
{
return socket->pendingDatagramSize();
}
/**
* @brief Disconnect the connection.
*
* @return True if connection has been disconnected, false if connection couldn't be disconnected.
**/
bool QGCFlightGearLink::disconnect()
{
delete socket;
socket = NULL;
connectState = false;
emit disconnected();
emit connected(false);
return !connectState;
}
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCFlightGearLink::connect()
{
socket = new QUdpSocket(this);
//Check if we are using a multicast-address
// bool multicast = false;
// if (host.isInSubnet(QHostAddress("224.0.0.0"),4))
// {
// multicast = true;
// connectState = socket->bind(port, QUdpSocket::ShareAddress);
// }
// else
// {
connectState = socket->bind(host, port);
// }
//Provides Multicast functionality to UdpSocket
/* not working yet
if (multicast)
{
int sendingFd = socket->socketDescriptor();
if (sendingFd != -1)
{
// set up destination address
struct sockaddr_in sendAddr;
memset(&sendAddr,0,sizeof(sendAddr));
sendAddr.sin_family=AF_INET;
sendAddr.sin_addr.s_addr=inet_addr(HELLO_GROUP);
sendAddr.sin_port=htons(port);
// set TTL
unsigned int ttl = 1; // restricted to the same subnet
if (setsockopt(sendingFd, IPPROTO_IP, IP_MULTICAST_TTL, (unsigned int*)&ttl, sizeof(ttl) ) < 0)
{
std::cout << "TTL failed\n";
}
}
}
*/
//QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
emit connected(connectState);
if (connectState) {
emit connected();
connectionStartTime = QGC::groundTimeUsecs()/1000;
}
start(HighPriority);
return connectState;
}
/**
* @brief Check if connection is active.
*
* @return True if link is connected, false otherwise.
**/
bool QGCFlightGearLink::isConnected()
{
return connectState;
}
QString QGCFlightGearLink::getName()
{
return name;
}
void QGCFlightGearLink::setName(QString name)
{
this->name = name;
emit nameChanged(this->name);
}

112
src/comm/QGCFlightGearLink.h
Unescape View File

@ -0,0 +1,112 @@
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief UDP connection (server) for unmanned vehicles
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef QGCFLIGHTGEARLINK_H
#define QGCFLIGHTGEARLINK_H
#include <QString>
#include <QList>
#include <QMap>
#include <QMutex>
#include <QUdpSocket>
#include <LinkInterface.h>
#include <configuration.h>
class QGCFlightGearLink : public QThread
{
Q_OBJECT
//Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface)
public:
QGCFlightGearLink(QHostAddress host = QHostAddress::Any, quint16 port = 5401);
//QGCFlightGearLink(QHostAddress host = "239.255.76.67", quint16 port = 7667);
~QGCFlightGearLink();
bool isConnected();
qint64 bytesAvailable();
int getPort() const {
return port;
}
/**
* @brief The human readable port name
*/
QString getName();
void run();
public slots:
void setAddress(QString address);
void setPort(int port);
/** @brief Add a new host to broadcast messages to */
void addHost(const QString& host);
/** @brief Remove a host from broadcasting messages to */
void removeHost(const QString& host);
// void readPendingDatagrams();
void readBytes();
/**
* @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 char* data, qint64 length);
bool connect();
bool disconnect();
protected:
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;
void setName(QString name);
signals:
// Signals are defined by LinkInterface
};
#endif // QGCFLIGHTGEARLINK_H
Write Preview
Loading…
Cancel
Save