Merge branch 'master' of pixhawk.ethz.ch:qgroundcontrol

src/comm/MAVLinkProtocol.cc

@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
 
 #include <QDebug>
 #include <QTime>
+#include <QApplication>
 
 #include "MG.h"
 #include "MAVLinkProtocol.h"
@@ -56,7 +57,9 @@ This file is part of the PIXHAWK project
 MAVLinkProtocol::MAVLinkProtocol() :
         heartbeatTimer(new QTimer(this)),
         heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
-        m_heartbeatsEnabled(false)
+        m_heartbeatsEnabled(false),
+        m_loggingEnabled(false),
+        m_logfile(NULL)
 {
     start(QThread::LowPriority);
     // Start heartbeat timer, emitting a heartbeat at the configured rate
@@ -77,12 +80,23 @@ MAVLinkProtocol::MAVLinkProtocol() :
 
 MAVLinkProtocol::~MAVLinkProtocol()
 {
+    if (m_logfile)
+    {
+        m_logfile->close();
+        delete m_logfile;
+    }
 }
 
 
 
 void MAVLinkProtocol::run()
 {
+
+}
+
+QString MAVLinkProtocol::getLogfileName()
+{
+    return QCoreApplication::applicationDirPath()+"/mavlink.log";
 }
 
 /**
@@ -111,6 +125,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
 
         if (decodeState == 1)
         {
+            // Log data
+            if (m_loggingEnabled)
+            {
+                uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+                mavlink_msg_to_send_buffer(buf, &message);
+                m_logfile->write((const char*) buf);
+                qDebug() << "WROTE LOGFILE";
+            }
+
             // ORDER MATTERS HERE!
             // If the matching UAS object does not yet exist, it has to be created
             // before emitting the packetReceived signal
@@ -316,11 +339,32 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled)
     emit heartbeatChanged(enabled);
 }
 
+void MAVLinkProtocol::enableLogging(bool enabled)
+{
+    if (enabled && !m_loggingEnabled)
+    {
+       m_logfile = new QFile(getLogfileName());
+       m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
+    }
+    else
+    {
+       m_logfile->close();
+       delete m_logfile;
+       m_logfile = NULL;
+    }
+    m_loggingEnabled = enabled;
+}
+
 bool MAVLinkProtocol::heartbeatsEnabled(void)
 {
     return m_heartbeatsEnabled;
 }
 
+bool MAVLinkProtocol::loggingEnabled(void)
+{
+    return m_loggingEnabled;
+}
+
 /**
  * The default rate is 1 Hertz.
  *

src/comm/MAVLinkProtocol.h

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
 #include <QMutex>
 #include <QString>
 #include <QTimer>
+#include <QFile>
 #include <QMap>
 #include <QByteArray>
 #include "ProtocolInterface.h"
@@ -65,6 +66,10 @@ public:
     int getHeartbeatRate();
     /** @brief Get heartbeat state */
     bool heartbeatsEnabled(void);
+    /** @brief Get logging state */
+    bool loggingEnabled(void);
+    /** @brief Get the name of the packet log file */
+    static QString getLogfileName();
 
 public slots:
     /** @brief Receive bytes from a communication interface */
@@ -79,14 +84,19 @@ public slots:
     /** @brief Enable / disable the heartbeat emission */
     void enableHeartbeats(bool enabled);
 
+    /** @brief Enable/disable binary packet logging */
+    void enableLogging(bool enabled);
+
     /** @brief Send an extra heartbeat to all connected units */
     void sendHeartbeat();
 
 protected:
-    QTimer* heartbeatTimer;  ///< Timer to emit heartbeats
-    int heartbeatRate;       ///< Heartbeat rate, controls the timer interval
+    QTimer* heartbeatTimer;    ///< Timer to emit heartbeats
+    int heartbeatRate;         ///< Heartbeat rate, controls the timer interval
     bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
-    QMutex receiveMutex;     ///< Mutex to protect receiveBytes function
+    bool m_loggingEnabled;     ///< Enable/disable packet logging
+    QFile* m_logfile;           ///< Logfile
+    QMutex receiveMutex;       ///< Mutex to protect receiveBytes function
     int lastIndex[256][256];
     int totalReceiveCounter;
     int totalLossCounter;
@@ -98,6 +108,8 @@ signals:
     void messageReceived(LinkInterface* link, mavlink_message_t message);
     /** @brief Emitted if heartbeat emission mode is changed */
     void heartbeatChanged(bool heartbeats);
+    /** @brief Emitted if logging is started / stopped */
+    void loggingChanged(bool enabled);
 };
 
 #endif // MAVLINKPROTOCOL_H_

