Added support for MAVLink XML inclusion, added support for multiple...

Added support for MAVLink XML inclusion, added support for multiple Autopilots, improved protocol generation

mavlinkgen.pro

+
+# }
+# Include general settings for MAVGround
+# necessary as last include to override any non-acceptable settings
+# done by the plugins above
+QT       += svg xml
+
+TEMPLATE = app
+TARGET = mavlinkgen
+
+BASEDIR = .
+BUILDDIR = build/mavlinkgen
+LANGUAGE = C++
+
+CONFIG += release
+CONFIG -= debug
+
+OBJECTS_DIR = $$BUILDDIR/obj
+MOC_DIR = $$BUILDDIR/moc
+UI_HEADERS_DIR = src/ui/generated
+
+macx:DESTDIR = $$BASEDIR/bin/mac
+
+INCLUDEPATH += . \
+    src \
+    src/ui \
+    src/comm \
+    include/ui \
+    src/ui/mavlink \
+    src/standalone/mavlinkgen
+
+# Input
+FORMS += src/ui/XMLCommProtocolWidget.ui
+
+HEADERS += src/standalone/mavlinkgen/MAVLinkGen.h \
+    src/ui/XMLCommProtocolWidget.h \
+    src/comm/MAVLinkXMLParser.h \
+    src/ui/mavlink/DomItem.h \
+    src/ui/mavlink/DomModel.h \
+    src/comm/MAVLinkSyntaxHighlighter.h
+SOURCES += src/standalone/mavlinkgen/main.cc \
+    src/standalone/mavlinkgen/MAVLinkGen.cc \
+    src/ui/XMLCommProtocolWidget.cc \
+    src/ui/mavlink/DomItem.cc \
+    src/ui/mavlink/DomModel.cc \
+    src/comm/MAVLinkXMLParser.cc \
+    src/comm/MAVLinkSyntaxHighlighter.cc
+RESOURCES = mavground.qrc

qgroundcontrol.pro

@@ -99,7 +99,6 @@ HEADERS += src/MG.h \
     src/input/JoystickInput.h \
     src/ui/JoystickWidget.h \
     src/ui/PFD.h \
-    src/ui/GaugePanel.h \
     src/ui/DebugConsole.h \
     src/ui/MapWidget.h \
     src/ui/XMLCommProtocolWidget.h \
@@ -116,7 +115,8 @@ HEADERS += src/MG.h \
     src/ui/linechart/Linecharts.h \
     src/uas/SlugsMAV.h \
     src/uas/PxQuadMAV.h \
-    src/uas/ArduPilotMAV.h
+    src/uas/ArduPilotMAV.h \
+    src/comm/MAVLinkSyntaxHighlighter.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -149,7 +149,6 @@ SOURCES += src/main.cc \
     src/input/JoystickInput.cc \
     src/ui/JoystickWidget.cc \
     src/ui/PFD.cc \
-    src/ui/GaugePanel.cc \
     src/ui/DebugConsole.cc \
     src/ui/MapWidget.cc \
     src/ui/XMLCommProtocolWidget.cc \
@@ -166,5 +165,6 @@ SOURCES += src/main.cc \
     src/ui/linechart/Linecharts.cc \
     src/uas/SlugsMAV.cc \
     src/uas/PxQuadMAV.cc \
-    src/uas/ArduPilotMAV.cc
+    src/uas/ArduPilotMAV.cc \
+    src/comm/MAVLinkSyntaxHighlighter.cc
 RESOURCES = mavground.qrc

src/comm/MAVLinkProtocol.cc

@@ -42,7 +42,9 @@ This file is part of the PIXHAWK project
 #include "UASManager.h"
 #include "UASInterface.h"
 #include "UAS.h"
-#include "SlugsMAV.h" /* FIXME REMOVE */
+#include "SlugsMAV.h"
+#include "PxQuadMAV.h"
+#include "ArduPilotMAV.h"
 #include "configuration.h"
 #include "LinkManager.h"
 #include <mavlink.h>
@@ -115,7 +117,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
             UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
 
             // Check and (if necessary) create UAS object
-            if (uas == NULL)
+            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
             {
                 // ORDER MATTERS HERE!
                 // The UAS object has first to be created and connected,
@@ -130,9 +132,33 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
                     qDebug() << "WARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\n\n RECEIVED MESSAGE FROM THIS SYSTEM WITH ID" << message.msgid << "FROM COMPONENT" << message.compid;
                 }
 
