Improving state updates for multiple MAVs, fixed Google Maps and Google Earth

images/earth.html

@@ -417,7 +417,9 @@ function createAircraft(id, type, color)
 	planeOrient = ge.createOrientation(''); 
 	planeModel.setOrientation(planeOrient); 
 
-	planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
+	//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae'); 
+	//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
+	planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae');
 	planeModel.setLink(planeLink);
 	planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); 
 
@@ -578,18 +580,16 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	      currAlt = trueGroundAlt+0.1;
 	    }
 		// Interpolate between t-1 and t and set new states
-		lastLat = lastLat*0.8+currLat*0.2;
-		lastLon = lastLon*0.8+currLon*0.2;
-		lastAlt = lastAlt*0.8+currAlt*0.2;
-		//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
+		lastLat = lastLat*0.5+currLat*0.5;
+		lastLon = lastLon*0.5+currLon*0.5;
+		lastAlt = lastAlt*0.5+currAlt*0.5;
 	
-	
-		// FIXME Currently invalid conversion from right-handed z-down to z-up frame
-		planeOrient.setRoll(((roll/M_PI))*180.0+180.0);
-		planeOrient.setTilt(((pitch/M_PI))*180.0+180.0);
+		planeOrient.setRoll(+((roll/M_PI))*180.0);
+		planeOrient.setTilt(-((pitch/M_PI))*180.0);
 		planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
+		planeModel.setOrientation(planeOrient);
 	
-		currFollowHeading = ((yaw/M_PI))*180.0;
+		currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
     
     	planeLoc.setLatitude(lastLat);
     	planeLoc.setLongitude(lastLon);

src/comm/MAVLinkSimulationMAV.cc

@@ -14,10 +14,10 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     timer1Hz(0),
     latitude(lat),
     longitude(lon),
-    altitude(0.0),
+    altitude(550.0),
     x(lat),
     y(lon),
-    z(550),
+    z(altitude),
     roll(0.0),
     pitch(0.0),
     yaw(0.0),

src/comm/QGCFlightGearLink.cc

