Skip to content

Q
qgroundcontrol
Commit 41eb8ba6 authored by pixhawk's avatar pixhawk
Browse files
Options

Changed MAVLink packet format, working on Google Earth visualization

parent 56ac4e92
Hide whitespace changes
Inline Side-by-side
Showing with 44 additions and 28 deletions
images/earth.html
View file @ 41eb8ba6
......@@ -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
View file @ 41eb8ba6
......@@ -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
View file @ 41eb8ba6
......@@ -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
View file @ 41eb8ba6
......@@ -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
View file @ 41eb8ba6
......@@ -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
View file @ 41eb8ba6
......@@ -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)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment