diff --git a/images/earth.html b/images/earth.html
index cd788e7647652eb6cfbeb9cc8f96d11371455715..0ab8487b0b60ec992fe5a268de81e9729ca6fb3d 100644
--- a/images/earth.html
+++ b/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);
}
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 5051fa1ebb8dd2db7646ea6a54a598234e71a4a0..abf1a5ecfbf0ea0651a23bc0188179553e3f179d 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/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);
diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h
index 25ad3ecb1b8977084909330b60776873a543310d..4e40139b3056bef21c9fa4b77acd1d05987667df 100644
--- a/src/comm/MAVLinkSimulationLink.h
+++ b/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();
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 87650a671aec587dc2df36047c484c9dc73e8b6e..a7510d4bbbfcb4272e8d1089b839ac115f2b7968 100644
--- a/src/uas/UAS.cc
+++ b/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)
{
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 5a781ccfe887e855074c3a4008b3eb626e1ec1ab..7826b819f89d8ee12deccdc6c7b3e6ec3ce5485c 100644
--- a/src/uas/UAS.h
+++ b/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;
diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc
index 9c15215166d94ea2a555d26493c97d9562fd866a..f182156578391fd9825ed490ed96060470003b9e 100644
--- a/src/ui/map3D/QGCGoogleEarthView.cc
+++ b/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)