Improving state updates for multiple MAVs, fixed Google Maps and Goog…

…le Earth

images/earth.html

@@ -417,7 +417,9 @@
  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 @@
  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

@@ -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

@@ -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

@@ -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

@@ -42,24 +42,29 @@ 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);
  emit valueChanged(uasId, "Pressure", "raw", raw.baro, time);
  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

@@ -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

@@ -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

@@ -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

@@ -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));