+                // Create a new UAS based on the heartbeat received
+                // Todo dynamically load plugin at run-time for MAV
+                // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION
+
                 // First create new UAS object
+                // Decode heartbeat message
+                mavlink_heartbeat_t heartbeat;
+                mavlink_msg_heartbeat_decode(&message, &heartbeat);
+                switch (heartbeat.autopilot)
+                {
+                case MAV_AUTOPILOT_GENERIC:
                     uas = new UAS(this, message.sysid);
-                //uas = new SlugsMAV(this, message.sysid);
+                    break;
+                case MAV_AUTOPILOT_PIXHAWK:
+                    // Fixme differentiate between quadrotor and coaxial here
+                    uas = new PxQuadMAV(this, message.sysid);
+                    break;
+                case MAV_AUTOPILOT_SLUGS:
+                    uas = new SlugsMAV(this, message.sysid);
+                    break;
+                case MAV_AUTOPILOT_ARDUPILOT:
+                    uas = new ArduPilotMAV(this, message.sysid);
+                    break;
+                default:
+                    uas = new UAS(this, message.sysid);
+                    break;
+                }
 
 
                 // Make UAS aware that this link can be used to communicate with the actual robot
@@ -142,6 +168,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
                 // Now add UAS to "official" list, which makes the whole application aware of it
                 UASManager::instance()->addUAS(uas);
             }
+
+            // Only count message if UAS exists for this message
+            if (uas != NULL)
+            {
                 // Increase receive counter
                 totalReceiveCounter++;
                 currReceiveCounter++;
@@ -206,6 +236,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
                 emit messageReceived(link, message);
             }
         }
+    }
     receiveMutex.unlock();
 }
 
@@ -273,7 +304,7 @@ void MAVLinkProtocol::sendHeartbeat()
     if (m_heartbeatsEnabled)
     {
         mavlink_message_t beat;
-        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU);
+        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU, MAV_AUTOPILOT_GENERIC);
         sendMessage(beat);
     }
 }

src/comm/MAVLinkSyntaxHighlighter.cc

+#include "MAVLinkSyntaxHighlighter.h"
+
+MAVLinkSyntaxHighlighter::MAVLinkSyntaxHighlighter(QObject *parent) :
+    QSyntaxHighlighter(parent)
+{
+}
+
+
+void MAVLinkSyntaxHighlighter::highlightBlock(const QString &text)
+ {
+     QTextCharFormat myClassFormat;
+     myClassFormat.setFontWeight(QFont::Bold);
+     myClassFormat.setForeground(Qt::darkMagenta);
+     QString pattern = "\"[A-Za-z0-9]+\"";
+
+     QRegExp expression(pattern);
+     int index = text.indexOf(expression);
+     while (index >= 0) {
+         int length = expression.matchedLength();
+         setFormat(index, length, myClassFormat);
+         index = text.indexOf(expression, index + length);
+     }
+ }

src/comm/MAVLinkSyntaxHighlighter.h

+#ifndef MAVLINKSYNTAXHIGHLIGHTER_H
+#define MAVLINKSYNTAXHIGHLIGHTER_H
+
+#include <QSyntaxHighlighter>
+
+class MAVLinkSyntaxHighlighter : public QSyntaxHighlighter
+{
+Q_OBJECT
+public:
+    explicit MAVLinkSyntaxHighlighter(QObject *parent = 0);
+
+signals:
+
+public slots:
+    void highlightBlock(const QString &text);
+
+};
+
+#endif // MAVLINKSYNTAXHIGHLIGHTER_H

src/comm/MAVLinkXMLParser.cc

@@ -2,6 +2,7 @@
 #include <QDir>
 #include <QPair>
 #include <QList>
+#include <QMap>
 #include <QDateTime>
 #include "MAVLinkXMLParser.h"
 
@@ -9,7 +10,8 @@
 
 MAVLinkXMLParser::MAVLinkXMLParser(QDomDocument* document, QString outputDirectory, QObject* parent) : QObject(parent),
 doc(document),
-outputDirName(outputDirectory)
+outputDirName(outputDirectory),
+fileName("")
 {
 }
 
@@ -22,6 +24,7 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO
         const QString instanceText(QString::fromUtf8(file.readAll()));
         doc->setContent(instanceText);
     }