@@ -188,7 +188,7 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
 void QGCFlightGearLink::readBytes()
 {
     const qint64 maxLength = 65536;
-    static char data[maxLength];
+    char data[maxLength];
     QHostAddress sender;
     quint16 senderPort;
 

src/comm/UDPLink.cc

@@ -210,7 +210,7 @@ void UDPLink::writeBytes(const char* data, qint64 size)
 void UDPLink::readBytes()
 {
     const qint64 maxLength = 65536;
-    static char data[maxLength];
+    char data[maxLength];
     QHostAddress sender;
     quint16 senderPort;
 

src/uas/PxQuadMAV.cc

@@ -42,9 +42,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 #ifdef MAVLINK_ENABLED_PIXHAWK
     mavlink_message_t* msg = &message;
 
-    if (message.sysid == uasId) {
-        switch (message.msgid) {
-        case MAVLINK_MSG_ID_RAW_AUX: {
+    if (message.sysid == uasId)
+    {
+        switch (message.msgid)
+        {
+        case MAVLINK_MSG_ID_RAW_AUX:
+            {
             mavlink_raw_aux_t raw;
             mavlink_msg_raw_aux_decode(&message, &raw);
             quint64 time = getUnixTime(0);
@@ -52,14 +55,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
         }
         break;
-        case MAVLINK_MSG_ID_IMAGE_TRIGGERED: {
+        case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
+        {
             // FIXME Kind of a hack to load data from disk
             mavlink_image_triggered_t img;
             mavlink_msg_image_triggered_decode(&message, &img);
             emit imageStarted(img.timestamp);
         }
         break;
-        case MAVLINK_MSG_ID_PATTERN_DETECTED: {
+        case MAVLINK_MSG_ID_PATTERN_DETECTED:
+        {
             mavlink_pattern_detected_t detected;
             mavlink_msg_pattern_detected_decode(&message, &detected);
             QByteArray b;

src/uas/UAS.cc

@@ -232,6 +232,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     case MAV_TYPE_QUADROTOR:
                         setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
                         break;
+                    case MAV_TYPE_HEXAROTOR:
+                        setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER);
+                        break;
                     default:
                         // Do nothing
                         break;
@@ -338,8 +341,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
                 emit loadChanged(this,state.load/10.0f);
 
-
-
                 currentVoltage = state.voltage_battery/1000.0f;
                 lpVoltage = filterVoltage(currentVoltage);
 
@@ -463,7 +464,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit globalPositionChanged(this, latitude, longitude, altitude, time);
                 emit speedChanged(this, speedX, speedY, speedZ, time);
                 // Set internal state
-                if (!positionLock) {
+                if (!positionLock)
+                {
                     // If position was not locked before, notify positive
                     GAudioOutput::instance()->notifyPositive();
                 }
@@ -483,29 +485,35 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 // quint64 time = getUnixTime(pos.time_usec);
                 quint64 time = getUnixTime(pos.time_usec);
 
-                if (pos.fix_type > 2) {
+                if (pos.fix_type > 2)
+                {
                     emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
                     latitude = pos.lat/(double)1E7;
                     longitude = pos.lon/(double)1E7;
                     altitude = pos.alt/1000.0;
                     positionLock = true;
+                    isGlobalPositionKnown = true;
 
                     // Check for NaN
                     int alt = pos.alt;
-                    if (alt != alt) {
+                    if (!isnan(alt) && !isinf(alt))
+                    {
                         alt = 0;
-                        emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
+                        emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
                     }
                     // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                     // Smaller than threshold and not NaN
 
                     float vel = pos.vel/100.0f;
 
-                    if (vel < 1000000 && !isnan(vel) && !isinf(vel)) {
+                    if (vel < 1000000 && !isnan(vel) && !isinf(vel))
+                    {
                         // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time);
                         //qDebug() << "GOT GPS RAW";
                         // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
-                    } else {
+                    }
+                    else
+                    {
                         emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
                     }
                 }

src/ui/HDDisplay.cc

@@ -186,9 +186,9 @@ void HDDisplay::triggerUpdate()
 void HDDisplay::paintEvent(QPaintEvent * event)
 {
     Q_UNUSED(event);
-    static quint64 interval = 0;
+    quint64 interval = 0;
     //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
-    interval = MG::TIME::getGroundTimeNow();
+    interval = QGC::groundTimeMilliseconds();
     renderOverlay();
 }
 

src/ui/QGCMAVLinkInspector.cc

@@ -41,7 +41,6 @@ void QGCMAVLinkInspector::refreshView()
         messageName = messageName.arg(messageInfo[msg->msgid].name).arg(messagesHz.value(msg->msgid, 0), 2, 'f', 0).arg(msg->msgid);
         if (!treeWidgetItems.contains(msg->msgid))
         {
-
             QStringList fields;
             fields << messageName;
             QTreeWidgetItem* widget = new QTreeWidgetItem(fields);
@@ -72,8 +71,6 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
 {
     Q_UNUSED(link);
     // Only overwrite if system filter is set
-//    int filterValue = ui->systemComboBox()->value().toInt();
-//    if (filterValue != )
     memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
 
     float msgHz = 0.0f;
@@ -85,14 +82,10 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
         {
             msgHz = 1;
         }
-        //qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid);
         float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1);
-        qDebug() << "HZ" << newHz;
         messagesHz.insert(message.msgid, newHz);
     }
 
-    //qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000);
-
     lastMessageUpdate.insert(message.msgid, receiveTime);
 }
 

src/ui/map3D/QGCGoogleEarthView.cc

@@ -284,8 +284,6 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, dou
 {
     Q_UNUSED(usec);
     javaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 22).arg(lon, 0, 'f', 22).arg(alt, 0, 'f', 22));
-
-    //qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15);
 }
 
 void QGCGoogleEarthView::clearTrails()
@@ -614,7 +612,8 @@ void QGCGoogleEarthView::updateState()
 #if (QGC_EVENTLOOP_DEBUG)
     qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
 #endif
-    if (gEarthInitialized) {
+    if (gEarthInitialized)
+    {
         int uasId = 0;
         double lat = 47.3769;
         double lon = 8.549444;
@@ -628,6 +627,8 @@ void QGCGoogleEarthView::updateState()
         QList<UASInterface*> mavs = UASManager::instance()->getUASList();
         foreach (UASInterface* currMav, mavs)
         {
+            // Only update if known
+            if (!currMav->globalPositionKnown()) continue;
             uasId = currMav->getUASID();
             lat = currMav->getLatitude();
             lon = currMav->getLongitude();
@@ -640,9 +641,9 @@ void QGCGoogleEarthView::updateState()
 
             javaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
                        .arg(uasId)
-                       .arg(lat, 0, 'f', 15)
-                       .arg(lon, 0, 'f', 15)
-                       .arg(alt, 0, 'f', 15)
+                       .arg(lat, 0, 'f', 22)
+                       .arg(lon, 0, 'f', 22)
+                       .arg(alt, 0, 'f', 22)
                        .arg(roll, 0, 'f', 9)
                        .arg(pitch, 0, 'f', 9)
                        .arg(yaw, 0, 'f', 9));

src/ui/uas/UASView.h

@@ -117,6 +117,7 @@ protected:
     float alt;
     float groundDistance;
     bool localFrame;
+    bool globalFrameKnown;
     QAction* removeAction;
     QAction* renameAction;
     QAction* selectAction;
@@ -126,6 +127,8 @@ protected:
     static const int updateInterval = 800;
     static const int errorUpdateInterval = 200;
     bool lowPowerModeEnabled; ///< Low power mode reduces update rates
+    unsigned int generalUpdateCount; ///< Skip counter for updates
+    double filterTime; ///< Filter time estimate of battery
 
 
     void mouseDoubleClickEvent (QMouseEvent * event);