Skip to content
GitLab
Commit bdcfb523 authored by LM's avatar LM
Browse files

Merged

parents 02ac005a 90e3dbf4
Hide whitespace changes
Inline Side-by-side
.gitignore
View file @ bdcfb523
......@@ -8,6 +8,7 @@ Info.plist
obj
*.log
*~
*~.skp
bin/*.exe
bin/*.txt
bin/mac
......
flightgear/connector_flightgear_uav.xml 0 → 100644
View file @ bdcfb523
<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>
models/ascent-park-glider.skp
View file @ bdcfb523
File suppressed by a .gitattributes entry or the file's encoding is unsupported.
models/multiplex-twinstar.skp
View file @ bdcfb523
File suppressed by a .gitattributes entry or the file's encoding is unsupported.
qgroundcontrol.pro
View file @ bdcfb523
......@@ -235,6 +235,7 @@ HEADERS += src/MG.h \
src/comm/SerialSimulationLink.h \
src/comm/ProtocolInterface.h \
src/comm/MAVLinkProtocol.h \
src/comm/QGCFlightGearLink.h \
src/ui/CommConfigurationWindow.h \
src/ui/SerialConfigurationWindow.h \
src/ui/MainWindow.h \
......@@ -361,6 +362,7 @@ SOURCES += src/main.cc \
src/comm/SerialLink.cc \
src/comm/SerialSimulationLink.cc \
src/comm/MAVLinkProtocol.cc \
src/comm/QGCFlightGearLink.cc \
src/ui/CommConfigurationWindow.cc \
src/ui/SerialConfigurationWindow.cc \
src/ui/MainWindow.cc \
......
src/comm/QGCFlightGearLink.cc 0 → 100644
View file @ bdcfb523
/*=====================================================================
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 "QGC.h"
#include <QHostInfo>
QGCFlightGearLink::QGCFlightGearLink(QString remoteHost, QHostAddress host, quint16 port)
{
this->host = host;
this->port = port;
this->connectState = false;
this->currentPort = 49000;
// Set unique ID and add link to the list of links
this->name = tr("FlightGear Link (port:%1)").arg(port);
setRemoteHost(remoteHost);
connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
refreshTimer.start(20); // 50 Hz UAV -> Simulation update rate
}
QGCFlightGearLink::~QGCFlightGearLink()
{
disconnect();
}
/**
* @brief Runs the thread
*
**/
void QGCFlightGearLink::run()
{
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
exec();
}
void QGCFlightGearLink::setPort(int port)
{
this->port = port;
disconnectSimulation();
connectSimulation();
}
/**
* @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(":").last().toInt();
}
}
else
{
QHostInfo info = QHostInfo::fromName(host);
if (info.error() == QHostInfo::NoError)
{
// Add host
currentHost = info.addresses().first();
}
}
}
void QGCFlightGearLink::updateGlobalPosition(quint64 time, double lat, double lon, double alt)
{
}
void QGCFlightGearLink::sendUAVUpdate()
{
// 37.613548,-122.357246,-9999.000000,0.000000,0.424000,297.899994,0.000000\n
// magnetos,aileron,elevator,rudder,throttle\n
float magnetos = 3.0f;
float aileron = 0.0f;
float elevator = 0.0f;
float rudder = 0.0f;
float throttle = 90.0f;
QString state("%1,%2,%3,%4,%5\n");
state = state.arg(magnetos).arg(aileron).arg(elevator).arg(rudder).arg(throttle);
writeBytes(state.toAscii().constData(), state.length());
}
void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
{
//#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);
// Print string
qDebug() << "FG LINK GOT:" << QString(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;
}
/**
* @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::disconnectSimulation()
{
delete socket;
socket = NULL;
connectState = false;
emit flightGearDisconnected();
emit flightGearConnected(false);
return !connectState;
}
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCFlightGearLink::connectSimulation()
{
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 flightGearConnected(connectState);
if (connectState) {
emit flightGearConnected();
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 @ bdcfb523
/*=====================================================================
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 <QTimer>
#include <LinkInterface.h>
#include <configuration.h>
class QGCFlightGearLink : public QThread
{
Q_OBJECT
//Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface)
public:
QGCFlightGearLink(QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005);
~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 setRemoteHost(const QString& host);
void updateGlobalPosition(quint64 time, double lat, double lon, double alt);
void sendUAVUpdate();
// /** @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 connectSimulation();
bool disconnectSimulation();
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;
QTimer refreshTimer;
void setName(QString name);
signals:
/**
* @brief This signal is emitted instantly when the link is connected
**/
void flightGearConnected();
/**
* @brief This signal is emitted instantly when the link is disconnected
**/
void flightGearDisconnected();
/**
* @brief This signal is emitted instantly when the link status changes
**/
void flightGearConnected(bool connected);
};
#endif // QGCFLIGHTGEARLINK_H
src/uas/UAS.cc
View file @ bdcfb523
......@@ -74,7 +74,9 @@ statusTimeout(new QTimer(this)),
paramsOnceRequested(false),
airframe(0),
attitudeKnown(false),
paramManager(NULL)
paramManager(NULL),
attitudeStamped(true),
lastAttitude(0)
{
color = UASInterface::getNextColor();
setBattery(LIPOLY, 3);
......@@ -187,7 +189,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;
if (message.sysid == uasId)
// Only accept messages from this system (condition 1)
// and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled
// and we already got one attitude packet
if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
{
QString uasState;
QString stateDescription;
......@@ -457,7 +462,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
quint64 time = getUnixTime(attitude.usec);
quint64 time = getUnixReferenceTime(attitude.usec);
lastAttitude = time;
roll = QGC::limitAngleToPMPIf(attitude.roll);
pitch = QGC::limitAngleToPMPIf(attitude.pitch);
yaw = QGC::limitAngleToPMPIf(attitude.yaw);
......@@ -1259,8 +1265,65 @@ void UAS::startPressureCalibration()
// sendMessage(msg);
}
quint64 UAS::getUnixReferenceTime(quint64 time)
{
// Same as getUnixTime, but does not react to attitudeStamped mode
if (time == 0)
{
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds();
}
// Check if time is smaller than 40 years,
// assuming no system without Unix timestamp
// runs longer than 40 years continuously without
// reboot. In worst case this will add/subtract the
// communication delay between GCS and MAV,
// it will never alter the timestamp in a safety
// critical way.
//
// Calculation:
// 40 years
// 365 days
// 24 hours
// 60 minutes
// 60 seconds
// 1000 milliseconds
// 1000 microseconds
#ifndef _MSC_VER
else if (time < 1261440000000000LLU)
#else
else if (time < 1261440000000000)
#endif
{
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
if (onboardTimeOffset == 0)
{
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
}
return time/1000 + onboardTimeOffset;
}
else
{
// Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing.
return time/1000;
}
}
/**
* @warning If attitudeStamped is enabled, this function will not actually return the precise time stamp
* of this measurement augmented to UNIX time, but will MOVE the timestamp IN TIME to match
* the last measured attitude. There is no reason why one would want this, except for
* system setups where the onboard clock is not present or broken and datasets should
* be collected that are still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED
* RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
*/
quint64 UAS::getUnixTime(quint64 time)
{
if (attitudeStamped)
{
return lastAttitude;
}
if (time == 0)
{
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
......
src/uas/UAS.h
View file @ bdcfb523
......@@ -204,6 +204,8 @@ protected: //COMMENTS FOR TEST UNIT
QGCUASParamManager* paramManager; ///< Parameter manager class
QString shortStateText; ///< Short textual state description
QString shortModeText; ///< Short textual mode description
bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
quint64 lastAttitude; ///< Timestamp of last attitude measurement
public:
/** @brief Set the current battery type */
......@@ -401,6 +403,8 @@ signals:
protected:
/** @brief Get the UNIX timestamp in milliseconds */
quint64 getUnixTime(quint64 time=0);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
quint64 getUnixReferenceTime(quint64 time);
protected slots:
/** @brief Write settings to disk */
......
src/ui/MainWindow.cc
View file @ bdcfb523
......@@ -157,6 +157,11 @@ MainWindow::MainWindow(QWidget *parent):
joystickWidget = 0;
joystick = new JoystickInput();
// Connect flighgear test link
// FIXME MOVE INTO UAV OBJECT
fgLink = new QGCFlightGearLink();
fgLink->connectSimulation();
// Load Toolbar
toolBar = new QGCToolBar(this);
this->addToolBar(toolBar);
......
src/ui/MainWindow.h
View file @ bdcfb523
......@@ -74,6 +74,7 @@ This file is part of the QGROUNDCONTROL project
#include "SlugsPadCameraControl.h"
#include "UASControlParameters.h"
#include "QGCFlightGearLink.h"
class QGCMapTool;
......@@ -443,6 +444,7 @@ protected:
QGC_MAINWINDOW_STYLE currentStyle;
Qt::WindowStates windowStateVal;
bool lowPowerMode; ///< If enabled, QGC reduces the update rates of all widgets
QGCFlightGearLink* fgLink;
private:
Ui::MainWindow ui;
......
src/ui/uas/UASView.cc
View file @ bdcfb523
......@@ -602,6 +602,8 @@ void UASView::refresh()
m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
m_ui->uasViewFrame->setStyleSheet(style);
refreshTimer->setInterval(errorUpdateInterval);
}
iconIsRed = !iconIsRed;
} else {
......@@ -609,10 +611,11 @@ void UASView::refresh()
{
// Fade heartbeat icon
// Make color darker
heartbeatColor = heartbeatColor.darker(150);
heartbeatColor = heartbeatColor.darker(210);
//m_ui->heartbeatIcon->setAutoFillBackground(true);
m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
refreshTimer->setInterval(updateInterval);
}
}
//setUpdatesEnabled(true);
......
src/ui/uas/UASView.h
View file @ bdcfb523
......@@ -122,7 +122,8 @@ protected:
QAction* selectAction;
QAction* selectAirframeAction;
QAction* setBatterySpecsAction;
static const int updateInterval = 700;
static const int updateInterval = 800;
static const int errorUpdateInterval = 200;
bool lowPowerModeEnabled; ///< Low power mode reduces update rates
......
Supports Markdown
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