+    fileName = document;
     outputDirName = outputDirectory;
 }
 
@@ -40,9 +43,13 @@ bool MAVLinkXMLParser::generate()
     // print out the element names of all elements that are direct children
     // of the outermost element.
     QDomElement docElem = doc->documentElement();
+    QDomNode n = docElem;//.firstChild();
+    QDomNode p = docElem;
+
+    // Sanity check variables
+    QList<int>* usedMessageIDs = new QList<int>();
+    QMap<QString, QString>* usedMessageNames = new QMap<QString, QString>();
 
-    QList<QString> parseErrors();
-    QDomNode n = docElem.firstChild();
 
     /*
     // Seek for element "messages" until end of document
@@ -61,28 +68,125 @@ bool MAVLinkXMLParser::generate()
     }
 
     qDebug() << "WORKING ON" << n.toElement().tagName();
-*/
+    */
+
     QList< QPair<QString, QString> > cFiles;
     QString lcmStructDefs = "";
 
-    while(!n.isNull()) {
+    // Run through root children
+    while(!n.isNull())
+    {
         // Each child is a message
         QDomElement e = n.toElement(); // try to convert the node to an element.
-        if(!e.isNull()) {
+        if(!e.isNull())
+        {
+            if (e.tagName() == "mavlink")
+            {
+                p = n;
+                n = n.firstChild();
+                while (!n.isNull())
+                {
+                    e = n.toElement();
+                    if (!e.isNull())
+                    {
+                        // Handle all include tags
+                        if (e.tagName() == "include")
+                        {
+                            QString fileName = e.text();
+                            // Load file
+                            QDomDocument includeDoc = QDomDocument();
+
+                            // Prepend file path if it is a relative path and
+                            // make it relative to opened file
+                            QFileInfo fInfo(fileName);
+                            if (fInfo.isRelative())
+                            {
+                                QFileInfo rInfo(this->fileName);
+                                fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName;
+                            }
+
+                            QFile file(fileName);
+                            if (file.open(QIODevice::ReadOnly | QIODevice::Text))
+                            {
+                                const QString instanceText(QString::fromUtf8(file.readAll()));
+                                includeDoc.setContent(instanceText);
+
+                                // Get all messages
+                                QDomNode in = includeDoc.documentElement().firstChild();
+                                //while (!in.isNull())
+                                //{
+                                    QDomElement ie = in.toElement();
+                                    if (!ie.isNull())
+                                    {
+                                        if (ie.tagName() == "messages" || ie.tagName() == "include")
+                                        {
+                                            QDomNode ref = n.parentNode().insertAfter(in, n);
+                                            if (ref.isNull())
+                                            {
+                                                emit parseState(QString("<font color=\"red\">ERROR: Inclusion failed: XML syntax error in file %1. Wrong/misspelled XML?\nAbort.</font>").arg(fileName));
+                                                return false;
+                                            }
+                                        }
+                                    }
+                                    //in = in.nextSibling();
+                                //}
+
+                                emit parseState(QString("<font color=\"green\">Included messages from file: %1</font>").arg(fileName));
+                            }
+                            else
+                            {
+                                // Include file could not be opened
+                                emit parseState(QString("<font color=\"red\">ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.</font>").arg(fileName));
+                                return false;
+                            }
+
+                        }
                         // Handle all message tags
-            if (e.tagName() == "message")
+                        else if (e.tagName() == "messages")
                         {
+                            p = n;
+                            n = n.firstChild();
+                            while (!n.isNull())
+                            {
+                                e = n.toElement();
+                                if(!e.isNull())
+                                {
+                                    //if (e.isNull()) continue;
                                     // Get message name
                                     QString messageName = e.attribute("name", "").toLower();
                                     if (messageName.size() == 0)
                                     {
-                    //parseErrors.append(tr("Missing name attribute at line "));
+                                        emit parseState(tr("<font color=\"red\">ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), e.tagName()));
+                                        return false;
                                     }
                                     else
                                     {
                                         // Get message id
                                         bool ok;
                                         int messageId = e.attribute("id", "-1").toInt(&ok, 10);
+                                        emit parseState(tr("Compiling message <strong>%1 \t(#%3)</strong> \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId)));
+
+                                        // Sanity check: Accept only message IDs not used previously
+                                        if (usedMessageIDs->contains(messageId))
+                                        {
+                                            emit parseState(tr("<font color=\"red\">ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.</font>").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName));
+                                            return false;
+                                        }
+                                        else
+                                        {
+                                            usedMessageIDs->append(messageId);
+                                        }
+
+                                        // Sanity check: Accept only message names not used previously
+                                        if (usedMessageNames->contains(messageName))
+                                        {
+                                            emit parseState(tr("<font color=\"red\">ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.</font>").arg(messageName, QString::number(e.lineNumber()), fileName));
+                                            return false;
+                                        }
+                                        else
+                                        {
+                                            usedMessageNames->insert(messageName, QString::number(e.lineNumber()));
+                                        }
 
                                         QString channelType = "mavlink_channel_t";
                                         QString messageType = "mavlink_message_t";
@@ -236,11 +340,22 @@ bool MAVLinkXMLParser::generate()
                                         compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters);
                                         QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode;
                                         cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile));
-                }
-            }
-        }
+                                    } // Check if tag = message
+                                } // Check if e = NULL
                                 n = n.nextSibling();
-    }
+                            } // While through <message>
+                            n = p;
+
+                        } // Check if tag = messages
+                    } // Check if e = NULL
+                    n = n.nextSibling();
+                } // While through include and messages
+                n = p;
+
+            } // Check if tag = mavlink
+        } // Check if e = NULL
+        n = n.nextSibling();
+    } // While through root children
 
     // XML parsed and converted to C code. Now generating the files
     QDateTime now = QDateTime::currentDateTime().toUTC();

