From efb95d00a018f3bc8dd57b38370b16e830221ff8 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 9 Jan 2012 10:40:04 +0100 Subject: [PATCH] Fixed log buttons, replay pausing does not work yet properly, but issue / solution is now known --- src/comm/MAVLinkProtocol.cc | 132 ++++++++++++++++++++++---------- src/ui/MAVLinkSettingsWidget.cc | 25 ++++-- src/ui/QGCMAVLinkLogPlayer.cc | 11 ++- src/ui/QGCMAVLinkLogPlayer.h | 1 + src/ui/QGCToolBar.cc | 4 +- 5 files changed, 120 insertions(+), 53 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 82fbe2b2c..17d19ec6a 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -63,8 +63,10 @@ MAVLinkProtocol::MAVLinkProtocol() : totalLossCounter = 0; currReceiveCounter = 0; currLossCounter = 0; - for (int i = 0; i < 256; i++) { - for (int j = 0; j < 256; j++) { + for (int i = 0; i < 256; i++) + { + for (int j = 0; j < 256; j++) + { lastIndex[i][j] = -1; } } @@ -83,9 +85,12 @@ void MAVLinkProtocol::loadSettings() enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool()); // Only set logfile if there is a name present in settings - if (settings.contains("LOGFILE_NAME") && m_logfile == NULL) { + if (settings.contains("LOGFILE_NAME") && m_logfile == NULL) + { m_logfile = new QFile(settings.value("LOGFILE_NAME").toString()); - } else if (m_logfile == NULL) { + } + else if (m_logfile == NULL) + { m_logfile = new QFile(QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink"); } // Enable logging @@ -93,7 +98,8 @@ void MAVLinkProtocol::loadSettings() // Only set system id if it was valid int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt(); - if (temp > 0 && temp < 256) { + if (temp > 0 && temp < 256) + { systemId = temp; } @@ -123,7 +129,8 @@ void MAVLinkProtocol::storeSettings() settings.setValue("GCS_SYSTEM_ID", systemId); settings.setValue("GCS_AUTH_KEY", m_authKey); settings.setValue("GCS_AUTH_ENABLED", m_authEnabled); - if (m_logfile) { + if (m_logfile) + { // Logfile exists, store the name settings.setValue("LOGFILE_NAME", m_logfile->fileName()); } @@ -139,20 +146,26 @@ void MAVLinkProtocol::storeSettings() MAVLinkProtocol::~MAVLinkProtocol() { storeSettings(); - if (m_logfile) { - if (m_logfile->isOpen()) { + if (m_logfile) + { + if (m_logfile->isOpen()) + { m_logfile->flush(); m_logfile->close(); } delete m_logfile; + m_logfile = NULL; } } QString MAVLinkProtocol::getLogfileName() { - if (m_logfile) { + if (m_logfile) + { return m_logfile->fileName(); - } else { + } + else + { return QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink"; } } @@ -174,7 +187,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) for (int position = 0; position < b.size(); position++) { unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status); - if (decodeState == 1) { + if (decodeState == 1) + { //#ifdef MAVLINK_MESSAGE_LENGTHS // const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS; // if (message.msgid >= sizeof(message_lengths) || @@ -216,7 +230,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) #endif // Log data - if (m_loggingEnabled && m_logfile) { + if (m_loggingEnabled && m_logfile) + { const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64); uint8_t buf[len]; quint64 time = QGC::groundTimeUsecs(); @@ -224,7 +239,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Write message to buffer mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); QByteArray b((const char*)buf, len); - if(m_logfile->write(b) < static_cast(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))) { + if(m_logfile->write(b) < static_cast(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))) + { emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName())); // Stop logging enableLogging(false); @@ -238,7 +254,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); // Check and (if necessary) create UAS object - if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { + if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) + { // ORDER MATTERS HERE! // The UAS object has first to be created and connected, // only then the rest of the application can be made aware @@ -246,7 +263,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // it's first messages. // Check if the UAS has the same id like this system - if (message.sysid == getSystemId()) { + if (message.sysid == getSystemId()) + { emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId())); } @@ -262,9 +280,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_msg_heartbeat_decode(&message, &heartbeat); // Check if the UAS has a different protocol version - if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION)) { + if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION)) + { // Bring up dialog to inform user - if (!versionMismatchIgnore) { + if (!versionMismatchIgnore) + { emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"), tr("It is unsafe to use different MAVLink versions. QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); versionMismatchIgnore = true; @@ -279,28 +299,39 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } // Only count message if UAS exists for this message - if (uas != NULL) { + if (uas != NULL) + { // Increase receive counter totalReceiveCounter++; currReceiveCounter++; qint64 lastLoss = totalLossCounter; // Update last packet index - if (lastIndex[message.sysid][message.compid] == -1) { + if (lastIndex[message.sysid][message.compid] == -1) + { lastIndex[message.sysid][message.compid] = message.seq; - } else { + } + else + { // TODO: This if-else block can (should) be greatly simplified - if (lastIndex[message.sysid][message.compid] == 255) { + if (lastIndex[message.sysid][message.compid] == 255) + { lastIndex[message.sysid][message.compid] = 0; - } else { + } + else + { lastIndex[message.sysid][message.compid]++; } int safeguard = 0; //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq; - while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255) { - if (lastIndex[message.sysid][message.compid] == 255) { + while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255) + { + if (lastIndex[message.sysid][message.compid] == 255) + { lastIndex[message.sysid][message.compid] = 0; - } else { + } + else + { lastIndex[message.sysid][message.compid]++; } totalLossCounter++; @@ -317,7 +348,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) //if () // If a new loss was detected or we just hit one 128th packet step - if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0)) { + if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0)) + { // Calculate new loss ratio // Receive loss float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter); @@ -335,12 +367,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) emit messageReceived(link, message); // Multiplex message if enabled - if (m_multiplexingEnabled) { + if (m_multiplexingEnabled) + { // Get all links connected to this unit QList links = LinkManager::instance()->getLinksForProtocol(this); // Emit message on all links that are currently connected - foreach (LinkInterface* currLink, links) { + foreach (LinkInterface* currLink, links) + { // Only forward this message to the other links, // not the link the message was received on if (currLink != link) sendMessage(currLink, message); @@ -387,7 +421,8 @@ void MAVLinkProtocol::sendMessage(mavlink_message_t message) // Emit message on all links that are currently connected QList::iterator i; - for (i = links.begin(); i != links.end(); ++i) { + for (i = links.begin(); i != links.end(); ++i) + { sendMessage(*i, message); //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size(); } @@ -407,7 +442,8 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message // Write message into buffer, prepending start sign int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected - if (link->isConnected()) { + if (link->isConnected()) + { // Send the portion of the buffer now occupied by the message link->writeBytes((const char*)buffer, len); } @@ -506,25 +542,36 @@ void MAVLinkProtocol::enableLogging(bool enabled) bool changed = false; if (enabled != m_loggingEnabled) changed = true; - if (enabled) { - if (m_logfile && m_logfile->isOpen()) { + if (enabled) + { + if (m_logfile && m_logfile->isOpen()) + { m_logfile->flush(); m_logfile->close(); } - if (m_logfile) { - if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) { + + if (m_logfile) + { + if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) + { emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file. Stopping logging.").arg(m_logfile->fileName())); m_loggingEnabled = false; } } - } else if (!enabled) { - if (m_logfile) { - if (m_logfile->isOpen()) { + else + { + emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot start logging, no logfile selected.")); + } + } + else if (!enabled) + { + if (m_logfile) + { + if (m_logfile->isOpen()) + { m_logfile->flush(); m_logfile->close(); } - delete m_logfile; - m_logfile = NULL; } } m_loggingEnabled = enabled; @@ -533,9 +580,12 @@ void MAVLinkProtocol::enableLogging(bool enabled) void MAVLinkProtocol::setLogfileName(const QString& filename) { - if (!m_logfile) { + if (!m_logfile) + { m_logfile = new QFile(filename); - } else { + } + else + { m_logfile->flush(); m_logfile->close(); } diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 9c35dc87d..ae6a311be 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -161,12 +161,14 @@ void MAVLinkSettingsWidget::chooseLogfileName() { QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink);;")); - if (!fileName.endsWith(".mavlink")) { + if (!fileName.endsWith(".mavlink")) + { fileName.append(".mavlink"); } QFileInfo file(fileName); - if (file.exists() && !file.isWritable()) { + if (file.exists() && !file.isWritable()) + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText(tr("The selected logfile is not writable")); @@ -174,7 +176,9 @@ void MAVLinkSettingsWidget::chooseLogfileName() msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - } else { + } + else + { updateLogfileName(fileName); protocol->setLogfileName(fileName); } @@ -189,12 +193,15 @@ void MAVLinkSettingsWidget::enableDroneOS(bool enable) // Delete from all lists first UDPLink* firstUdp = NULL; QList links = LinkManager::instance()->getLinksForProtocol(protocol); - foreach (LinkInterface* link, links) { + foreach (LinkInterface* link, links) + { UDPLink* udp = dynamic_cast(link); - if (udp) { + if (udp) + { if (!firstUdp) firstUdp = udp; // Remove current hosts - for (int i = 0; i < m_ui->droneOSComboBox->count(); ++i) { + for (int i = 0; i < m_ui->droneOSComboBox->count(); ++i) + { QString oldHostString = m_ui->droneOSComboBox->itemText(i); oldHostString = hostString.split(":").first(); udp->removeHost(oldHostString); @@ -203,8 +210,10 @@ void MAVLinkSettingsWidget::enableDroneOS(bool enable) } // Re-add if enabled - if (enable) { - if (firstUdp) { + if (enable) + { + if (firstUdp) + { firstUdp->addHost(hostString); } // Set key diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index a05aeb5c8..f7982149e 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -21,6 +21,7 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare mavlinkLogFormat(true), binaryBaudRate(57600), isPlaying(false), + currPacketCount(0), ui(new Ui::QGCMAVLinkLogPlayer) { ui->setupUi(this); @@ -102,6 +103,7 @@ void QGCMAVLinkLogPlayer::play() loopTimer.start(interval*accelerationFactor); } isPlaying = true; + ui->logStatsLabel->setText(tr("Started playing..")); ui->playButton->setIcon(QIcon(":images/actions/media-playback-pause.svg")); } else @@ -272,7 +274,8 @@ bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file) minutes -= 60*hours; QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2); - ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel)); + currPacketCount = logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64)); + ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(currPacketCount).arg(timelabel)); } else { @@ -302,6 +305,10 @@ bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file) QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2); ui->logStatsLabel->setText(tr("%2 MB, %4 at %5 KB/s").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(timelabel).arg(binaryBaudRate/10.0f/1024.0f, 0, 'f', 2)); } + + // Reset current state + reset(0); + return true; } } @@ -458,7 +465,7 @@ void QGCMAVLinkLogPlayer::logLoop() // Update status label // Update progress bar - if (loopCounter % 40 == 0) + if (loopCounter % 40 == 0 || currPacketCount < 500) { QFileInfo logFileInfo(logFile); int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast(logFileInfo.size())); diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index b9850a5fa..9e2028ca9 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -78,6 +78,7 @@ protected: bool mavlinkLogFormat; int binaryBaudRate; bool isPlaying; + unsigned int currPacketCount; static const int packetLen = MAVLINK_MAX_PACKET_LEN; static const int timeLen = sizeof(quint64); void changeEvent(QEvent *e); diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 681682db8..00ac31c6d 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -45,7 +45,7 @@ QGCToolBar::QGCToolBar(QWidget *parent) : toggleLoggingAction = new QAction(QIcon(":"), "Logging", this); toggleLoggingAction->setCheckable(true); logReplayAction = new QAction(QIcon(":"), "Replay", this); - logReplayAction->setCheckable(true); + logReplayAction->setCheckable(false); addAction(toggleLoggingAction); addAction(logReplayAction); @@ -143,7 +143,7 @@ void QGCToolBar::logging(bool enabled) } QFileInfo file(fileName); - if (file.exists() && !file.isWritable()) + if ((file.exists() && !file.isWritable())) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); -- 2.22.0