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

Pushed HIL with X-plane to being close to operational. Receiving from X-Plane implemented, sending to it is prepared. Needs an iteration of onboard SW first

QGC4.4
Lorenz Meier 13 years ago
parent
432ce33003
commit
bbdc9a84bb
9 changed files with 244 additions and 72 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. 9
      qgroundcontrol.pro
  2. 100
      src/comm/QGCXPlaneLink.cc
  3. 24
      src/comm/QGCXPlaneLink.h
  4. 2
      src/uas/UAS.cc
  5. 4
      src/uas/UAS.h
  6. 15
      src/ui/MainWindow.cc
  7. 36
      src/ui/QGCHilConfiguration.cc
  8. 31
      src/ui/QGCHilConfiguration.h
  9. 91
      src/ui/QGCHilConfiguration.ui

9
qgroundcontrol.pro
Unescape View File

@ -223,7 +223,8 @@ FORMS += src/ui/MainWindow.ui \ @@ -223,7 +223,8 @@ FORMS += src/ui/MainWindow.ui \
src/ui/mission/QGCMissionNavSweep.ui \
src/ui/mission/QGCMissionDoStartSearch.ui \
src/ui/mission/QGCMissionDoFinishSearch.ui \
src/ui/QGCVehicleConfig.ui
src/ui/QGCVehicleConfig.ui \
src/ui/QGCHilConfiguration.ui
INCLUDEPATH += src \
src/ui \
src/ui/linechart \
@ -360,7 +361,8 @@ HEADERS += src/MG.h \ @@ -360,7 +361,8 @@ HEADERS += src/MG.h \
src/ui/mission/QGCMissionDoStartSearch.h \
src/ui/mission/QGCMissionDoFinishSearch.h \
src/ui/QGCVehicleConfig.h \
src/comm/QGCHilLink.h
src/comm/QGCHilLink.h \
src/ui/QGCHilConfiguration.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@ -517,7 +519,8 @@ SOURCES += src/main.cc \ @@ -517,7 +519,8 @@ SOURCES += src/main.cc \
src/ui/mission/QGCMissionDoStartSearch.cc \
src/ui/mission/QGCMissionDoFinishSearch.cc \
src/ui/QGCVehicleConfig.cc \
src/comm/QGCHilLink.cc
src/comm/QGCHilLink.cc \
src/ui/QGCHilConfiguration.cc
# Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

100
src/comm/QGCXPlaneLink.cc
Unescape View File