src/comm/MAVLinkXMLParser.h

@@ -17,9 +17,14 @@ public slots:
     /** @brief Parse XML and generate C files */
     bool generate();
 
+signals:
+    /** @brief Status message on the parsing */
+    void parseState(QString message);
+
 protected:
     QDomDocument* doc;
     QString outputDirName;
+    QString fileName;
 };
 
 #endif // MAVLINKXMLPARSER_H

src/standalone/mavlinkgen/MAVLinkGen.cc

+/*=====================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+    PIXHAWK is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    PIXHAWK is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Implementation of class MAVLinkGen
+ *
+ *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *
+ */
+
+
+#include <QFile>
+#include <QFlags>
+#include <QThread>
+#include <QSplashScreen>
+#include <QPixmap>
+#include <QDesktopWidget>
+#include <QPainter>
+#include <QStyleFactory>
+#include <QAction>
+#include <QSettings>
+#include <QFontDatabase>
+#include <QMainWindow>
+
+#include "MAVLinkGen.h"
+#include "XMLCommProtocolWidget.h"
+
+
+/**
+ * @brief Constructor for the main application.
+ *
+ * This constructor initializes and starts the whole application. It takes standard
+ * command-line parameters
+ *
+ * @param argc The number of command-line parameters
+ * @param argv The string array of parameters
+ **/
+
+MAVLinkGen::MAVLinkGen(int &argc, char* argv[]) : QApplication(argc, argv)
+{
+    this->setApplicationName("MAVLink Generator");
+    this->setApplicationVersion("v. 0.1.0 (Beta)");
+    this->setOrganizationName(QLatin1String("OpenMAV Association"));
+    this->setOrganizationDomain("http://qgroundcontrol.org");
+	
+    QSettings::setDefaultFormat(QSettings::IniFormat);
+    // Exit main application when last window is closed
+    connect(this, SIGNAL(lastWindowClosed()), this, SLOT(quit()));
+	
+    // Set application font
+    QFontDatabase fontDatabase = QFontDatabase();
+    const QString fontFileName = ":/general/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app
+    const QString fontFamilyName = "Bitstream Vera Sans";
+    if(!QFile::exists(fontFileName)) printf("ERROR! font file: %s DOES NOT EXIST!\n", fontFileName.toStdString().c_str());
+    fontDatabase.addApplicationFont(fontFileName);
+    setFont(fontDatabase.font(fontFamilyName, "Roman", 12));
+
+    // Create main window
+    QMainWindow* window = new QMainWindow();
+    window->setCentralWidget(new XMLCommProtocolWidget(window));
+    window->setWindowTitle(applicationName() + " " + applicationVersion());
+    window->show();
+}
+
+/**
+ * @brief Destructor for the groundstation. It destroys all loaded instances.
+ *
+ **/
+MAVLinkGen::~MAVLinkGen()
+{
+}
+