src/comm/MAVLinkSimulationLink.cc

@@ -38,6 +38,7 @@ This file is part of the PIXHAWK project
 #include <QDebug>
 #include "MG.h"
 #include "LinkManager.h"
+#include "MAVLinkProtocol.h"
 #include "MAVLinkSimulationLink.h"
 // MAVLINK includes
 #include <mavlink.h>
@@ -92,6 +93,10 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
+
+    // Open packet log
+    mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
+    mavlinkLogFile->open(QIODevice::ReadOnly);
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
@@ -121,6 +126,21 @@ void MAVLinkSimulationLink::run()
             if (_isConnected)
             {
                 mainloop();
+
+                // FIXME Hacky code to read from packet log file
+//                readyBufferMutex.lock();
+//                for (int i = 0; i < streampointer; i++)
+//                {
+//                    readyBuffer.enqueue(*(stream + i));
+//                }
+//                readyBufferMutex.unlock();
+
+
+
+
+
+
+
                 emit bytesReady(this);
             }
             last = MG::TIME::getGroundTimeNow();

src/comm/MAVLinkSimulationLink.h

@@ -98,6 +98,7 @@ protected:
     QTimer* timer;
     /** File which contains the input data (simulated robot messages) **/
     QFile* simulationFile;
+    QFile* mavlinkLogFile;
     QString simulationHeader;
     /** File where the commands sent by the groundstation are stored **/
     QFile* receiveFile;

src/uas/UAS.cc

@@ -338,7 +338,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "lon", pos.lon, time);
                 emit valueChanged(uasId, "alt", pos.alt, time);
                 emit valueChanged(uasId, "speed", pos.v, time);
-                qDebug() << "GOT GPS RAW";
+                //qDebug() << "GOT GPS RAW";
                 emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
             }
             break;
@@ -346,7 +346,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_gps_status_t pos;
                 mavlink_msg_gps_status_decode(&message, &pos);
-                for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++)
+                //qDebug() << "GOT GPS STATUS FOR "<< pos.satellites_visible << "SATELLITES";
+                for(int i = 0; i < (int)pos.satellites_visible; i++)
                 {
                     emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
                 }

src/ui/HSIDisplay.cc

@@ -43,7 +43,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         gpsSatellites(),
         satellitesUsed(0)
 {
-    
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 }
 
 void HSIDisplay::paintEvent(QPaintEvent * event)
@@ -134,6 +134,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
     }
 
+    connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
+
     // Now connect the new UAS
 
     //if (this->uas != uas)
@@ -147,7 +149,14 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
 void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used)
 {
     Q_UNUSED(uasid);
+    //qDebug() << "UPDATED SATELLITE";
     // If slot is empty, insert object
+    if (gpsSatellites.size() <= satid)
+    {
+        gpsSatellites.resize(satid+1);
+//        gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used));
+    }
+
     if (gpsSatellites.at(satid) == NULL)
     {
         gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used));
