Skip to content

Q
qgroundcontrol
Commit 58137306 authored by LM's avatar LM
Browse files
Options

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

parent ba97a1e3
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 177 additions and 106 deletions
images/earth.html
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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
View file @ 58137306
......@@ -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.cc
View file @ 58137306
This diff is collapsed.
src/ui/uas/UASView.h
View file @ 58137306
......@@ -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);
......
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