Commit d2b18730 authored by Bryan Godbolt's avatar Bryan Godbolt

Merge remote branch 'internal/experimental' into experimental

parents 71bc6364 22658273
......@@ -126,8 +126,7 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
//mainWindow->addLink(simulationLink);
mainWindow = MainWindow::instance();
// Remove splash screen
// Remove splash screen
splashScreen->finish(mainWindow);
// Check if link could be connected
......
/*=====================================================================
/*===================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
......@@ -10,15 +9,15 @@ This file is part of the QGROUNDCONTROL project
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
......@@ -32,6 +31,7 @@ This file is part of the QGROUNDCONTROL project
#include <QTextStream>
#include <QStringList>
#include <QFileInfo>
#include <QList>
#include "LogCompressor.h"
#include <QDebug>
......@@ -56,32 +56,46 @@ void LogCompressor::run()
QFile file(fileName);
QFile outfile(outFileName);
QStringList* keys = new QStringList();
QStringList* times = new QStringList();
QList<quint64> times;// = new QList<quint64>();
QList<quint64> finalTimes;
if (!file.exists()) return;
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
return;
if (outFileName != "")
qDebug() << "LOG COMPRESSOR: Starting" << fileName;
if (!file.exists() || !file.open(QIODevice::ReadOnly | QIODevice::Text))
{
qDebug() << "LOG COMPRESSOR: INPUT FILE DOES NOT EXIST";
emit logProcessingStatusChanged(tr("Log Compressor: Cannot start/compress log file, since input file %1 is not readable").arg(QFileInfo(fileName).absoluteFilePath()));
return;
}
// Check if file is writeable
if (!QFileInfo(outfile).isWritable())
if (outFileName == ""/* || !QFileInfo(outfile).isWritable()*/)
{
qDebug() << "LOG COMPRESSOR: OUTPUT FILE DOES NOT EXIST" << outFileName;
emit logProcessingStatusChanged(tr("Log Compressor: Cannot start/compress log file, since output file %1 is not writable").arg(QFileInfo(outFileName).absoluteFilePath()));
return;
}
}
// Find all keys
QTextStream in(&file);
while (!in.atEnd()) {
// Search only a certain region, assuming that not more
// than N dimensions at H Hertz can be send
const unsigned int keySearchLimit = 15000;
// e.g. 500 Hz * 30 values or
// e.g. 100 Hz * 150 values
unsigned int keyCounter = 0;
while (!in.atEnd() && keyCounter < keySearchLimit) {
QString line = in.readLine();
// Accumulate map of keys
// Data field name is at position 2
QString key = line.split(separator).at(2);
if (!keys->contains(key)) keys->append(key);
keyCounter++;
}
keys->sort();
QString header = "";
QString spacer = "";
for (int i = 0; i < keys->length(); i++)
......@@ -90,48 +104,67 @@ void LogCompressor::run()
spacer += " " + separator;
}
emit logProcessingStatusChanged(tr("Log compressor: Dataset contains dimension: ") + header);
//qDebug() << header;
//qDebug() << "NOW READING TIMES";
// Find all times
//in.reset();
file.reset();
in.reset();
in.resetStatus();
while (!in.atEnd()) {
bool ok;
while (!in.atEnd())
{
QString line = in.readLine();
// Accumulate map of keys
// Data field name is at position 2
QString time = line.split(separator).at(0);
if (!times->contains(time))
// Data field name is at position 2b
quint64 time = static_cast<QString>(line.split(separator).at(0)).toLongLong(&ok);
if (ok)
{
times->append(time);
times.append(time);
}
}
dataLines = times->length();
times->sort();
qSort(times);
qint64 lastTime = -1;
// Create lines
QStringList* outLines = new QStringList();
for (int i = 0; i < times->length(); i++)
for (int i = 0; i < times.length(); i++)
{
outLines->append(times->at(i) + separator + spacer);
// Cast to signed on purpose, 64 bit timestamp still long enough
if (static_cast<qint64>(times.at(i)) != lastTime)
{
outLines->append(QString("%1").arg(times.at(i)) + separator + spacer);
lastTime = static_cast<qint64>(times.at(i));
finalTimes.append(times.at(i));
//qDebug() << "ADDED:" << outLines->last();
}
}
dataLines = finalTimes.length();
emit logProcessingStatusChanged(tr("Log compressor: Now processing %1 log lines").arg(finalTimes.length()));
// Fill in the values for all keys
file.reset();
QTextStream data(&file);
int linecounter = 0;
while (!data.atEnd()) {
quint64 lastTimeIndex = 0;
bool failed = false;
while (!data.atEnd())
{
linecounter++;
currentDataLine = linecounter;
QString line = data.readLine();
QStringList parts = line.split(separator);
// Get time
QString time = parts.first();
quint64 time = static_cast<QString>(parts.first()).toLongLong(&ok);
QString field = parts.at(2);
QString value = parts.at(3);
// Enforce NaN if no value is present
......@@ -140,20 +173,66 @@ void LogCompressor::run()
value = "NaN";
}
// Get matching output line
quint64 index = times->indexOf(time);
QString outLine = outLines->at(index);
QStringList outParts = outLine.split(separator);
// Replace measurement placeholder with current value
outParts.replace(keys->indexOf(field)+1, value);
outLine = outParts.join(separator);
outLines->replace(index, outLine);
}
// Constraining the search area might result in not finding a key,
// but it significantly reduces the time needed for the search
// setting a window of 1000 entries means that a 1 Hz data point
// can still be located
quint64 offsetLimit = 100;
quint64 offset;
qint64 index = -1;
failed = false;
// Search the index until it is valid (!= -1)
// or the start of the list has been reached (failed)
while (index == -1 && !failed)
{
if (lastTimeIndex > offsetLimit)
{
offset = lastTimeIndex - offsetLimit;
}
else
{
offset = 0;
}
index = finalTimes.indexOf(time, offset);
if (index == -1)
{
if (offset == 0)
{
emit logProcessingStatusChanged(tr("Log compressor: Timestamp %1 not found in dataset, ignoring log line %2").arg(time).arg(linecounter));
qDebug() << "Completely failed finding value";
//continue;
failed = true;
}
else
{
emit logProcessingStatusChanged(tr("Log compressor: Timestamp %1 not found in dataset, restarting search.").arg(time));
offsetLimit*=2;
}
}
}
if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines));
if (!failed)
{
// When the algorithm reaches here the correct index was found
lastTimeIndex = index;
QString outLine = outLines->at(index);
QStringList outParts = outLine.split(separator);
// Replace measurement placeholder with current value
outParts.replace(keys->indexOf(field)+1, value);
outLine = outParts.join(separator);
outLines->replace(index, outLine);
}
}
// Add header, write out file
file.close();
if (outFileName == "")
{
QFile::remove(file.fileName());
......@@ -163,18 +242,19 @@ void LogCompressor::run()
return;
outfile.write(QString(QString("unix_timestamp") + separator + header.replace(" ", "_") + QString("\n")).toLatin1());
//QString fileHeader = QString("unix_timestamp") + header.replace(" ", "_") + QString("\n");
// File output
for (int i = 0; i < outLines->length(); i++)
{
//qDebug() << outLines->at(i);
outfile.write(QString(outLines->at(i) + "\n").toLatin1());
}
currentDataLine = 0;
dataLines = 1;
delete keys;
emit logProcessingStatusChanged(tr("Log compressor: Finished processing file: %1").arg(outfile.fileName()));
qDebug() << "Done with logfile processing";
emit finishedFile(outfile.fileName());
running = false;
......
......@@ -27,6 +27,7 @@ signals:
/** @brief This signal is emitted once a logfile has been finished writing
* @param fileName The name out the output (CSV) file
*/
void logProcessingStatusChanged(QString);
void finishedFile(QString fileName);
};
......
......@@ -59,7 +59,9 @@ MAVLinkProtocol::MAVLinkProtocol() :
heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
m_heartbeatsEnabled(false),
m_loggingEnabled(false),
m_logfile(NULL)
m_logfile(NULL),
m_enable_version_check(true),
versionMismatchIgnore(false)
{
start(QThread::LowPriority);
// Start heartbeat timer, emitting a heartbeat at the configured rate
......@@ -76,6 +78,8 @@ MAVLinkProtocol::MAVLinkProtocol() :
lastIndex[i][j] = -1;
}
}
emit versionCheckChanged(m_enable_version_check);
}
MAVLinkProtocol::~MAVLinkProtocol()
......@@ -91,7 +95,6 @@ MAVLinkProtocol::~MAVLinkProtocol()
void MAVLinkProtocol::run()
{
}
QString MAVLinkProtocol::getLogfileName()
......@@ -146,7 +149,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// Check if the UAS has the same id like this system
if (message.sysid == getSystemId())
{
qDebug() << "WARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\n\n RECEIVED MESSAGE FROM THIS SYSTEM WITH ID" << message.msgid << "FROM COMPONENT" << message.compid;
emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
}
// Create a new UAS based on the heartbeat received
......@@ -161,16 +164,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
mavlink_msg_heartbeat_decode(&message, &heartbeat);
// Check if the UAS has a different protocol version
if (heartbeat.mavlink_version != MAVLINK_VERSION)
if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
{
// Bring up dialog to inform user
QMessageBox msgBox(MainWindow::instance());
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"));
msgBox.setInformativeText(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));
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
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;
}
// Ignore this message and continue gracefully
continue;
......@@ -401,6 +403,11 @@ void MAVLinkProtocol::enableLogging(bool enabled)
m_loggingEnabled = enabled;
}
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
m_enable_version_check = enabled;
}
bool MAVLinkProtocol::heartbeatsEnabled(void)
{
return m_heartbeatsEnabled;
......@@ -411,6 +418,11 @@ bool MAVLinkProtocol::loggingEnabled(void)
return m_loggingEnabled;
}
bool MAVLinkProtocol::versionCheckEnabled(void)
{
return m_enable_version_check;
}
/**
* The default rate is 1 Hertz.
*
......
......@@ -68,6 +68,8 @@ public:
bool heartbeatsEnabled(void);
/** @brief Get logging state */
bool loggingEnabled(void);
/** @brief Get protocol version check state */
bool versionCheckEnabled(void);
/** @brief Get the name of the packet log file */
static QString getLogfileName();
......@@ -87,6 +89,9 @@ public slots:
/** @brief Enable/disable binary packet logging */
void enableLogging(bool enabled);
/** @brief Enable / disable version check */
void enableVersionCheck(bool enabled);
/** @brief Send an extra heartbeat to all connected units */
void sendHeartbeat();
......@@ -96,12 +101,14 @@ protected:
bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission
bool m_loggingEnabled; ///< Enable/disable packet logging
QFile* m_logfile; ///< Logfile
bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC
QMutex receiveMutex; ///< Mutex to protect receiveBytes function
int lastIndex[256][256];
int totalReceiveCounter;
int totalLossCounter;
int currReceiveCounter;
int currLossCounter;
bool versionMismatchIgnore;
signals:
/** @brief Message received and directly copied via signal */
......@@ -110,6 +117,10 @@ signals:
void heartbeatChanged(bool heartbeats);
/** @brief Emitted if logging is started / stopped */
void loggingChanged(bool enabled);
/** @brief Emitted if version check is enabled / disabled */
void versionCheckChanged(bool enabled);
/** @brief Emitted if a message from the protocol should reach the user */
void protocolStatusMessage(const QString& title, const QString& message);
};
#endif // MAVLINKPROTOCOL_H_
......@@ -61,8 +61,8 @@ namespace OpalRT
const QGCParamID& getParamID() const {return *paramID;}
void setOpalID(unsigned short opalID) {this->opalID = opalID;}
const QString& getSimulinkPath() {return *simulinkPath;}
const QString& getSimulinkName() {return *simulinkName;}
const QString& getSimulinkPath() const {return *simulinkPath;}
const QString& getSimulinkName() const {return *simulinkName;}
uint8_t getComponentID() const {return componentID;}
float getValue();
void setValue(float value);
......
......@@ -53,11 +53,11 @@ namespace OpalRT
const_iterator(const const_iterator& other);
const_iterator& operator+=(int i) {index += i; return *this;}
bool operator<(const const_iterator& other) {return (this->paramList == other.paramList)
bool operator<(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index<other.index);}
bool operator==(const const_iterator& other) {return (this->paramList == other.paramList)
bool operator==(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index==other.index);}
bool operator!=(const const_iterator& other) {return !((*this) == other);}
bool operator!=(const const_iterator& other) const {return !((*this) == other);}
const Parameter& operator*() const {return paramList[index];}
const Parameter* operator->() const {return &paramList[index];}
......
......@@ -85,6 +85,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
#endif
loadSettings();
// Link is setup, register it with link manager
LinkManager::instance()->add(this);
}
......
......@@ -118,6 +118,21 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "vis. z", pos.z, time);
}
break;
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
{
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
//emit valueChanged(uasId, "vis. time", pos.usec, time);
emit valueChanged(uasId, "vicon roll", pos.roll, time);
emit valueChanged(uasId, "vicon pitch", pos.pitch, time);
emit valueChanged(uasId, "vicon yaw", pos.yaw, time);
emit valueChanged(uasId, "vicon x", pos.x, time);
emit valueChanged(uasId, "vicon y", pos.y, time);
emit valueChanged(uasId, "vicon z", pos.z, time);
emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
}
break;
case MAVLINK_MSG_ID_AUX_STATUS:
{
mavlink_aux_status_t status;
......
......@@ -37,15 +37,19 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget
{
m_ui->setupUi(this);
// Initialize state
m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled());
m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled());
m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled());
// Connect actions
connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool)));
connect(m_ui->heartbeatCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableHeartbeats(bool)));
connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->loggingCheckBox, SLOT(setChecked(bool)));
connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool)));
connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionCheckBox, SLOT(setChecked(bool)));
connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableVersionCheck(bool)));
// Initialize state
m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled());
m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled());
}
MAVLinkSettingsWidget::~MAVLinkSettingsWidget()
......
......@@ -31,6 +31,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="versionCheckBox">
<property name="text">
<string>Only accept MAVs with same protocol version</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
......
......@@ -120,6 +120,8 @@ MainWindow::MainWindow(QWidget *parent):
// Load mavlink view as default widget set
//loadMAVLinkView();
statusBar()->setSizeGripEnabled(true);
if (settings.contains("geometry"))
{
// Restore the window geometry
......@@ -144,14 +146,14 @@ MainWindow::MainWindow(QWidget *parent):
MainWindow::~MainWindow()
{
delete statusBar;
statusBar = NULL;
}
void MainWindow::buildCommonWidgets()
{
//TODO: move protocol outside UI
mavlink = new MAVLinkProtocol();
connect(mavlink, SIGNAL(protocolStatusMessage(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection);
// Dock widgets
if (!controlDockWidget)
......@@ -201,6 +203,12 @@ void MainWindow::buildCommonWidgets()
protocolWidget = new XMLCommProtocolWidget(this);
addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL);
}
if (!dataplotWidget)
{
dataplotWidget = new QGCDataPlot2D(this);
addToCentralWidgetsMenu (dataplotWidget, "Data Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT);
}
}
void MainWindow::buildPxWidgets()
......@@ -741,6 +749,7 @@ void MainWindow::arrangeCommonCenterStack()
if (!centerStack) return;
if (mapWidget && (centerStack->indexOf(mapWidget) == -1)) centerStack->addWidget(mapWidget);
if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget);
if (protocolWidget && (centerStack->indexOf(protocolWidget) == -1)) centerStack->addWidget(protocolWidget);
setCentralWidget(centerStack);
......@@ -811,15 +820,6 @@ void MainWindow::configureWindowName()
#endif
}
QStatusBar* MainWindow::createStatusBar()
{
QStatusBar* bar = new QStatusBar();
/* Add status fields and messages */
/* Enable resize grip in the bottom right corner */
bar->setSizeGripEnabled(true);
return bar;
}
void MainWindow::startVideoCapture()
{
QString format = "bmp";
......@@ -882,17 +882,48 @@ void MainWindow::reloadStylesheet()
delete styleSheet;
}
/**
* The status message will be overwritten if a new message is posted to this function
*
* @param status message text
* @param timeout how long the status should be displayed
*/
void MainWindow::showStatusMessage(const QString& status, int timeout)
{
Q_UNUSED(status);
Q_UNUSED(timeout);
//statusBar->showMessage(status, timeout);
statusBar()->showMessage(status, timeout);
}
/**
* The status message will be overwritten if a new message is posted to this function.
* it will be automatically hidden after 5 seconds.
*
* @param status message text
*/
void MainWindow::showStatusMessage(const QString& status)
{
Q_UNUSED(status);
//statusBar->showMessage(status, 5);
statusBar()->showMessage(status, 20000);
}
void MainWindow::showCriticalMessage(const QString& title, const QString& message)
{
QMessageBox msgBox(MainWindow::instance());
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText(title);
msgBox.setInformativeText(message);
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
}
void MainWindow::showInfoMessage(const QString& title, const QString& message)
{
QMessageBox msgBox(MainWindow::instance());
msgBox.setIcon(QMessageBox::Information);
msgBox.setText(title);
msgBox.setInformativeText(message);
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
}
/**
......
......@@ -92,24 +92,15 @@ public slots:
// /** @brief Store the mainwindow settings */
// void storeSettings();
/**
* @brief Shows a status message on the bottom status bar
*
* The status message will be overwritten if a new message is posted to this function
*
* @param status message text
* @param timeout how long the status should be displayed
*/
/** @brief Shows a status message on the bottom status bar */
void showStatusMessage(const QString& status, int timeout);
/**
* @brief Shows a status message on the bottom status bar
*
* The status message will be overwritten if a new message is posted to this function.
* it will be automatically hidden after 5 seconds.
*
* @param status message text
*/
/** @brief Shows a status message on the bottom status bar */
void showStatusMessage(const QString& status);
/** @brief Shows a critical message as popup or as widget */
void showCriticalMessage(const QString& title, const QString& message);
/** @brief Shows an info message as popup or as widget */
void showInfoMessage(const QString& title, const QString& message);
void addLink();
void addLink(LinkInterface* link);
void configure();
......@@ -307,10 +298,6 @@ protected:
VIEW_SECTIONS currentView;
bool aboutToCloseFlag;
QStatusBar* statusBar;
QStatusBar* createStatusBar();
void clearView();
void buildCommonWidgets();
......
......@@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCDataPlot2D.h"
#include "ui_QGCDataPlot2D.h"
#include "MG.h"
#include "MainWindow.h"
#include <cmath>
#include <QDebug>
......@@ -75,7 +76,7 @@ void QGCDataPlot2D::reloadFile()
{
if (QFileInfo(fileName).isReadable())
{
if (ui->inputFileType->currentText().contains("pxIMU"))
if (ui->inputFileType->currentText().contains("pxIMU") || ui->inputFileType->currentText().contains("RAW"))
{
loadRawLog(fileName, ui->xAxis->currentText(), ui->yAxis->text());
}
......@@ -88,9 +89,10 @@ void QGCDataPlot2D::reloadFile()
void QGCDataPlot2D::loadFile()
{
qDebug() << "DATA PLOT: Loading file:" << fileName;
if (QFileInfo(fileName).isReadable())
{
if (ui->inputFileType->currentText().contains("pxIMU"))
if (ui->inputFileType->currentText().contains("pxIMU") || ui->inputFileType->currentText().contains("RAW"))
{
loadRawLog(fileName);
}
......@@ -110,7 +112,7 @@ void QGCDataPlot2D::loadFile(QString file)
{
loadRawLog(fileName);
}
else if (fileName.contains(".txt") || fileName.contains(".csv"))
else if (fileName.contains(".txt") || fileName.contains(".csv") || fileName.contains(".csv"))
{
loadCsvLog(fileName);
}
......@@ -129,7 +131,7 @@ void QGCDataPlot2D::savePlot()
if (!fileName.contains("."))
{
// .csv is default extension
// .pdf is default extension
fileName.append(".pdf");
}
......@@ -269,7 +271,16 @@ void QGCDataPlot2D::selectFile()
// Let user select the log file name
//QDate date(QDate::currentDate());
// QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log")
fileName = QFileDialog::getOpenFileName(this, tr("Specify log file name"), QString(), "Logfile (*.csv *.txt *.log)");
if (ui->inputFileType->currentText().contains("pxIMU") || ui->inputFileType->currentText().contains("RAW"))
{
fileName = QFileDialog::getOpenFileName(this, tr("Specify log file name"), QString(), "Logfile (*.imu *.raw)");
}
else
{
fileName = QFileDialog::getOpenFileName(this, tr("Specify log file name"), QString(), "Logfile (*.csv *.txt *.log)");
}
// Store reference to file
QFileInfo fileInfo(fileName);
......@@ -296,32 +307,36 @@ void QGCDataPlot2D::selectFile()
void QGCDataPlot2D::loadRawLog(QString file, QString xAxisName, QString yAxisFilter)
{
qDebug() << "LOADING RAW LOG!";
if (logFile != NULL)
{
logFile->close();
delete logFile;
}
// Postprocess log file
logFile = new QTemporaryFile();
logFile = new QTemporaryFile("qt_qgc_temp_log.XXXXXX.csv");
compressor = new LogCompressor(file, logFile->fileName());
connect(compressor, SIGNAL(logProcessingStatusChanged(QString)), MainWindow::instance(), SLOT(showStatusMessage(QString)));
connect(compressor, SIGNAL(finishedFile(QString)), this, SLOT(loadFile(QString)));
compressor->startCompression();
// Block UI
QProgressDialog progress("Transforming RAW log file to CSV", "Abort Transformation", 0, 1, this);
progress.setWindowModality(Qt::WindowModal);
// // Block UI
// QProgressDialog progress("Transforming RAW log file to CSV", "Abort Transformation", 0, 1, this);
// progress.setWindowModality(Qt::WindowModal);
while (!compressor->isFinished())
{
MG::SLEEP::usleep(100000);
progress.setMaximum(compressor->getDataLines());
progress.setValue(compressor->getCurrentLine());
}
// Enforce end
progress.setMaximum(compressor->getDataLines());
progress.setValue(compressor->getDataLines());
// while (!compressor->isFinished())
// {
// MG::SLEEP::usleep(100000);
//// progress.setMaximum(compressor->getDataLines());
//// progress.setValue(compressor->getCurrentLine());
// }
// // Enforce end
// progress.setMaximum(compressor->getDataLines());
// progress.setValue(compressor->getDataLines());
// Done with preprocessing - now load csv log
loadCsvLog(logFile->fileName(), xAxisName, yAxisFilter);
//loadFile(logFile->fileName());
}
/**
......@@ -654,6 +669,13 @@ void QGCDataPlot2D::saveCsvLog()
fileName = QFileDialog::getSaveFileName(
this, "Export CSV File Name", QDesktopServices::storageLocation(QDesktopServices::DesktopLocation),
"CSV file (*.csv);;Text file (*.txt)");
if (!fileName.contains("."))
{
// .csv is default extension
fileName.append(".csv");
}
// QFileInfo fileInfo(fileName);
//
// // Check if we could create a new file in this directory
......
......@@ -48,18 +48,14 @@ const float* RadioCalibrationData::operator [](int i) const
return NULL;
}
const QVector<float>& RadioCalibrationData::operator ()(int i) const
const QVector<float>& RadioCalibrationData::operator ()(const int i) const throw(std::out_of_range)
{
if (i < data->size())
if ((i < data->size()) && (i >=0))
{
return (*data)[i];
}
// FIXME Bryan
// FIXME James
// This is not good. If it is ever used after being returned it will cause a crash
// return QVector<float>();
throw std::out_of_range("Invalid channel index");
}
QString RadioCalibrationData::toString(RadioElement element) const
......
......@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project
#include <QDebug>
#include <QVector>
#include <QString>
#include <stdexcept>
/**
......@@ -66,7 +67,7 @@ public:
};
const float* operator[](int i) const;
const QVector<float>& operator()(int i) const;
const QVector<float>& operator()(int i) const throw(std::out_of_range);
void set(int element, int index, float value) {(*data)[element][index] = value;}
public slots:
......
......@@ -52,38 +52,38 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
setUASId(0);
}
void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
{
/** this expects a particular channel to function mapping
\todo allow run-time channel mapping
*/
switch (ch)
{
case 0:
aileron->channelChanged(raw);
break;
case 1:
elevator->channelChanged(raw);
break;
case 2:
throttle->channelChanged(raw);
break;
case 3:
rudder->channelChanged(raw);
break;
case 4:
gyro->channelChanged(raw);
break;
case 5:
pitch->channelChanged(raw);
break;
//void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
//{
// /** this expects a particular channel to function mapping
// \todo allow run-time channel mapping
// */
// switch (ch)
// {
// case 0:
// aileron->channelChanged(raw);
// break;
// case 1:
// elevator->channelChanged(raw);
// break;
// case 2:
// throttle->channelChanged(raw);
// break;
// case 3:
// rudder->channelChanged(raw);
// break;
// case 4:
// gyro->channelChanged(raw);
// break;
// case 5:
// pitch->channelChanged(raw);
// break;
}
}
// }
//}
void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
{
//void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
//{
// FIXME James
// FIXME Bryan
......@@ -113,9 +113,9 @@ void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
// }
}
//}
void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized)
void RadioCalibrationWindow::setChannel(int ch, float raw)
{
/** this expects a particular channel to function mapping
\todo allow run-time channel mapping
......
......@@ -66,9 +66,10 @@ public:
explicit RadioCalibrationWindow(QWidget *parent = 0);
public slots:
void setChannel(int ch, float raw, float normalized);
void setChannelRaw(int ch, float raw);
void setChannelScaled(int ch, float normalized);
void setChannel(int ch, float raw);
// @todo remove these functions if they are not needed - were added by lm on dec 14, 2010
// void setChannelRaw(int ch, float raw);
// void setChannelScaled(int ch, float normalized);
void loadFile();
void saveFile();
void send();
......
......@@ -48,6 +48,7 @@ This file is part of the PIXHAWK project
#include "LinechartWidget.h"
#include "LinechartPlot.h"
#include "LogCompressor.h"
#include "MainWindow.h"
#include "QGC.h"
#include "MG.h"
......@@ -348,7 +349,7 @@ void LinechartWidget::startLogging()
// Let user select the log file name
QDate date(QDate::currentDate());
// QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log")
QString fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.csv, *.txt);;"));
QString fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.csv *.txt);;"));
if (!fileName.contains("."))
{
......@@ -399,6 +400,8 @@ void LinechartWidget::stopLogging()
// Postprocess log file
compressor = new LogCompressor(logFile->fileName());
connect(compressor, SIGNAL(finishedFile(QString)), this, SIGNAL(logfileWritten(QString)));
connect(compressor, SIGNAL(logProcessingStatusChanged(QString)), MainWindow::instance(), SLOT(showStatusMessage(QString)));
MainWindow::instance()->showInfoMessage("Logging ended", "QGroundControl is now compressing the logfile in a consistent CVS file. This may take a while, you can continue to use QGroundControl. Status updates appear at the bottom of the window.");
compressor->startCompression();
}
logButton->setText(tr("Start logging"));
......
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