Fixed issues with loss counting, added support for usec timestamps

src/MG.h

@@ -303,6 +303,24 @@ namespace MG
             return static_cast<quint64>(milliseconds + time.time().msec());
         }
 
+        /**
+         * @brief Convenience method to get the milliseconds time stamp for now
+         *
+         * The timestamp is created at the instant of calling this method. It is
+         * defined as the number of milliseconds since unix epoch, which is
+         * 1.1.1970, 00:00 UTC.
+         *
+         * @return The number of milliseconds elapsed since unix epoch
+         * @deprecated Will the replaced by time helper class
+         **/
+        static quint64 getGroundTimeNowUsecs() {
+            QDateTime time = QDateTime::currentDateTime();
+            time = time.toUTC();
+            /* Return seconds and milliseconds, in milliseconds unit */
+            quint64 microseconds = time.toTime_t() * static_cast<quint64>(1000000);
+            return static_cast<quint64>(microseconds + (time.time().msec()*1000));
+        }
+
         /*tatic quint64 getMissionTimeUsecs()
         {
             ;

src/comm/MAVLinkProtocol.cc

@@ -149,7 +149,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
                 }
 
                 int safeguard = 0;
-                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
+                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                 while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 1)
                 {
                     if (lastIndex[message.sysid][message.compid] == 255)
@@ -171,13 +171,12 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
             //                while (lastCompIndex->value(message.compid, 0)+1 )
             //            }
             //if ()
-            if (lastLoss != totalLossCounter)
+            if (lastLoss != totalLossCounter || (totalReceiveCounter / 64) == 0)
             {
                 // Calculate new loss ratio
                 // Receive loss
                 float receiveLoss = (double)totalLossCounter/(double)(totalReceiveCounter+totalLossCounter);
                 receiveLoss *= 100.0f;
-                qDebug() << "LOSS" << receiveLoss;
                 emit receiveLossChanged(receiveLoss);
             }