@@ -161,11 +170,30 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float dire
 
 QColor HSIDisplay::getColorForSNR(float snr)
 {
-    return QColor(200, 250, 200);
+    QColor color;
+    if (snr < 10)
+    {
+        color = QColor(250, 200, 200);
+    }
+    else if (snr > 20)
+    {
+        color = QColor(200, 250, 200);
+    }
+    else if (snr > 30)
+    {
+        color = QColor(100, 250, 100);
+    }
+    else
+    {
+        color = QColor(180, 180, 180);
+    }
+    return color;
 }
 
 void HSIDisplay::drawGPS()
 {
+    float xCenter = vwidth/2.0f;
+    float yCenter = vwidth/2.0f;
     QPainter painter(this);
     painter.setRenderHint(QPainter::Antialiasing, true);
     painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
@@ -173,7 +201,8 @@ void HSIDisplay::drawGPS()
     // Max satellite circle radius
 
     const float margin = 0.2f;  // 20% margin of total width on each side
-    float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
+    float radius = (vwidth - vwidth * 2.0f * margin) / 4.0f;
+    radius = radius;
 
     // Draw satellite labels
     //    QString label;
@@ -201,10 +230,10 @@ void HSIDisplay::drawGPS()
             painter.setPen(color);
             painter.setBrush(brush);
 
-            float xPos = sin(sat->direction) * sat->azimuth * radius;
-            float yPos = cos(sat->direction) * sat->azimuth * radius;
+            float xPos = xCenter + sin(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius;
+            float yPos = yCenter + cos(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius;
 
-            drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter);
+            drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
         }
     }
 }

src/ui/HSIDisplay.h

@@ -49,6 +49,7 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     void paintEvent(QPaintEvent * event);
 
 protected slots:
@@ -59,7 +60,6 @@ protected slots:
 
 
 protected:
-    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     static QColor getColorForSNR(float snr);
 
     /**
@@ -69,11 +69,11 @@ protected:
     {
     public:
         GPSSatellite(int id, float azimuth, float direction, float snr, bool used) :
-            id(id),
-            azimuth(azimuth),
-            direction(direction),
-            snr(snr),
-            used(used)
+                id(id),
+                azimuth(azimuth),
+                direction(direction),
+                snr(snr),
+                used(used)
         {
 
         }

src/ui/MAVLinkSettingsWidget.cc

@@ -11,9 +11,12 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget
     // 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)));
 
     // Initialize state
     m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled());
+    m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled());
 }
 
 MAVLinkSettingsWidget::~MAVLinkSettingsWidget()

src/ui/MAVLinkSettingsWidget.ui

@@ -24,6 +24,13 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QCheckBox" name="loggingCheckBox">
+     <property name="text">
+      <string>Log all MAVLink packets</string>
+     </property>
+    </widget>
+   </item>
    <item>
     <spacer name="verticalSpacer">
      <property name="orientation">

src/ui/MainWindow.cc

@@ -85,7 +85,7 @@ settings()
     waypoints->setVisible(false);
     info = new UASInfoWidget(this);
     info->setVisible(false);
-    detection = new ObjectDetectionView("test", this);
+    detection = new ObjectDetectionView("patterns", this);
     detection->setVisible(false);
     hud = new HUD(640, 480, this);
     hud->setVisible(false);

src/ui/ObjectDetectionView.cc

@@ -94,7 +94,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
         {
             //qDebug() << "REDETECTED";
 
-            /*
             QList<QAction*> actions = m_ui->listWidget->actions();
             // Find action and update it
             foreach (QAction* act, actions)
@@ -107,7 +106,9 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
                     act->setText(patternPath + separator + "(#" + QString::number(count) + ")" + separator + QString::number(confidence));
                 }
             }
-            QPixmap image = QPixmap(patternFolder + "/" + patternPath);
+            QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/").last();
+            qDebug() << "Loading:" << filePath;
+            QPixmap image = QPixmap(filePath);
             image = image.scaledToWidth(m_ui->imageLabel->width());
             m_ui->imageLabel->setPixmap(image);
             QString patternName = patternPath.split("//").last(); // Remove preceding folder names
@@ -115,7 +116,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
 
             // Set name and label
             m_ui->nameLabel->setText(patternName);
-            */
         }
         else
         {