Commit 2ead9b7d authored by Lorenz Meier's avatar Lorenz Meier

Get X-Plane 9 supported

parent f1c2703d
......@@ -41,7 +41,8 @@ This file is part of the QGROUNDCONTROL project
QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port) :
process(NULL),
terraSync(NULL)
terraSync(NULL),
flightGearVersion(0)
{
this->host = host;
this->port = port+mav->getUASID();
......
......@@ -69,6 +69,16 @@ public:
*/
QString getRemoteHost();
QString getVersion()
{
return QString("FlightGear %1").arg(flightGearVersion);
}
int getAirFrameIndex()
{
return -1;
}
void run();
public slots:
......@@ -83,6 +93,11 @@ public slots:
// void removeHost(const QString& host);
// void readPendingDatagrams();
void processError(QProcess::ProcessError err);
/** @brief Set the simulator version as text string */
void setVersion(const QString& version)
{
Q_UNUSED(version);
}
void readBytes();
/**
......@@ -118,6 +133,7 @@ protected:
UASInterface* mav;
QProcess* process;
QProcess* terraSync;
unsigned int flightGearVersion;
void setName(QString name);
......
......@@ -25,6 +25,18 @@ public:
*/
virtual QString getRemoteHost() = 0;
/**
* @brief Get the application name and version
* @return A string containing a unique application name and compatibility version
*/
virtual QString getVersion() = 0;
/**
* @brief Get index of currently selected airframe
* @return -1 if default is selected, index else
*/
virtual int getAirFrameIndex() = 0;
public slots:
virtual void setPort(int port) = 0;
/** @brief Add a new host to broadcast messages to */
......@@ -33,6 +45,8 @@ public slots:
virtual void updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode) = 0;
virtual void updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8) = 0;
virtual void processError(QProcess::ProcessError err) = 0;
/** @brief Set the simulator version as text string */
virtual void setVersion(const QString& version) = 0;
virtual void readBytes() = 0;
/**
......@@ -74,6 +88,12 @@ signals:
/** @brief Status text message from link */
void statusMessage(const QString& message);
/** @brief Airframe changed */
void airframeChanged(const QString& airframe);
/** @brief Selected sim version changed */
void versionChanged(const QString& version);
};
#endif // QGCHILLINK_H
......@@ -46,7 +46,8 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
process(NULL),
terraSync(NULL),
airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
xPlaneConnected(false)
xPlaneConnected(false),
xPlaneVersion(0)
{
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
......@@ -71,6 +72,8 @@ void QGCXPlaneLink::loadSettings()
settings.sync();
settings.beginGroup("QGC_XPLANE_LINK");
setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString());
setVersion(settings.value("XPLANE_VERSION", 10).toString());
selectPlane(settings.value("AIRFRAME", "default").toString());
settings.endGroup();
}
......@@ -80,10 +83,44 @@ void QGCXPlaneLink::storeSettings()
QSettings settings;
settings.beginGroup("QGC_XPLANE_LINK");
settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
settings.setValue("XPLANE_VERSION", xPlaneVersion);
settings.setValue("AIRFRAME", airframeName);
settings.endGroup();
settings.sync();
}
void QGCXPlaneLink::setVersion(const QString& version)
{
unsigned int oldVersion = xPlaneVersion;
if (version.contains("9"))
{
xPlaneVersion = 9;
}
else if (version.contains("10"))
{
xPlaneVersion = 10;
}
else if (version.contains("11"))
{
xPlaneVersion = 11;
}
else if (version.contains("12"))
{
xPlaneVersion = 12;
}
if (oldVersion != xPlaneVersion)
{
emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
}
}
//void QGCXPlaneLink::setVersion(unsigned int version)
//{
//// bool changed = (xPlaneVersion != version);
// xPlaneVersion = version;
//}
/**
* @brief Runs the thread
......@@ -321,6 +358,9 @@ void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
**/
void QGCXPlaneLink::readBytes()
{
// Only emit updates on attitude message
bool emitUpdate = false;
const qint64 maxLength = 65536;
char data[maxLength];
QHostAddress sender;
......@@ -388,12 +428,13 @@ void QGCXPlaneLink::readBytes()
pitchspeed = p.f[1];
yawspeed = p.f[0];
}
else if (p.index == 17)
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
{
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
pitch = p.f[0] / 180.0f * M_PI;
roll = p.f[1] / 180.0f * M_PI;
yaw = p.f[2] / 180.0f * M_PI;
emitUpdate = true;
}
// else if (p.index == 19)
......@@ -449,9 +490,12 @@ void QGCXPlaneLink::readBytes()
}
// Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
if (emitUpdate)
{
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
}
if (!oldConnectionState && xPlaneConnected)
{
......@@ -535,22 +579,28 @@ void QGCXPlaneLink::selectPlane(const QString& plane)
if (plane.contains("QRO"))
{
if (plane.contains("MK"))
if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
{
airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
emit airframeChanged("QRO_X / MK");
}
else if (plane.contains("ARDRONE"))
else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
{
airframeID = AIRFRAME_QUAD_X_ARDRONE;
emit airframeChanged("QRO_X / ARDRONE");
}
else
{
bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
}
}
else
{
bool changed = (airframeID != AIRFRAME_UNKNOWN);
airframeID = AIRFRAME_UNKNOWN;
if (changed) emit airframeChanged("X Plane default");
}
}
......
......@@ -100,6 +100,18 @@ public slots:
void updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
/** @brief Send new motor control states to the simulation */
void updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8);
/** @brief Set the simulator version as text string */
void setVersion(const QString& version);
QString getVersion()
{
return QString("X-Plane %1").arg(xPlaneVersion);
}
int getAirFrameIndex()
{
return (int)airframeID;
}
void processError(QProcess::ProcessError err);
void readBytes();
......@@ -175,6 +187,7 @@ protected:
QString airframeName;
enum AIRFRAME airframeID;
bool xPlaneConnected;
unsigned int xPlaneVersion;
void setName(QString name);
......
......@@ -239,6 +239,34 @@ void UAS::updateState()
GAudioOutput::instance()->notifyNegative();
}
}
//#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0
//#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1
//#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2
//#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3
//#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4
//#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
//#warning THIS IS A HUGE HACK AND SHOULD NEVER SHOW UP IN ANY GIT REPOSITORY
// mavlink_message_t message;
// mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t sp;
// sp.group = 0;
// /* set rate mode, set zero rates and 20% throttle */
// sp.mode = MAVLINK_OFFBOARD_CONTROL_MODE_RATES | MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED;
// sp.roll[0] = INT16_MAX * 0.0f;
// sp.pitch[0] = INT16_MAX * 0.0f;
// sp.yaw[0] = INT16_MAX * 0.0f;
// sp.thrust[0] = UINT16_MAX * 0.3f;
// /* send from system 200 and component 0 */
// mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(200, 0, &message, &sp);
// sendMessage(message);
}
/**
......
......@@ -13,6 +13,9 @@ QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
connect(ui->hostComboBox, SIGNAL(activated(QString)), link, SLOT(setRemoteHost(QString)));
connect(link, SIGNAL(remoteChanged(QString)), ui->hostComboBox, SLOT(setEditText(QString)));
connect(link, SIGNAL(statusMessage(QString)), this, SLOT(receiveStatusMessage(QString)));
connect(link, SIGNAL(versionChanged(QString)), ui->simComboBox, SLOT(setEditText(QString)));
connect(ui->simComboBox, SIGNAL(activated(QString)), link, SLOT(setVersion(QString)));
ui->simComboBox->setEditText(link->getVersion());
ui->startButton->setText(tr("Connect"));
......@@ -23,6 +26,7 @@ QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
connect(ui->randomAttitudeButton, SIGNAL(clicked()), link, SLOT(setRandomAttitude()));
connect(ui->randomPositionButton, SIGNAL(clicked()), link, SLOT(setRandomPosition()));
connect(ui->airframeComboBox, SIGNAL(activated(QString)), link, SLOT(setAirframe(QString)));
ui->airframeComboBox->setCurrentIndex(link->getAirFrameIndex());
}
ui->hostComboBox->clear();
......
......@@ -19,6 +19,11 @@
<property name="editable">
<bool>true</bool>
</property>
<item>
<property name="text">
<string>X-Plane default</string>
</property>
</item>
<item>
<property name="text">
<string>QRO_X/MK</string>
......@@ -98,7 +103,12 @@
<widget class="QComboBox" name="simComboBox">
<item>
<property name="text">
<string>X-Plane</string>
<string>X-Plane 10</string>
</property>
</item>
<item>
<property name="text">
<string>X-Plane 9</string>
</property>
</item>
</widget>
......
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