Changed MAVLink packet format, working on Google Earth visualization

images/earth.html

@@ -15,6 +15,9 @@ var ge = null;
 var initialized = false;
 var currAircraft = 220;
 var followEnabled = false;
+
+var lastLat = 0;
+var lastLon = 0;
 var currLat = 47.3769;
 var currLon = 8.549444;
 var currAlt = 470;
@@ -48,6 +51,7 @@ var lineStringPlacemark;
 var lineStyle;
 
 // Aircraft class
+var planeColor = '6600ffff';
 
 
 function isInitialized()
@@ -127,6 +131,7 @@ function createAircraft(id, type, color)
           aircraft[id] = planePlacemark;
           attitudes[id] = planeOrient;
           locations[id] = planeLoc;
+          //planeColor = color;
           
           //createTrail(id, color);
 }
@@ -139,7 +144,7 @@ function createTrail(id, color)
 // and set the altitude mode
 trail = ge.createLineString('');
 lineStringPlacemark.setGeometry(trail);
-trail.setExtrude(true);
+trail.setExtrude(false);
 trail.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
 
 // Add LineString points
@@ -149,7 +154,8 @@ trail.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
 lineStringPlacemark.setStyleSelector(ge.createStyle(''));
 lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle();
 lineStyle.setWidth(5);
-lineStyle.getColor().set(color);
+lineStyle.getColor().set(planeColor);  // aabbggrr format
+//lineStyle.getColor().set(color);
 //lineStyle.getColor().set(color);  // aabbggrr format
 
 // Add the feature to Earth
@@ -159,7 +165,7 @@ ge.getFeatures().appendChild(lineStringPlacemark);
 
 function addTrailPosition(id, lat, lon, alt)
 {
-	trail.setExtrude(true);
+	trail.setExtrude(false);
 trail.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
 
 // Add LineString points
@@ -170,7 +176,7 @@ trail.getCoordinates().pushLatLngAlt(currLat, currLon, currAlt);
 lineStringPlacemark.setStyleSelector(ge.createStyle(''));
 lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle();
 lineStyle.setWidth(5);
-lineStyle.getColor().set('99bbaaff');
+lineStyle.getColor().set(planeColor);  // aabbggrr format
 //lineStyle.getColor().set(color);  // aabbggrr format
 
 // Add the feature to Earth
@@ -204,10 +210,12 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 {
 	if (id == currAircraft)
 	{
+		lastLat = currLat;
+		lastLon = lastLon;
 		currLat = lat;
 		currLon = lon;
 		currAlt = alt;
-		currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
+		//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
 	}
 	
 	// FIXME Currently invalid conversion from right-handed z-down to z-up frame
@@ -259,7 +267,8 @@ function updateFollowAircraft()
 	currView.setAltitude(currAlt);
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currView.setHeading(currFollowHeading-90.0);
+	currFollowHeading = 0.9*currFollowHeading+0.1*((Math.atan2(lastLat-currLat, lastLon-currLon)/M_PI)+1.0)*360.0;
+	currView.setHeading(currFollowHeading-0.0);
 	ge.getView().setAbstractView(currView);
 }
 

src/comm/MAVLinkSimulationLink.cc

@@ -413,7 +413,7 @@ void MAVLinkSimulationLink::mainloop()
 //        streampointer += bufferlength;
 
         // GLOBAL POSITION
-        mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.0005), 8.54899892510421+(y*0.0005), z+35.0, 0, 0, 0);
+        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);

src/comm/MAVLinkSimulationLink.h

@@ -85,6 +85,7 @@ public:
 public slots:
     void writeBytes(const char* data, qint64 size);
     void readBytes();
+    /** @brief Mainloop simulating the mainloop of the MAV */
     virtual void mainloop();
     bool connectLink(bool connect);
     void connectLink();

src/uas/UAS.cc

@@ -350,24 +350,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 positionLock = true;
             }
             break;
-        case MAVLINK_MSG_ID_GLOBAL_POSITION:
+        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
             //std::cerr << std::endl;
             //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
             {
-                mavlink_global_position_t pos;
-                mavlink_msg_global_position_decode(&message, &pos);
-                quint64 time = getUnixTime(pos.usec);
-                latitude = pos.lat;
-                longitude = pos.lon;
-                altitude = pos.alt;
+                mavlink_global_position_int_t pos;
+                mavlink_msg_global_position_int_decode(&message, &pos);
+                quint64 time = QGC::groundTimeUsecs();
+                latitude = pos.lat/(double)1E7;
+                longitude = pos.lon/(double)1E7;
+                altitude = pos.alt/1000.0;
+                speedX = pos.vx/100.0;
+                speedY = pos.vy/100.0;
+                speedZ = pos.vz/100.0;
                 emit valueChanged(uasId, "lat", pos.lat, time);
                 emit valueChanged(uasId, "lon", pos.lon, time);
                 emit valueChanged(uasId, "alt", pos.alt, time);
-                emit valueChanged(uasId, "g-vx", pos.vx, time);
-                emit valueChanged(uasId, "g-vy", pos.vy, time);
-                emit valueChanged(uasId, "g-vz", pos.vz, time);
-                emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
-                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
+                emit valueChanged(uasId, "g-vx", speedX, time);
+                emit valueChanged(uasId, "g-vy", speedY, time);
+                emit valueChanged(uasId, "g-vz", speedZ, time);
+                emit globalPositionChanged(this, longitude, latitude, altitude, time);
+                emit speedChanged(this, speedX, speedY, speedZ, time);
                 // Set internal state
                 if (!positionLock)
                 {

src/uas/UAS.h

@@ -146,6 +146,9 @@ protected:
     double latitude;
     double longitude;
     double altitude;
+    double speedX;              ///< True speed in X axis
+    double speedY;              ///< True speed in Y axis
+    double speedZ;              ///< True speed in Z axis
     double roll;
     double pitch;
     double yaw;

src/ui/map3D/QGCGoogleEarthView.cc

@@ -29,7 +29,7 @@
 QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) :
         QWidget(parent),
         updateTimer(new QTimer(this)),
-        refreshRateMs(60),
+        refreshRateMs(40),
         mav(NULL),
         followCamera(true),
         trailEnabled(true),
@@ -108,8 +108,8 @@ QGCGoogleEarthView::~QGCGoogleEarthView()
 void QGCGoogleEarthView::addUAS(UASInterface* uas)
 {
     // uasid, type, color (in aarrbbgg format)
-    //javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg(uas->getColor().name().remove(0, 1).prepend("50")));
-    javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg("0"));
+    javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg(uas->getColor().name().remove(0, 1).prepend("50")));
+    //javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg("0"));
     // Automatically receive further position updates
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
 }
@@ -324,12 +324,12 @@ void QGCGoogleEarthView::updateState()
 
             javaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
                        .arg(uasId)
-                       .arg(lat)
-                       .arg(lon)
-                       .arg(alt+500)
-                       .arg(roll)
-                       .arg(pitch)
-                       .arg(yaw));
+                       .arg(lat, 0, 'f', 15)
+                       .arg(lon, 0, 'f', 15)
+                       .arg(alt, 0, 'f', 15)
+                       .arg(roll, 0, 'f', 9)
+                       .arg(pitch, 0, 'f', 9)
+                       .arg(yaw, 0, 'f', 9));
         }
 
         if (followCamera)