Added VICON position message

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())
     {

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.
  *

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);
 };

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);
 }

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;

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()

src/ui/MAVLinkSettingsWidget.ui

@@ -31,6 +31,13 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QCheckBox" name="versionCheckBox">
+     <property name="text">
+      <string>Only accept MAVs with same protocol version</string>
+     </property>
+    </widget>
+   </item>
    <item>
     <spacer name="verticalSpacer">
      <property name="orientation">