src/standalone/mavlinkgen/MAVLinkGen.h

+/*=====================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+    PIXHAWK is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    PIXHAWK is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of class MAVLinkGen
+ *
+ *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *
+ */
+
+
+#ifndef MAVLINKGEN_H
+#define MAVLINKGEN_H
+
+#include <QApplication>
+
+/**
+ * @brief The main application and management class.
+ *
+ * This class is started by the main method and provides
+ * the central management unit of the groundstation application.
+ *
+ **/
+class MAVLinkGen : public QApplication
+{
+	Q_OBJECT
+
+public:
+        MAVLinkGen(int &argc, char* argv[]);
+        ~MAVLinkGen();
+
+protected:
+
+private:
+};
+
+#endif /* MAVLINKGEN_H */

src/standalone/mavlinkgen/main.cc

+/*=====================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+    PIXHAWK is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    PIXHAWK is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Main executable
+ *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *
+ */
+
+#include <QtGui/QApplication>
+#include "MAVLinkGen.h"
+
+/**
+ * @brief Starts the application
+ *
+ * @param argc Number of commandline arguments
+ * @param argv Commandline arguments
+ * @return exit code, 0 for normal exit and !=0 for error cases
+ */
+int main(int argc, char *argv[])
+{
+
+    MAVLinkGen gen(argc, argv);
+    return gen.exec();
+}

src/uas/ArduPilotMAV.cc

@@ -5,3 +5,30 @@ ArduPilotMAV::ArduPilotMAV(MAVLinkProtocol* mavlink, int id) :
         // place other initializers here
 {
 }
+
+/**
+ * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
+ * mavlink packet is received.
+ *
+ * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
+ *             messages can be sent back to the system via this link
+ * @param message MAVLink message, as received from the MAVLink protocol stack
+ */
+void ArduPilotMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
+{
+    // Let UAS handle the default message set
+    UAS::receiveMessage(link, message);
+
+    // Handle your special messages
+    switch (message.msgid)
+    {
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        {
+            qDebug() << "ARDUPILOT RECEIVED HEARTBEAT";
+            break;
+        }
+    default:
+        qDebug() << "\nARDUPILOT RECEIVED MESSAGE WITH ID" << message.msgid;
+        break;
+    }
+}

src/uas/ArduPilotMAV.h

@@ -8,6 +8,9 @@ class ArduPilotMAV : public UAS
     Q_OBJECT
 public:
     ArduPilotMAV(MAVLinkProtocol* mavlink, int id = 0);
+public slots:
+    /** @brief Receive a MAVLink message from this MAV */
+    void receiveMessage(LinkInterface* link, mavlink_message_t message);
 };
 
 #endif // ARDUPILOTMAV_H

src/uas/PxQuadMAV.cc

@@ -4,3 +4,29 @@ PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) :
         UAS(mavlink, id)
 {
 }
+
+/**
+ * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
+ * mavlink packet is received.
+ *
+ * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
+ *             messages can be sent back to the system via this link
+ * @param message MAVLink message, as received from the MAVLink protocol stack
+ */
+void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
+{
+    // Let UAS handle the default message set
+    UAS::receiveMessage(link, message);
+
+    // Handle your special messages
+    switch (message.msgid)
+    {
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        {
+            break;
+        }
+    default:
+        // Do nothing
+        break;
+    }
+}

src/uas/PxQuadMAV.h

@@ -8,6 +8,9 @@ class PxQuadMAV : public UAS
     Q_OBJECT
 public:
     PxQuadMAV(MAVLinkProtocol* mavlink, int id);
+public slots:
+    /** @brief Receive a MAVLink message from this MAV */
+    void receiveMessage(LinkInterface* link, mavlink_message_t message);
 };
 
 #endif // PXQUADMAV_H

src/uas/SlugsMAV.cc

@@ -8,7 +8,14 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
 {
 }
 
