Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
5ab17db6
Commit
5ab17db6
authored
Aug 02, 2011
by
lm
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Added FlightGear interface framework, not yet compiled and not yet fully operational
parent
1dd78635
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
574 additions
and
0 deletions
+574
-0
connector_flightgear_uav.xml
flightgear/connector_flightgear_uav.xml
+161
-0
QGCFlightGearLink.cc
src/comm/QGCFlightGearLink.cc
+301
-0
QGCFlightGearLink.h
src/comm/QGCFlightGearLink.h
+112
-0
No files found.
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
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment