Added MAVLinkLight as an example how to add new protocols, it is currently not...

Added MAVLinkLight as an example how to add new protocols, it is currently not an actual implemented protocol

qgroundcontrol.pro

@@ -149,7 +149,8 @@ HEADERS += src/MG.h \
     src/ui/HSIDisplay.h \
     src/QGC.h \
     src/ui/QGCFirmwareUpdate.h \
-    src/ui/QGCPxImuFirmwareUpdate.h
+    src/ui/QGCPxImuFirmwareUpdate.h \
+    src/comm/MAVLinkLightProtocol.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -207,5 +208,6 @@ SOURCES += src/main.cc \
     src/ui/HSIDisplay.cc \
     src/QGC.cc \
     src/ui/QGCFirmwareUpdate.cc \
-    src/ui/QGCPxImuFirmwareUpdate.cc
+    src/ui/QGCPxImuFirmwareUpdate.cc \
+    src/comm/MAVLinkLightProtocol.cc
 RESOURCES = mavground.qrc

src/comm/MAVLinkLightProtocol.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL 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.
+
+    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Example implementation of a different protocol than the default MAVLink.
+ *   @author Lorenz Meier <mail@qgroundcontrol.org>
+ */
+
+#include "MAVLinkLightProtocol.h"
+#include "UASManager.h"
+#include "ArduPilotMAV.h"
+#include "LinkManager.h"
+
+MAVLinkLightProtocol::MAVLinkLightProtocol() :
+    MAVLinkProtocol()
+{
+}
+
+
+/**
+ * @param message message to send
+ */
+void MAVLinkLightProtocol::sendMessage(mavlink_light_message_t message)
+{
+    // Get all links connected to this unit
+    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
+
+    // Emit message on all links that are currently connected
+    QList<LinkInterface*>::iterator i;
+    for (i = links.begin(); i != links.end(); ++i)
+    {
+        sendMessage(*i, message);
+    }
+}
+
+/**
+ * @param link the link to send the message over
+ * @param message message to send
+ */
+void MAVLinkLightProtocol::sendMessage(LinkInterface* link, mavlink_light_message_t message)
+{
+    // Create buffer
+    uint8_t buffer[100]; // MAXIMUM PACKET LENGTH, INCLUDING STX BYTES
+    // Write message into buffer, prepending start sign
+    //int len = mavlink_msg_to_send_buffer(buffer, &message);
+
+
+    // FIXME TO SEND BUFFER FUNCTION MISSING
+    Q_UNUSED(message);
+    int len = 0;
+
+    // If link is connected
+    if (link->isConnected())
+    {
+        // Send the portion of the buffer now occupied by the message
+        link->writeBytes((const char*)buffer, len);
+    }
+}
+
+/**
+ * The bytes are copied by calling the LinkInterface::readBytes() method.
+ * This method parses all incoming bytes and constructs a MAVLink packet.
+ * It can handle multiple links in parallel, as each link has it's own buffer/
+ * parsing state machine.
+ * @param link The interface to read from
+ * @see LinkInterface
+ **/
+void MAVLinkLightProtocol::receiveBytes(LinkInterface* link)
+{
+    receiveMutex.lock();
+    // Prepare buffer
+    static const int maxlen = 4096 * 100;
+    static char buffer[maxlen];
+    qint64 bytesToRead = link->bytesAvailable();
+
+    // Get all data at once, let link read the bytes in the buffer array
+    link->readBytes(buffer, maxlen);
+
+    // Run through all bytes
+    for (int position = 0; position < bytesToRead; position++)
+    {
+        mavlink_light_message_t msg;
+        // FIXME PARSE
+        if (1 == 0/* parsing returned a message */)
+        {
+
+            int sysid = 0; // system id from message, or always null if only one MAV is supported
+            UASInterface* uas = UASManager::instance()->getUASForId(sysid);
+
+            // Check and (if necessary) create UAS object
+            if (uas == NULL)
+            {
+                ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists
+                // Connect this robot to the UAS object
+                // it is IMPORTANT here to use the right object type,
+                // else the slot of the parent object is called (and thus the special
+                // packets never reach their goal)
+                connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+                uas = mav;
+                // Make UAS aware that this link can be used to communicate with the actual robot
+                uas->addLink(link);
+                // Now add UAS to "official" list, which makes the whole application aware of it
+                UASManager::instance()->addUAS(uas);
+            }
+
+            // The packet is emitted as a whole, as it is only 255 - 261 bytes short
+            // kind of inefficient, but no issue for a groundstation pc.
+            // It buys as reentrancy for the whole code over all threads
+            emit messageReceived(link, msg);
+        }
+    }
+    receiveMutex.unlock();
+}
+
+/**
+ * @return The name of this protocol
+ **/
+QString MAVLinkLightProtocol::getName()
+{
+    return QString(tr("MAVLinkLight protocol"));
+}