-
+/**
+ * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
+ * mavlink packet is received.
+ *
+ * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
+ *             messages can be sent back to the system via this link
+ * @param message MAVLink message, as received from the MAVLink protocol stack
+ */
 void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 {
     // Let UAS handle the default message set
@@ -19,7 +26,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
     {
     case MAVLINK_MSG_ID_HEARTBEAT:
         {
-            qDebug() << "RECEIVED HEARTBEAT";
+            qDebug() << "SLUGS RECEIVED HEARTBEAT";
             break;
         }
     default:

src/uas/SlugsMAV.h

@@ -10,6 +10,7 @@ public:
     SlugsMAV(MAVLinkProtocol* mavlink, int id = 0);
 
 public slots:
+    /** @brief Receive a MAVLink message from this MAV */
     void receiveMessage(LinkInterface* link, mavlink_message_t message);
 };
 

src/uas/UAS.cc

@@ -1053,7 +1053,20 @@ int UAS::calculateTimeRemaining()
  */
 double UAS::getChargeLevel()
 {
-    return 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
+    float chargeLevel;
+    if (lpVoltage < emptyVoltage)
+    {
+        chargeLevel = 0.0f;
+    }
+    else if (lpVoltage > fullVoltage)
+    {
+        chargeLevel = 100.0f;
+    }
+    else
+    {
+        chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
+    }
+    return chargeLevel;
 }
 
 void UAS::startLowBattAlarm()

src/ui/DebugConsole.cc

@@ -163,7 +163,7 @@ void DebugConsole::setAutoHold(bool hold)
 
 void DebugConsole::receiveTextMessage(int id, int severity, QString text)
 {
-    m_ui->receiveText->appendHtml(QString("<b color=\"red\">(MAV" + QString::number(id) + QString(":") + QString::number(severity) + QString(") ") + text + QString("</b>")));
+    m_ui->receiveText->appendHtml(QString("<font color=\"yellow\">(MAV" + QString::number(id) + QString(":") + QString::number(severity) + QString(") ") + text + QString("</font>")));
 }
 
 void DebugConsole::updateTrafficMeasurements()

src/ui/HDDisplay.cc

@@ -516,8 +516,10 @@ void HDDisplay::paintText(QString text, QColor color, float fontSize, float refX
     float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f);
     float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f);
 
-    //QFont font("Bitstream Vera Sans");
-    font.setPixelSize((int)(fontSize*scalingFactor*1.26f));
+    QFont font("Bitstream Vera Sans");
+    // Enforce minimum font size of 5 pixels
+    int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f));
+    font.setPixelSize(fSize);
 
     QFontMetrics metrics = QFontMetrics(font);
     int border = qMax(4, metrics.leading());

src/ui/HUD.cc

@@ -444,8 +444,10 @@ void HUD::paintText(QString text, QColor color, float fontSize, float refX, floa
     float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f);
     float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f);
 
-    //QFont font("Bitstream Vera Sans");
-    font.setPixelSize((int)(fontSize*scalingFactor*1.26f));
+    QFont font("Bitstream Vera Sans");
+    // Enforce minimum font size of 5 pixels
+    int fSize = qMax(1, (int)(fontSize*scalingFactor*1.26f));
+    font.setPixelSize(fSize);
 
     QFontMetrics metrics = QFontMetrics(font);
     int border = qMax(4, metrics.leading());

src/ui/XMLCommProtocolWidget.cc

@@ -6,6 +6,7 @@
 #include "XMLCommProtocolWidget.h"
 #include "ui_XMLCommProtocolWidget.h"
 #include "MAVLinkXMLParser.h"
+#include "MAVLinkSyntaxHighlighter.h"
 
 #include <QDebug>
 #include <iostream>