@ -39,13 +39,14 @@ This file is part of the QGROUNDCONTROL project @@ -39,13 +39,14 @@ This file is part of the QGROUNDCONTROL project
#include "MainWindow.h"
QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
mav(mav),
socket(NULL),
process(NULL),
terraSync(NULL)
{
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
this->connectState = false;
this->mav = mav;
this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
setRemoteHost(remoteHost);
}
@ -209,55 +210,62 @@ void QGCXPlaneLink::readBytes() @@ -209,55 +210,62 @@ void QGCXPlaneLink::readBytes()
qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
union dataconv {
char c[8];
float f;
double d;
int i;
} u;
#pragma pack(push, 1)
struct payload {
int index;
float f[8];
} p;
#pragma pack(pop)
for (unsigned i = 0; i < nsegs; i++)
{
// Get index
unsigned ioff = (5+i*36);
memcpy(&(u.i), data+ioff, sizeof(u.i));
qDebug() << "ind:" << u.i;
unsigned doff = ioff+sizeof(u.i);
unsigned dsize = sizeof(u.d);
memcpy(&(u.d), data+doff, dsize);
doff += dsize;
qDebug() << "val1:" << u.d;
}
return;
// Parse string
double time;
quint64 time;
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
double lat, lon, alt;
double vx, vy, vz, xacc, yacc, zacc;
double airspeed;
// time = values.at(0).toDouble();
// lat = values.at(1).toDouble();
// lon = values.at(2).toDouble();
// alt = values.at(3).toDouble();
// roll = values.at(4).toDouble();
// pitch = values.at(5).toDouble();
// yaw = values.at(6).toDouble();
// rollspeed = values.at(7).toDouble();
// pitchspeed = values.at(8).toDouble();
// yawspeed = values.at(9).toDouble();
if (data[0] == 'D' &&
data[1] == 'A' &&
data[2] == 'T' &&
data[3] == 'A')
{
for (unsigned i = 0; i < nsegs; i++)
{
// Get index
unsigned ioff = (5+i*36);;
memcpy(&(p), data+ioff, sizeof(p));
if (p.index == 3)
{
qDebug() << "SPEEDS:" << p.f[0] << p.f[1] << p.f[2];
}
// xacc = values.at(10).toDouble();
// yacc = values.at(11).toDouble();
// zacc = values.at(12).toDouble();
if (p.index == 18)
{
qDebug() << "ANG VEL:" << p.f[0] << p.f[1] << p.f[2];
roll = p.f[0] / 180.0 * M_PI;
pitch = p.f[1] / 180.0 * M_PI;
yaw = p.f[1] / 180.0 * M_PI;
}
// vx = values.at(13).toDouble();
// vy = values.at(14).toDouble();
// vz = values.at(15).toDouble();
if (p.index == 19)
{
qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
}
// airspeed = values.at(16).toDouble();
if (p.index == 20)
{
qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
lat = p.f[0];
lon = p.f[1];
alt = p.f[2];
}
}
}
else
{
qDebug() << "UNKNOWN PACKET:" << data;
}
// Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
@ -293,10 +301,15 @@ qint64 QGCXPlaneLink::bytesAvailable() @@ -293,10 +301,15 @@ qint64 QGCXPlaneLink::bytesAvailable()
**/
bool QGCXPlaneLink::disconnectSimulation()
{
disconnect(process, SIGNAL(error(QProcess::ProcessError)),
if (!connectState) return true;
if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
this, SLOT(processError(QProcess::ProcessError)));
if (mav)
{
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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
}
if (process)
{
@ -334,6 +347,7 @@ bool QGCXPlaneLink::connectSimulation() @@ -334,6 +347,7 @@ bool QGCXPlaneLink::connectSimulation()
if (!mav) return false;
socket = new QUdpSocket(this);
connectState = socket->bind(localHost, localPort);
if (!connectState) return false;
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
@ -505,7 +519,7 @@ bool QGCXPlaneLink::connectSimulation() @@ -505,7 +519,7 @@ bool QGCXPlaneLink::connectSimulation()
// qDebug() << "STARTING: " << processFgfs << processCall;
qDebug() << "STARTING X-PLANE LINK";
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
start(LowPriority);
return connectState;

24
src/comm/QGCXPlaneLink.h
Unescape View File

@ -89,6 +89,7 @@ public slots: @@ -89,6 +89,7 @@ public slots:
bool disconnectSimulation();
protected:
UASInterface* mav;
QString name;
QHostAddress localHost;
quint16 localPort;
@ -108,34 +109,11 @@ protected: @@ -108,34 +109,11 @@ protected:
QMutex statisticsMutex;
QMutex dataMutex;
QTimer refreshTimer;
UASInterface* mav;
QProcess* process;
QProcess* terraSync;
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);
/** @brief State update from FlightGear */
void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc);
};
#endif // QGCXPLANESIMULATIONLINK_H

2
src/uas/UAS.cc
Unescape View File

@ -2603,7 +2603,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo @@ -2603,7 +2603,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
**/
void UAS::startHil()
{
// Connect Flight Gear Link
// Connect HIL simulation link
simulation->connectSimulation();
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);

4
src/uas/UAS.h
Unescape View File

@ -362,6 +362,10 @@ public: @@ -362,6 +362,10 @@ public:
QGCUASParamManager* getParamManager() const {
return paramManager;
}
/** @brief Get the HIL simulation */
QGCHilLink* getHILSimulation() const {
return simulation;
}
// TODO Will be removed
/** @brief Set reference to the param manager **/
void setParamManager(QGCUASParamManager* manager) {

15
src/ui/MainWindow.cc
Unescape View File

@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project @@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project
#include <QTimer>
#include <QHostInfo>
#include <QSplashScreen>
#include <QGCHilLink.h>
#include <QGCHilConfiguration.h>
#include "QGC.h"
#include "MAVLinkSimulationLink.h"
@ -1382,6 +1384,19 @@ void MainWindow::UASCreated(UASInterface* uas) @@ -1382,6 +1384,19 @@ void MainWindow::UASCreated(UASInterface* uas)
if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true);
if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true);
// Add simulation configuration widget
UAS* mav = dynamic_cast<UAS*>(uas);
if (mav)
{
QGCHilConfiguration* hconf = new QGCHilConfiguration(mav->getHILSimulation(), this);
QString hilDockName = tr("HIL Config (%1)").arg(uas->getUASName());
QDockWidget* hilDock = new QDockWidget(hilDockName, this);
hilDock->setWidget(hconf);
hilDock->setObjectName(QString("HIL_CONFIG_%1").arg(uas->getUASID()));
addTool(hilDock, hilDockName, Qt::RightDockWidgetArea);
}
// Reload view state in case new widgets were added
loadViewState();
}

