Skip to content

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

Fixed log buttons, replay pausing does not work yet properly, but issue / solution is now known

parent c31ba506
Hide whitespace changes
Inline Side-by-side
Showing with 120 additions and 53 deletions
src/comm/MAVLinkProtocol.cc
View file @ efb95d00
......@@ -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<qint64>(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))) {
if(m_logfile->write(b) < static_cast<qint64>(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<LinkInterface*> 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<LinkInterface*>::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();
}
......
src/ui/MAVLinkSettingsWidget.cc
View file @ efb95d00
......@@ -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<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(protocol);
foreach (LinkInterface* link, links) {
foreach (LinkInterface* link, links)
{
UDPLink* udp = dynamic_cast<UDPLink*>(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
......
src/ui/QGCMAVLinkLogPlayer.cc
View file @ efb95d00
......@@ -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<float>(logFileInfo.size()));
......
src/ui/QGCMAVLinkLogPlayer.h
View file @ efb95d00
......@@ -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);
......
src/ui/QGCToolBar.cc
View file @ efb95d00
......@@ -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);
......
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