diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 29c4dbc8136ebff618587b42f530dee6a1173a49..81d178a73f80d6039e3a9abbe4ace6a373e98854 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -50,7 +50,7 @@ OBJECTS_DIR = $${BUILDDIR}/obj MOC_DIR = $${BUILDDIR}/moc UI_DIR = $${BUILDDIR}/ui RCC_DIR = $${BUILDDIR}/rcc -MAVLINK_CONF = "" +MAVLINK_CONF = "pixhawk" MAVLINKPATH = $$BASEDIR/libs/mavlink/include/mavlink/v1.0 DEFINES += MAVLINK_NO_DATA diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 0bfeb5a1737e0aba7b5876a31c6e46cfe220ace5..9823c2827dd9b066dfcc02244c53c961f1132558 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -466,6 +466,70 @@ bool QGCXPlaneLink::disconnectSimulation() return !connectState; } +void QGCXPlaneLink::selectPlane(const QString& plane) +{ + +} + +void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw) +{ + struct VEH1_struct + { + quint32 p; + double lat_lon_ele[3]; + float psi_the_phi[3]; + float gear_flap_vect[3]; + } pos; + + pos.p = 0; + pos.lat_lon_ele[0] = lat; + pos.lat_lon_ele[1] = lon; + pos.lat_lon_ele[2] = alt; +} + +/** + * Sets a random position with an offset of max 1/1000 degree + * and max 100 m altitude + */ +void QGCXPlaneLink::setRandomPosition() +{ + // Initialize generator + srand(0); + + double offLat = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; + double offLon = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; + double offAlt = rand() / static_cast(RAND_MAX) * 200.0 + 100.0; + + if (mav->getAltitude() + offAlt < 0) + { + offAlt *= -1.0; + } + + setPositionAttitude(mav->getLatitude() + offLat, + mav->getLongitude() + offLon, + mav->getAltitude() + offAlt, + mav->getRoll(), + mav->getPitch(), + mav->getYaw()); +} + +void QGCXPlaneLink::setRandomAttitude() +{ + // Initialize generator + srand(0); + + double roll = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; + double pitch = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; + double yaw = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; + + setPositionAttitude(mav->getLatitude(), + mav->getLongitude(), + mav->getAltitude(), + roll, + pitch, + yaw); +} + /** * @brief Connect the connection. * diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index 2e11ea2d0f07be9c40e22f19c3c92856a8fd95d8..fc6d11b5a4da37c5db69980bae678ad02d122b20 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -87,6 +87,31 @@ public slots: void writeBytes(const char* data, qint64 length); bool connectSimulation(); bool disconnectSimulation(); + /** + * @brief Select airplane model + * @param plane the name of the airplane + */ + void selectPlane(const QString& plane); + /** + * @brief Set the airplane position and attitude + * @param lat + * @param lon + * @param alt + * @param roll + * @param pitch + * @param yaw + */ + void setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw); + + /** + * @brief Set a random position + */ + void setRandomPosition(); + + /** + * @brief Set a random attitude + */ + void setRandomAttitude(); protected: UASInterface* mav; diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index cf98bc78f169a739e931249e3128b3599ef6e6ff..c43506dfdc7f860950f2deaf511af341bd6ec4a7 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -216,7 +216,10 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 { mavlink_debug_vect_t debug; mavlink_msg_debug_vect_decode(msg, &debug); - name = name.arg(QString(debug.name), fieldName); + char buf[11]; + strncpy(buf, debug.name, 10); + buf[10] = '\0'; + name = QString(buf); time = getUnixTimeFromMs(msg->sysid, (debug.time_usec+500)/1000); // Scale to milliseconds, round up/down correctly } else if (msgid == MAVLINK_MSG_ID_DEBUG) @@ -230,14 +233,20 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 { mavlink_named_value_float_t debug; mavlink_msg_named_value_float_decode(msg, &debug); - name = debug.name; + char buf[11]; + strncpy(buf, debug.name, 10); + buf[10] = '\0'; + name = QString(buf); time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms); } else if (msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) { mavlink_named_value_int_t debug; mavlink_msg_named_value_int_decode(msg, &debug); - name = name.arg(debug.name).arg(fieldName); + char buf[11]; + strncpy(buf, debug.name, 10); + buf[10] = '\0'; + name = QString(buf); time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms); } // else if (msgid == MAVLINK_MSG_ID_HIGHRES_IMU) diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc index abefd1d28798dd736e563a88c6dce48641aec8b5..437f91e47293940ab6ece056e1866a22fe1a3303 100644 --- a/src/ui/QGCHilConfiguration.cc +++ b/src/ui/QGCHilConfiguration.cc @@ -1,5 +1,6 @@ #include "QGCHilConfiguration.h" #include "ui_QGCHilConfiguration.h" +#include "QGCXPlaneLink.h" QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) : QWidget(parent), @@ -12,6 +13,16 @@ QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) : connect(ui->hostComboBox, SIGNAL(activated(QString)), link, SLOT(setRemoteHost(QString))); ui->startButton->setText(tr("Connect")); + + QGCXPlaneLink* xplane = dynamic_cast(link); + + if (xplane) + { + 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))); + } +// connect(ui->) } void QGCHilConfiguration::toggleSimulation(bool connect) diff --git a/src/ui/QGCHilConfiguration.ui b/src/ui/QGCHilConfiguration.ui index 7af82b2c53f6f16e75ba39e65ee29c2263494c24..1db0c99f09cb2d4ca1cf24a4f2636bfba40fc814 100644 --- a/src/ui/QGCHilConfiguration.ui +++ b/src/ui/QGCHilConfiguration.ui @@ -7,28 +7,28 @@ 0 0 305 - 172 + 202 Form - + Status - + Random ATT - + Start @@ -42,21 +42,21 @@ - + Host - + Set HOME - + true @@ -77,13 +77,62 @@ - + Random POS + + + + Airframe + + + + + + + true + + + + QRO_X + + + + + Unlimited + + + + + Twinjet + + + + + Early Bird + + + + + Reno Racer + + + + + Slowstick + + + + + Tiny + + + +