Skip to content

Q
qgroundcontrol
Commit 5ab17db6 authored by lm's avatar lm
Browse files
Options

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

parent 1dd78635
Hide whitespace changes
Inline Side-by-side
Showing with 574 additions and 0 deletions
flightgear/connector_flightgear_uav.xml 0 → 100644
View file @ 5ab17db6
<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>
src/comm/QGCFlightGearLink.cc 0 → 100644
View file @ 5ab17db6
/*=====================================================================
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);
}
src/comm/QGCFlightGearLink.h 0 → 100644
View file @ 5ab17db6
/*=====================================================================
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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment