Bugfixed log replaying, still working on it

images/earth.html

@@ -250,9 +250,9 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
     }
         
                 
-    //planeLoc.setLatitude(lat);
-    //planeLoc.setLongitude(lon);
-    //planeLoc.setAltitude(alt);
+    planeLoc.setLatitude(lat);
+    planeLoc.setLongitude(lon);
+    planeLoc.setAltitude(alt);
 	
 }
 
@@ -297,13 +297,7 @@ function updateFollowAircraft()
 
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currFollowHeading = 0;// 0.9*currFollowHeading+0.1*((Math.atan2(lastLat-currLat, lastLon-currLon)/M_PI)+1.0)*360.0;
-	currView.setHeading(currFollowHeading-0.0);
-	
-	// FIXME Shouldn't update aircraft in here, but works for now
-	planeLoc.setLatitude(lastLat);
-    planeLoc.setLongitude(lastLon);
-    planeLoc.setAltitude(lastAlt);
+	currView.setHeading(currFollowHeading-90.0);
 	
 	ge.getView().setAbstractView(currView);
 }

src/Core.cc

@@ -189,6 +189,15 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
             QTimer::singleShot(200, mainWindow, SLOT(close()));
         }
     }
+
+//    forever
+//    {
+//        QGC::SLEEP::msleep(5000);
+//    }
+
+//    mainWindow->close();
+//    mainWindow->deleteLater();
+//    QGC::SLEEP::msleep(200);
 }
 
 /**

src/comm/LinkManager.cc

@@ -143,11 +143,9 @@ bool LinkManager::disconnectLink(LinkInterface* link)
 void LinkManager::removeLink(QObject* link)
 {
     LinkInterface* linkInterface = dynamic_cast<LinkInterface*>(link);
-    // Add link to link list
-    if (links.contains(linkInterface))
+    if (linkInterface)
     {
-        int linkIndex = links.indexOf(linkInterface);
-        links.removeAt(linkIndex);
+        removeLink(linkInterface);
     }
 }
 
@@ -162,6 +160,12 @@ bool LinkManager::removeLink(LinkInterface* link)
                     links.removeAt(i); //remove from link list
                 }
             }
+            // Remove link from protocol map
+            QList<ProtocolInterface* > protocols = protocolLinks.keys(link);
+            foreach (ProtocolInterface* proto, protocols)
+            {
+                protocolLinks.remove(proto, link);
+            }
             return true;
         }
         return false;

src/comm/LinkManager.h

@@ -34,7 +34,7 @@ This file is part of the PIXHAWK project
 
 #include <QThread>
 #include <QList>
-#include <QMap>
+#include <QMultiMap>
 #include <LinkInterface.h>
 #include <ProtocolInterface.h>
 
@@ -79,7 +79,7 @@ public slots:
 protected:
     LinkManager();
     QList<LinkInterface*> links;
-    QMap<ProtocolInterface*,LinkInterface*> protocolLinks;
+    QMultiMap<ProtocolInterface*,LinkInterface*> protocolLinks;
 
 private:
     static LinkManager* _instance;

src/comm/MAVLinkSimulationLink.cc

@@ -97,6 +97,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
+    QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*)));
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()

src/uas/UAS.cc

@@ -956,18 +956,22 @@ void UAS::forwardMessage(mavlink_message_t message)
 {
     // Emit message on all links that are currently connected
     QList<LinkInterface*>link_list = LinkManager::instance()->getLinksForProtocol(mavlink);
+
     foreach(LinkInterface* link, link_list)
     {
-        SerialLink* serial = dynamic_cast<SerialLink*>(link);
-        if(serial != 0)
+        if (link)
         {
-
-            for(int i=0;i<links->size();i++)
+            SerialLink* serial = dynamic_cast<SerialLink*>(link);
+            if(serial != 0)
             {
-                if(serial != links->at(i))
+
+                for(int i=0;i<links->size();i++)
                 {
-                    qDebug()<<"Forwarding Over link: "<<serial->getName()<<" "<<serial;
-                    sendMessage(serial, message);
+                    if(serial != links->at(i))
+                    {
+                        qDebug()<<"Forwarding Over link: "<<serial->getName()<<" "<<serial;
+                        sendMessage(serial, message);
+                    }
                 }
             }
         }
@@ -1671,11 +1675,21 @@ void UAS::addLink(LinkInterface* link)
     if (!links->contains(link))
     {
         links->append(link);
+        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
     }
     //links->append(link);
     //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK";
 }
 
+void UAS::removeLink(QObject* object)
+{
+    LinkInterface* link = dynamic_cast<LinkInterface*>(object);
+    if (link)
+    {
+        links->removeAt(links->indexOf(link));
+    }
+}
+
 /**
  * @brief Get the links associated with this robot
  *

src/uas/UAS.h

@@ -223,6 +223,8 @@ public slots:
 
     /** @brief Add a link associated with this robot */
     void addLink(LinkInterface* link);
