Fixed a series of minor bugs ruining restarting behaviour when rebooting MAV

src/uas/UAS.cc

@@ -469,7 +469,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             if ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)
             {
-                GAudioOutput::instance()->startEmergency();
+                GAudioOutput::instance()->say(QString("emergency for system %1").arg(this->getUASID()));
+                QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency()));
             }
             else if (modechanged || statechanged)
             {
@@ -521,7 +522,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
                     && (currentVoltage > 3.3f)
                     /* warn only if current voltage is really still lower by a reasonable amount */
-                    && ((currentVoltage - 0.1f) < tickVoltage)
+                    && ((currentVoltage - 0.2f) < tickVoltage)
                     /* warn only every 12 seconds */
                     && (QGC::groundTimeUsecs() - lastVoltageWarning) > 12000000)
             {
@@ -548,7 +549,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 			}
 
             // LOW BATTERY ALARM
-            if (lpVoltage < warnVoltage && (startVoltage > 0.0f))
+            if (lpVoltage < warnVoltage && (currentVoltage - 0.2f) < warnVoltage && (currentVoltage > 3.3))
             {
                 startLowBattAlarm();
             }
@@ -1535,31 +1536,12 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
 */
 quint64 UAS::getUnixTime(quint64 time)
 {
-    bool isNull = (time == 0);
-    // Check if the offset estimation likely went wrong
-    // and we're talking to a new instance / the system
-    // has rebooted. Only reset if this is consistent.
-    if (time != 0 && lastNonNullTime > time)
-    {
-        onboardTimeOffsetInvalidCount++;
-    }
-    else if (time != 0 && lastNonNullTime < time)
-    {
-        onboardTimeOffsetInvalidCount = 0;
-    }
-
-    // Reset onboard time offset estimation, since it seems to be really off
-    if (onboardTimeOffsetInvalidCount > 20)
-    {
-        onboardTimeOffset = 0;
-        onboardTimeOffsetInvalidCount = 0;
-    }
-
     quint64 ret = 0;
     if (attitudeStamped)
     {
         ret = lastAttitude;
     }
+
     if (time == 0)
     {
         ret = QGC::groundTimeMilliseconds();
@@ -1587,10 +1569,13 @@ quint64 UAS::getUnixTime(quint64 time)
 #endif
     {
         //        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
-        if (onboardTimeOffset == 0)
+        if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100))
         {
+            lastNonNullTime = time;
             onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
         }
+        if (time > lastNonNullTime) lastNonNullTime = time;
+
         ret = time/1000 + onboardTimeOffset;
     }
     else
@@ -1600,10 +1585,6 @@ quint64 UAS::getUnixTime(quint64 time)
         ret = time/1000;
     }
 
-    if (!isNull) {
-        lastNonNullTime = time;
-    }
-
     return ret;
 }
 
@@ -2969,7 +2950,7 @@ void UAS::startLowBattAlarm()
     if (!lowBattAlarm)
     {
         GAudioOutput::instance()->alert(tr("system %1 has low battery").arg(getUASName()));
-        QTimer::singleShot(2500, GAudioOutput::instance(), SLOT(startEmergency()));
+        QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency()));
         lowBattAlarm = true;
     }
 }

src/uas/UASInterface.h

@@ -548,7 +548,7 @@ signals:
 protected:
 
     // TIMEOUT CONSTANTS
-    static const unsigned int timeoutIntervalHeartbeat = 2000 * 1000; ///< Heartbeat timeout is 1.5 seconds
+    static const unsigned int timeoutIntervalHeartbeat = 1500 * 1000; ///< Heartbeat timeout is 1.5 seconds
 
 };
 

src/ui/MAVLinkDecoder.cc

@@ -100,11 +100,13 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
 }
 
 quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
-{
+{ 
+    bool isNull = false;
     quint64 ret = 0;
     if (time == 0)
     {
         ret = QGC::groundTimeMilliseconds() - onboardToGCSUnixTimeOffsetAndDelay[systemID];
+        isNull = true;
     }
     // Check if time is smaller than 40 years,
     // assuming no system without Unix timestamp
@@ -128,11 +130,14 @@ quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
     else if (time < 1261440000000)
 #endif
     {
-        if (onboardTimeOffset[systemID] == 0 || time < (firstOnboardTime[systemID]-2000))
+        if (onboardTimeOffset[systemID] == 0 || time < (firstOnboardTime[systemID]-100))
         {
             firstOnboardTime[systemID] = time;
             onboardTimeOffset[systemID] = QGC::groundTimeMilliseconds() - time;
         }
+
+        if (time > firstOnboardTime[systemID]) firstOnboardTime[systemID] = time;
+
         ret = time + onboardTimeOffset[systemID];
     }
     else
@@ -142,6 +147,35 @@ quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
         ret = time;
     }
 
+
+//    // Check if the offset estimation likely went wrong
+//    // and we're talking to a new instance / the system
+//    // has rebooted. Only reset if this is consistent.
+//    if (!isNull && lastNonNullTime > ret)
+//    {
+//        onboardTimeOffsetInvalidCount++;
+//    }
+//    else if (!isNull && lastNonNullTime < ret)
+//    {
+//        onboardTimeOffsetInvalidCount = 0;
+//    }
+
+//    // Reset onboard time offset estimation, since it seems to be really off
+//    if (onboardTimeOffsetInvalidCount > 20)
+//    {
+//        onboardTimeOffset = 0;
+//        onboardTimeOffsetInvalidCount = 0;
+//        lastNonNullTime = 0;
+//        qDebug() << "RESETTET ONBOARD TIME OFFSET";
+//    }
+
+//    // If we're progressing in time, set it
+//    // else wait for the reboot detection to
+//    // catch the timestamp wrap / reset
+//    if (!isNull && (lastNonNullTime < ret)) {
+//        lastNonNullTime = ret;
+//    }
+
     return ret;
 }
 

src/ui/SerialConfigurationWindow.cc

@@ -113,7 +113,6 @@ SerialConfigurationWindow::SerialConfigurationWindow(LinkInterface* link, QWidge
 
         // Load current link config
         ui.portName->setCurrentIndex(ui.baudRate->findText(QString("%1").arg(this->link->getPortName())));
-        qDebug() << __FILE__ << __LINE__ << "port name:" << QString("%1").arg(this->link->getPortName());
 
         connect(action, SIGNAL(triggered()), this, SLOT(configureCommunication()));