36
src/ui/QGCHilConfiguration.cc
Unescape View File

@ -0,0 +1,36 @@ @@ -0,0 +1,36 @@
#include "QGCHilConfiguration.h"
#include "ui_QGCHilConfiguration.h"
QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
QWidget(parent),
link(link),
ui(new Ui::QGCHilConfiguration)
{
ui->setupUi(this);
connect(ui->startButton, SIGNAL(clicked(bool)), this, SLOT(toggleSimulation(bool)));
connect(ui->hostComboBox, SIGNAL(activated(QString)), link, SLOT(setRemoteHost(QString)));
ui->startButton->setText(tr("Connect"));
}
void QGCHilConfiguration::toggleSimulation(bool connect)
{
Q_UNUSED(connect);
if (!link->isConnected())
{
link->setRemoteHost(ui->hostComboBox->currentText());
link->connectSimulation();
ui->startButton->setText(tr("Disconnect"));
}
else
{
link->disconnectSimulation();
ui->startButton->setText(tr("Connect"));
}
}
QGCHilConfiguration::~QGCHilConfiguration()
{
delete ui;
}

31
src/ui/QGCHilConfiguration.h
Unescape View File

@ -0,0 +1,31 @@ @@ -0,0 +1,31 @@
#ifndef QGCHILCONFIGURATION_H
#define QGCHILCONFIGURATION_H
#include <QWidget>
#include "QGCHilLink.h"
namespace Ui {
class QGCHilConfiguration;
}
class QGCHilConfiguration : public QWidget
{
Q_OBJECT
public:
QGCHilConfiguration(QGCHilLink* link, QWidget *parent = 0);
~QGCHilConfiguration();
public slots:
/** Start / stop simulation */
void toggleSimulation(bool connect);
protected:
QGCHilLink* link;
private:
Ui::QGCHilConfiguration *ui;
};
#endif // QGCHILCONFIGURATION_H

91
src/ui/QGCHilConfiguration.ui
Unescape View File

@ -0,0 +1,91 @@ @@ -0,0 +1,91 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>QGCHilConfiguration</class>
<widget class="QWidget" name="QGCHilConfiguration">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>305</width>
<height>172</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="40,60,50,50">
<item row="4" column="0" colspan="3">
<widget class="QLabel" name="statusLabel">
<property name="text">
<string>Status</string>
</property>
</widget>
</item>
<item row="5" column="0" colspan="2">
<widget class="QPushButton" name="randomAttitudeButton">
<property name="text">
<string>Random ATT</string>
</property>
</widget>
</item>
<item row="3" column="0" colspan="2">
<widget class="QPushButton" name="startButton">
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="simLabel">
<property name="text">
<string>Simulator</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="hostLabel">
<property name="text">
<string>Host</string>
</property>
</widget>
</item>
<item row="3" column="2" colspan="2">
<widget class="QPushButton" name="setHomeButton">
<property name="text">
<string>Set HOME</string>
</property>
</widget>
</item>
<item row="1" column="1" colspan="3">
<widget class="QComboBox" name="hostComboBox">
<property name="editable">
<bool>true</bool>
</property>
<item>
<property name="text">
<string>127.0.0.1:49000</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1" colspan="3">
<widget class="QComboBox" name="simComboBox">
<item>
<property name="text">
<string>X-Plane</string>
</property>
</item>
</widget>
</item>
<item row="5" column="2" colspan="2">
<widget class="QPushButton" name="randomPositionButton">
<property name="text">
<string>Random POS</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
Write Preview
Loading…
Cancel
Save