src/comm/MAVLinkLightProtocol.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL 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.
+
+    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Example implementation of a different protocol than the default MAVLink.
+ *   @author Lorenz Meier <mail@qgroundcontrol.org>
+ */
+
+#ifndef MAVLINKLIGHTPROTOCOL_H
+#define MAVLINKLIGHTPROTOCOL_H
+
+#include <inttypes.h>
+#include <MAVLinkProtocol.h>
+
+#define MAVLINK_LIGHT_MAX_PAYLOAD_LEN 50
+
+// Not part of message struct, but sent on link
+// uint8_t stx1;
+// uint8_t stx2;
+typedef struct {
+    uint8_t msgid;   ///< ID of message in payload
+    uint8_t payload[MAVLINK_LIGHT_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU
+    uint8_t ck_a;    ///< Checksum high byte
+    uint8_t ck_b;    ///< Checksum low byte
+} mavlink_light_message_t;
+
+class MAVLinkLightProtocol : public MAVLinkProtocol
+{
+Q_OBJECT
+public:
+    explicit MAVLinkLightProtocol();
+    QString getName();
+
+signals:
+    /** @brief Message received and directly copied via signal */
+    void messageReceived(LinkInterface* link, mavlink_light_message_t message);
+
+public slots:
+    void sendMessage(mavlink_light_message_t message);
+    void sendMessage(LinkInterface* link, mavlink_light_message_t message);
+    void receiveBytes(LinkInterface* link);
+
+};
+
+#endif // MAVLINKLIGHTPROTOCOL_H

src/comm/MAVLinkProtocol.cc

@@ -323,7 +323,7 @@ void MAVLinkProtocol::sendMessage(mavlink_message_t message)
     for (i = links.begin(); i != links.end(); ++i)
     {
         sendMessage(*i, message);
-        qDebug() << __FILE__ << __LINE__ << "SENT HEARTBEAT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
+        //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
     }
 }
 

src/comm/MAVLinkSimulationLink.cc

@@ -186,10 +186,13 @@ void MAVLinkSimulationLink::mainloop()
     static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second
 
     mavlink_attitude_t attitude;
+    memset(&attitude, 0, sizeof(mavlink_attitude_t));
     #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
       mavlink_raw_aux_t rawAuxValues;
+      memset(&rawAuxValues, 0, sizeof(mavlink_raw_aux_t));
     #endif
     mavlink_raw_imu_t rawImuValues;
+    memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t));
 
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     int bufferlength;

src/uas/ArduPilotMAV.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL 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.
+
+    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Implementation of class MainWindow
+ *   @author Your Name here
+ */
+
 #include "ArduPilotMAV.h"
 
 ArduPilotMAV::ArduPilotMAV(MAVLinkProtocol* mavlink, int id) :

src/ui/MainWindow.cc

@@ -2,9 +2,7 @@
 
 QGroundControl Open Source Ground Control Station
 
-(c) 2009, 2010 QGROUNDCONTROL/PIXHAWK PROJECT
-<http://www.qgroundcontrol.org>
-<http://pixhawk.ethz.ch>
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
@@ -142,13 +140,15 @@ void MainWindow::buildWidgets()
   joystick    = new JoystickInput();
 }
 
-void MainWindow::connectWidgets(){
+void MainWindow::connectWidgets()
+{
   connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), linechart, SLOT(addSystem(UASInterface*)));
   connect(UASManager::instance(), SIGNAL(activeUASSet(int)), linechart, SLOT(selectSystem(int)));
   connect(mavlink, SIGNAL(receiveLossChanged(int, float)), info, SLOT(updateSendLoss(int, float)));
 }
 
-void MainWindow::arrangeCenterStack(){
+void MainWindow::arrangeCenterStack()
+{
 
   centerStack = new QStackedWidget(this);
 
@@ -160,7 +160,8 @@ void MainWindow::arrangeCenterStack(){
   setCentralWidget(centerStack);
 }
 
-void MainWindow::configureWindowName(){
+void MainWindow::configureWindowName()
+{
   QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses();
   QString windowname = qApp->applicationName() + " " + qApp->applicationVersion();
   bool prevAddr = false;
@@ -245,11 +246,14 @@ void MainWindow::reloadStylesheet()
     {
         styleSheet = new QFile(":/images/style-mission.css");
     }
-    if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) {
+    if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text))
+    {
         QString style = QString(styleSheet->readAll());
         style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/");
         qApp->setStyleSheet(style);
-    } else {
+    }
+    else
+    {
         qDebug() << "Style not set:" << styleSheet->fileName() << "opened: " << styleSheet->isOpen();
     }
     delete styleSheet;

src/ui/QGCParamWidget.cc

@@ -165,7 +165,7 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
 {
     Q_UNUSED(uas);
     // Reference to item in tree
-    QTreeWidgetItem* parameterItem;
+    QTreeWidgetItem* parameterItem = NULL;
 
     // Get component
     if (!components->contains(component))