@@ -16,6 +17,9 @@ XMLCommProtocolWidget::XMLCommProtocolWidget(QWidget *parent) :
 {
     m_ui->setupUi(this);
 
+    // Now set syntax highlighter
+    highlighter = new MAVLinkSyntaxHighlighter(m_ui->xmlTextView->document());
+
     connect(m_ui->selectFileButton, SIGNAL(clicked()), this, SLOT(selectXMLFile()));
     connect(m_ui->selectOutputButton, SIGNAL(clicked()), this, SLOT(selectOutputDirectory()));
     connect(m_ui->generateButton, SIGNAL(clicked()), this, SLOT(generate()));
@@ -66,11 +70,11 @@ void XMLCommProtocolWidget::setXML(const QString& xml)
 
     if (doc.setContent(xml))
     {
-        m_ui->validXMLLabel->setText(tr("Valid XML file"));
+        m_ui->validXMLLabel->setText(tr("<font color=\"green\">Valid XML file</font>"));
     }
     else
     {
-        m_ui->validXMLLabel->setText(tr("File is NOT valid XML, please fix in editor"));
+        m_ui->validXMLLabel->setText(tr("<font color=\"red\">File is NOT valid XML, please fix in editor</font>"));
     }
 
     if (model != NULL)
@@ -112,8 +116,11 @@ void XMLCommProtocolWidget::generate()
 {
     // First save file
     save();
-    MAVLinkXMLParser parser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed());
-    bool result = parser.generate();
+    // Clean log
+    m_ui->compileLog->clear();
+    MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed());
+    connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString)));
+    bool result = parser->generate();
     if (result)
     {
         QMessageBox msgBox;
@@ -122,8 +129,9 @@ void XMLCommProtocolWidget::generate()
     }
     else
     {
-        QMessageBox::critical(this, tr("Could not write files"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok);
+        QMessageBox::critical(this, tr("C code generation failed, please see the compile log for further information"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok);
     }
+    delete parser;
 }
 
 void XMLCommProtocolWidget::save()

src/ui/XMLCommProtocolWidget.h

@@ -3,6 +3,7 @@
 
 #include <QtGui/QWidget>
 #include "DomModel.h"
+#include "MAVLinkSyntaxHighlighter.h"
 
 namespace Ui {
     class XMLCommProtocolWidget;
@@ -31,6 +32,7 @@ protected slots:
     void save();
 
 protected:
+    MAVLinkSyntaxHighlighter* highlighter;
     DomModel* model;
     void changeEvent(QEvent *e);
 

src/ui/XMLCommProtocolWidget.ui

@@ -6,22 +6,43 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>402</width>
+    <width>846</width>
     <height>480</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <item row="0" column="0" colspan="2">
+  <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,10,0,0,0" columnstretch="1,1,1,10">
+   <property name="topMargin">
+    <number>6</number>
+   </property>
+   <property name="rightMargin">
+    <number>6</number>
+   </property>
+   <property name="bottomMargin">
+    <number>6</number>
+   </property>
+   <property name="spacing">
+    <number>12</number>
+   </property>
+   <item row="0" column="0">
     <widget class="QLabel" name="fileNameLabel">
+     <property name="maximumSize">
+      <size>
+       <width>300</width>
+       <height>16777215</height>
+      </size>
+     </property>
      <property name="text">
       <string>Select input file</string>
      </property>
      <property name="scaledContents">
       <bool>true</bool>
      </property>
+     <property name="alignment">
+      <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+     </property>
     </widget>
    </item>
    <item row="0" column="2">
@@ -35,8 +56,21 @@
      </property>
     </widget>
    </item>
+   <item row="0" column="3" rowspan="6">
+    <widget class="QTextEdit" name="xmlTextView">
+     <property name="readOnly">
+      <bool>false</bool>
+     </property>
+    </widget>
+   </item>
    <item row="1" column="0" colspan="2">
     <widget class="QLabel" name="outputDirNameLabel">
+     <property name="maximumSize">
+      <size>
+       <width>400</width>
+       <height>16777215</height>
+      </size>
+     </property>
      <property name="text">
       <string>Select output directory</string>
      </property>
@@ -59,28 +93,31 @@
    <item row="2" column="0" colspan="3">
     <widget class="QTreeView" name="xmlTreeView"/>
    </item>
-   <item row="3" column="0" colspan="3">
-    <widget class="QTextEdit" name="xmlTextView">
-     <property name="readOnly">
-      <bool>false</bool>
+   <item row="3" column="0" colspan="2">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Compile Output</string>
      </property>
     </widget>
    </item>
-   <item row="4" column="0">
+   <item row="4" column="0" colspan="3">
+    <widget class="QPlainTextEdit" name="compileLog"/>
+   </item>
+   <item row="5" column="0">
     <widget class="QLabel" name="validXMLLabel">
      <property name="text">
       <string>No file loaded</string>
      </property>
     </widget>
    </item>
-   <item row="4" column="1">
+   <item row="5" column="1">
     <widget class="QPushButton" name="saveButton">
      <property name="text">
       <string>Save file</string>
      </property>
     </widget>
    </item>
-   <item row="4" column="2">
+   <item row="5" column="2">
     <widget class="QPushButton" name="generateButton">
      <property name="text">
       <string>Save and generate</string>