+    /** @brief Remove a link associated with this robot */
+    void removeLink(QObject* object);
 
     /** @brief Receive a message from one of the communication links. */
     virtual void receiveMessage(LinkInterface* link, mavlink_message_t message);

src/ui/HUD.cc

@@ -142,9 +142,9 @@ HUD::HUD(int width, int height, QWidget* parent)
     fill.setColor(2, qRgb(0, 0, 0));
     fill.fill(0);
 
-    //QString imagePath = MG::DIR::getIconDirectory() + "hud-template.png";
-    //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
-    //fill = QImage(imagePath);
+    QString imagePath = "/Users/user/Desktop/frame0000.png";
+    qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
+    fill = QImage(imagePath);
 
     glImage = QGLWidget::convertToGLFormat(fill);
 

src/ui/JoystickWidget.cc

@@ -136,5 +136,5 @@ void JoystickWidget::pressKey(int key)
 
 void JoystickWidget::updateStatus(const QString& status)
 {
-
+    m_ui->statusLabel->setText(status);
 }

src/ui/QGCMAVLinkLogPlayer.cc

@@ -23,6 +23,9 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     ui->setupUi(this);
     ui->gridLayout->setAlignment(Qt::AlignTop);
 
+    // Connect protocol
+    connect(this, SIGNAL(bytesReady(LinkInterface*,QByteArray)), mavlink, SLOT(receiveBytes(LinkInterface*,QByteArray)));
+
     // Setup timer
     connect(&loopTimer, SIGNAL(timeout()), this, SLOT(logLoop()));
 
@@ -31,9 +34,12 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     connect(ui->pauseButton, SIGNAL(clicked()), this, SLOT(pause()));
     connect(ui->playButton, SIGNAL(clicked()), this, SLOT(play()));
     connect(ui->speedSlider, SIGNAL(valueChanged(int)), this, SLOT(setAccelerationFactorInt(int)));
+    connect(ui->positionSlider, SIGNAL(valueChanged(int)), this, SLOT(jumpToSliderVal(int)));
+    connect(ui->positionSlider, SIGNAL(sliderPressed()), this, SLOT(pause()));
 
     setAccelerationFactorInt(49);
     ui->speedSlider->setValue(49);
+    ui->positionSlider->setValue(ui->positionSlider->minimum());
 }
 
 QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer()
@@ -47,8 +53,10 @@ void QGCMAVLinkLogPlayer::play()
     {
         ui->pauseButton->setChecked(false);
         ui->selectFileButton->setEnabled(false);
-        if (logLink != NULL)
+        if (logLink)
         {
+            logLink->disconnect();
+            LinkManager::instance()->removeLink(logLink);
             delete logLink;
         }
         logLink = new MAVLinkSimulationLink("");
@@ -74,18 +82,44 @@ void QGCMAVLinkLogPlayer::pause()
     loopTimer.stop();
     ui->playButton->setChecked(false);
     ui->selectFileButton->setEnabled(true);
-    delete logLink;
-    logLink = NULL;
+    if (logLink)
+    {
+        logLink->disconnect();
+        LinkManager::instance()->removeLink(logLink);
+        delete logLink;
+        logLink = NULL;
+    }
 }
 
-void QGCMAVLinkLogPlayer::reset()
+bool QGCMAVLinkLogPlayer::reset(int packetIndex)
 {
-    pause();
-    loopCounter = 0;
-    logFile.reset();
-    ui->pauseButton->setChecked(true);
-    ui->progressBar->setValue(0);
-    startTime = 0;
+    // Reset only for valid values
+    if (packetIndex >= 0 && packetIndex <= logFile.size() - (packetLen+timeLen))
+    {
+        bool result = true;
+        pause();
+        loopCounter = 0;
+        logFile.reset();
+        if (!logFile.seek(packetIndex*(timeLen + packetLen)))
+        {
+            // Fallback: Start from scratch
+            logFile.reset();
+            ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
+            result = false;
+        }
+
+        ui->pauseButton->setChecked(true);
+        ui->positionSlider->blockSignals(true);
+        int sliderVal = (packetIndex / (double)(logFile.size()/(packetLen+timeLen))) * (ui->positionSlider->maximum() - ui->positionSlider->minimum());
+        ui->positionSlider->setValue(sliderVal);
+        ui->positionSlider->blockSignals(false);
+        startTime = 0;
+        return result;
+    }
+    else
+    {
+        return false;
+    }
 }
 
 void QGCMAVLinkLogPlayer::selectLogFile()
