From 2c92489ba26aa7075e2b7d88f4c1231ee7e98c8e Mon Sep 17 00:00:00 2001 From: lm Date: Mon, 3 Jan 2011 17:10:36 +0100 Subject: [PATCH] Added VICON position message --- src/Core.cc | 9 ++++----- src/comm/MAVLinkProtocol.cc | 15 ++++++++++++++- src/comm/MAVLinkProtocol.h | 8 ++++++++ src/comm/SerialLink.cc | 2 ++ src/uas/PxQuadMAV.cc | 15 +++++++++++++++ src/ui/MAVLinkSettingsWidget.cc | 10 +++++++--- src/ui/MAVLinkSettingsWidget.ui | 7 +++++++ 7 files changed, 57 insertions(+), 9 deletions(-) diff --git a/src/Core.cc b/src/Core.cc index cf0f0c3d1..3e5602704 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -91,8 +91,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // "Warning: Do not use this function in conjunction with Qt Style Sheets." // setFont(fontDatabase.font(fontFamilyName, "Roman", 12)); - mainWindow = MainWindow::instance(); - // Start the comm link manager splashScreen->showMessage(tr("Starting Communication Links"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141)); startLinkManager(); @@ -108,9 +106,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) splashScreen->showMessage(tr("Starting User Interface"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141)); // Start UI - // Remove splash screen - splashScreen->finish(mainWindow); - // Connect links // to make sure that all components are initialized when the // first messages arrive @@ -130,6 +125,10 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) simulationLink->disconnect(); //mainWindow->addLink(simulationLink); + mainWindow = MainWindow::instance(); + // Remove splash screen + splashScreen->finish(mainWindow); + // Check if link could be connected if (!udpLink->connect()) { diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index dad9d91d1..64fd62561 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -60,6 +60,7 @@ MAVLinkProtocol::MAVLinkProtocol() : m_heartbeatsEnabled(false), m_loggingEnabled(false), m_logfile(NULL), + m_enable_version_check(true), versionMismatchIgnore(false) { start(QThread::LowPriority); @@ -77,6 +78,8 @@ MAVLinkProtocol::MAVLinkProtocol() : lastIndex[i][j] = -1; } } + + emit versionCheckChanged(m_enable_version_check); } MAVLinkProtocol::~MAVLinkProtocol() @@ -161,7 +164,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_msg_heartbeat_decode(&message, &heartbeat); // Check if the UAS has a different protocol version - if (heartbeat.mavlink_version != MAVLINK_VERSION) + if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION)) { // Bring up dialog to inform user if (!versionMismatchIgnore) @@ -400,6 +403,11 @@ void MAVLinkProtocol::enableLogging(bool enabled) m_loggingEnabled = enabled; } +void MAVLinkProtocol::enableVersionCheck(bool enabled) +{ + m_enable_version_check = enabled; +} + bool MAVLinkProtocol::heartbeatsEnabled(void) { return m_heartbeatsEnabled; @@ -410,6 +418,11 @@ bool MAVLinkProtocol::loggingEnabled(void) return m_loggingEnabled; } +bool MAVLinkProtocol::versionCheckEnabled(void) +{ + return m_enable_version_check; +} + /** * The default rate is 1 Hertz. * diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 44ba9489d..76171e262 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -68,6 +68,8 @@ public: bool heartbeatsEnabled(void); /** @brief Get logging state */ bool loggingEnabled(void); + /** @brief Get protocol version check state */ + bool versionCheckEnabled(void); /** @brief Get the name of the packet log file */ static QString getLogfileName(); @@ -87,6 +89,9 @@ public slots: /** @brief Enable/disable binary packet logging */ void enableLogging(bool enabled); + /** @brief Enable / disable version check */ + void enableVersionCheck(bool enabled); + /** @brief Send an extra heartbeat to all connected units */ void sendHeartbeat(); @@ -96,6 +101,7 @@ protected: bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission bool m_loggingEnabled; ///< Enable/disable packet logging QFile* m_logfile; ///< Logfile + bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC QMutex receiveMutex; ///< Mutex to protect receiveBytes function int lastIndex[256][256]; int totalReceiveCounter; @@ -111,6 +117,8 @@ signals: void heartbeatChanged(bool heartbeats); /** @brief Emitted if logging is started / stopped */ void loggingChanged(bool enabled); + /** @brief Emitted if version check is enabled / disabled */ + void versionCheckChanged(bool enabled); /** @brief Emitted if a message from the protocol should reach the user */ void protocolStatusMessage(const QString& title, const QString& message); }; diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index afb4da35e..ee320e736 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -85,6 +85,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P #endif + loadSettings(); + // Link is setup, register it with link manager LinkManager::instance()->add(this); } diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 2b9739ee4..bde4e9e52 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -118,6 +118,21 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "vis. z", pos.z, time); } break; + case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: + { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + //emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "vicon roll", pos.roll, time); + emit valueChanged(uasId, "vicon pitch", pos.pitch, time); + emit valueChanged(uasId, "vicon yaw", pos.yaw, time); + emit valueChanged(uasId, "vicon x", pos.x, time); + emit valueChanged(uasId, "vicon y", pos.y, time); + emit valueChanged(uasId, "vicon z", pos.z, time); + emit localPositionChanged(this, pos.x, pos.y, pos.z, time); + } + break; case MAVLINK_MSG_ID_AUX_STATUS: { mavlink_aux_status_t status; diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index d41983891..054ff10a5 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -37,15 +37,19 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget { m_ui->setupUi(this); + // Initialize state + m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); + m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); + m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); + // Connect actions connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool))); connect(m_ui->heartbeatCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableHeartbeats(bool))); connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->loggingCheckBox, SLOT(setChecked(bool))); connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool))); + connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionCheckBox, SLOT(setChecked(bool))); + connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableVersionCheck(bool))); - // Initialize state - m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); - m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); } MAVLinkSettingsWidget::~MAVLinkSettingsWidget() diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index c65627d3b..c24aaddf2 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -31,6 +31,13 @@ + + + + Only accept MAVs with same protocol version + + + -- 2.22.0