Commit 58137306 authored by LM's avatar LM

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

parent ba97a1e3
......@@ -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);
......
......@@ -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),
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
......
......@@ -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));
}
}
......
......@@ -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();
}
......
......@@ -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);
}
......
......@@ -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));
......
This diff is collapsed.
......@@ -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