@@ -129,8 +163,7 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
     // Ensure that the playback process is stopped
     if (logFile.isOpen())
     {
-        loopTimer.stop();
-        logFile.reset();
+        pause();
         logFile.close();
     }
     logFile.setFileName(file);
@@ -146,15 +179,52 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
         logFile.reset();
         startTime = 0;
         ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName()));
-        ui->logStatsLabel->setText(tr("Log: %2 MB, %3 packets").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))));
+
+        // Get the time interval from the logfile
+        QByteArray timestamp = logFile.read(timeLen);
+
+        // First timestamp
+        quint64 starttime = *((quint64*)(timestamp.constData()));
+
+        // Last timestamp
+        logFile.seek(logFile.size()-packetLen-timeLen);
+        QByteArray timestamp2 = logFile.read(timeLen);
+        quint64 endtime = *((quint64*)(timestamp2.constData()));
+        // Reset everything
+        logFile.reset();
+
+        qDebug() << "Starttime:" << starttime << "End:" << endtime;
+
+        // WARNING: Order matters in this computation
+        int seconds = (endtime - starttime)/1000000;
+        int minutes = seconds / 60;
+        int hours = minutes / 60;
+        seconds -= 60*minutes;
+        minutes -= 60*hours;
+
+        QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
+        ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel));
     }
 }
 
-union log64
+/**
+ * Jumps to the current percentage of the position slider
+ */
+void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
 {
-    quint64 q;
-    const char* b;
-};
+    loopTimer.stop();
+    // Set the logfile to the correct percentage and
+    // align to the timestamp values
+    int packetCount = logFile.size() / (packetLen + timeLen);
+    int packetIndex = (packetCount - 1) * (slidervalue / (double)(ui->positionSlider->maximum() - ui->positionSlider->minimum()));
+
+    // Do only accept valid jumps
+    if (reset(packetIndex))
+    {
+        ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex));
+        //qDebug() << "SET POSITION TO PACKET:" << packetIndex;
+    }
+}
 
 /**
  * This function is the "mainloop" of the log player, reading one line
@@ -166,8 +236,6 @@ union log64
  */
 void QGCMAVLinkLogPlayer::logLoop()
 {
-    const int packetLen = MAVLINK_MAX_PACKET_LEN;
-    const int timeLen = sizeof(quint64);
     bool ok;
 
     // First check initialization
@@ -207,7 +275,7 @@ void QGCMAVLinkLogPlayer::logLoop()
     QByteArray packet = chunk.mid(0, packetLen);
 
     // Emit this packet
-    mavlink->receiveBytes(logLink, packet);
+    emit bytesReady(logLink, packet);
 
     // Check if reached end of file before reading next timestamp
     if (chunk.length() < timeLen+packetLen || logFile.atEnd())
@@ -217,7 +285,6 @@ void QGCMAVLinkLogPlayer::logLoop()
 
         QString status = tr("Reached end of MAVLink log file.");
         ui->logStatsLabel->setText(status);
-        ui->progressBar->setValue(100);
         MainWindow::instance()->showStatusMessage(status);
         return;
     }
@@ -265,9 +332,11 @@ void QGCMAVLinkLogPlayer::logLoop()
         if (loopCounter % 40 == 0)
         {
             QFileInfo logFileInfo(logFile);
-            int progress = 100.0f*(loopCounter/(float)(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))));
+            int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast<float>(logFileInfo.size()));
             //qDebug() << "Progress:" << progress;
-            ui->progressBar->setValue(progress);
+            ui->positionSlider->blockSignals(true);
+            ui->positionSlider->setValue(progress);
+            ui->positionSlider->blockSignals(false);
         }
         loopCounter++;
         // Ui update: Only every 20 messages

src/ui/QGCMAVLinkLogPlayer.h

@@ -5,6 +5,7 @@
 #include <QFile>
 
 #include "MAVLinkProtocol.h"
+#include "LinkInterface.h"
 #include "MAVLinkSimulationLink.h"
 
 namespace Ui {
@@ -32,16 +33,22 @@ public slots:
     /** @brief Pause the logfile */
     void pause();
     /** @brief Reset the logfile */
-    void reset();
+    bool reset(int packetIndex=0);
     /** @brief Select logfile */
     void selectLogFile();
     /** @brief Load log file */
     void loadLogFile(const QString& file);
+    /** @brief Jump to a position in the logfile */
+    void jumpToSliderVal(int slidervalue);
     /** @brief The logging mainloop */
     void logLoop();
     /** @brief Set acceleration factor in percent */
     void setAccelerationFactorInt(int factor);
 
+signals:
+    /** @brief Send ready bytes */
+    void bytesReady(LinkInterface* link, const QByteArray& bytes);
+
 protected:
     int lineCounter;
     int totalLines;
@@ -54,6 +61,8 @@ protected:
     QFile logFile;
     QTimer loopTimer;
     int loopCounter;
+    static const int packetLen = MAVLINK_MAX_PACKET_LEN;
+    static const int timeLen = sizeof(quint64);
     void changeEvent(QEvent *e);
 
 private:

src/ui/QGCMAVLinkLogPlayer.ui

@@ -7,13 +7,13 @@
     <x>0</x>
     <y>0</y>
     <width>407</width>
-    <height>144</height>
+    <height>152</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" columnstretch="1,10,10,1,1,0">
+  <layout class="QGridLayout" name="gridLayout" columnstretch="1,0,0,0,0,0">
    <property name="horizontalSpacing">
     <number>12</number>
    </property>
@@ -101,22 +101,6 @@
      </property>
     </widget>
    </item>
-   <item row="4" column="0" colspan="6">
-    <widget class="QProgressBar" name="progressBar">
-     <property name="toolTip">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="statusTip">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="whatsThis">
-      <string>Percent of the logfile replayed</string>
-     </property>
-     <property name="value">
-      <number>0</number>
-     </property>
-    </widget>
-   </item>
    <item row="1" column="0">
     <widget class="QLabel" name="speedLabel">
      <property name="toolTip">
@@ -156,6 +140,22 @@
      </property>
     </widget>
    </item>
+   <item row="4" column="0" colspan="6">
+    <widget class="QSlider" name="positionSlider">
+     <property name="maximum">
+      <number>10000</number>
+     </property>
+     <property name="pageStep">
+      <number>100</number>
+     </property>
+     <property name="tracking">
+      <bool>false</bool>
+     </property>
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources>

src/ui/map/MAV2DIcon.cc

@@ -46,50 +46,50 @@ void MAV2DIcon::setYaw(float yaw)
 void MAV2DIcon::drawIcon(QPen* pen)
 {
 
-    mypixmap = new QPixmap(radius+1, radius+1);
-    mypixmap->fill(Qt::transparent);
-    QPainter painter(mypixmap);
+//    mypixmap = new QPixmap(radius+1, radius+1);
+//    mypixmap->fill(Qt::transparent);
+//    QPainter painter(mypixmap);
 
-    // DRAW WAYPOINT
-    QPointF p(radius/2, radius/2);
+//    // DRAW WAYPOINT
+//    QPointF p(radius/2, radius/2);
 
-    float waypointSize = radius;
-    QPolygonF poly(4);
-    // Top point
-    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-    // Right point
-    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-    // Bottom point
-    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+//    float waypointSize = radius;
+//    QPolygonF poly(4);
+//    // Top point
+//    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+//    // Right point
+//    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+//    // Bottom point
+//    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+//    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
 
-//    // Select color based on if this is the current waypoint
-//    if (list.at(i)->getCurrent())
+////    // Select color based on if this is the current waypoint
+////    if (list.at(i)->getCurrent())
+////    {
+////        color = QGC::colorCyan;//uas->getColor();
+////        pen.setWidthF(refLineWidthToPen(0.8f));
+////    }
+////    else
+////    {
+////        color = uas->getColor();
+////        pen.setWidthF(refLineWidthToPen(0.4f));
+////    }
+
+//    //pen.setColor(color);
+//    if (pen)
 //    {
-//        color = QGC::colorCyan;//uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.8f));
+//        pen->setWidthF(2);
+//        painter.setPen(*pen);
 //    }
 //    else
 //    {
-//        color = uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.4f));
+//        QPen pen2(Qt::red);
+//        pen2.setWidth(2);
+//        painter.setPen(pen2);
 //    }
+//    painter.setBrush(Qt::NoBrush);
 
-    //pen.setColor(color);
-    if (pen)
-    {
-        pen->setWidthF(2);
-        painter.setPen(*pen);
-    }
-    else
-    {
-        QPen pen2(Qt::red);
-        pen2.setWidth(2);
-        painter.setPen(pen2);
-    }
-    painter.setBrush(Qt::NoBrush);
-
-    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
-    painter.drawPolygon(poly);
+//    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+//    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+//    painter.drawPolygon(poly);
 }