Commit 94348cb7 authored by pixhawk's avatar pixhawk

Added not yet working version of logging / log replay, fixed ALL bugs in main...

Added not yet working version of logging / log replay, fixed ALL bugs in main window, enabled full persistence in main window and serial link.
parent b54faa4b
......@@ -153,7 +153,8 @@ FORMS += src/ui/MainWindow.ui \
src/ui/uas/QGCUnconnectedInfoWidget.ui \
src/ui/designer/QGCToolWidget.ui \
src/ui/designer/QGCParamSlider.ui \
src/ui/designer/QGCActionButton.ui
src/ui/designer/QGCActionButton.ui \
src/ui/QGCMAVLinkLogPlayer.ui
INCLUDEPATH += src \
src/ui \
......@@ -259,7 +260,8 @@ HEADERS += src/MG.h \
src/ui/designer/QGCToolWidget.h \
src/ui/designer/QGCParamSlider.h \
src/ui/designer/QGCActionButton.h \
src/ui/designer/QGCToolWidgetItem.h
src/ui/designer/QGCToolWidgetItem.h \
src/ui/QGCMAVLinkLogPlayer.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|win32-msvc2008: {
......@@ -380,7 +382,8 @@ SOURCES += src/main.cc \
src/ui/designer/QGCToolWidget.cc \
src/ui/designer/QGCParamSlider.cc \
src/ui/designer/QGCActionButton.cc \
src/ui/designer/QGCToolWidgetItem.cc
src/ui/designer/QGCToolWidgetItem.cc \
src/ui/QGCMAVLinkLogPlayer.cc
macx|win32-msvc2008: {
SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
......@@ -3,6 +3,7 @@
#include <QDateTime>
#include <QColor>
#include <QThread>
namespace QGC
{
......@@ -20,6 +21,36 @@ namespace QGC
const QString APPNAME = "QGROUNDCONTROL";
const QString COMPANYNAME = "OPENMAV";
const int APPLICATIONVERSION = 80; // 0.8.0
class SLEEP : public QThread
{
public:
/**
* @brief Set a thread to sleep for seconds
* @param s time in seconds to sleep
**/
static void sleep(unsigned long s)
{
QThread::sleep(s);
}
/**
* @brief Set a thread to sleep for milliseconds
* @param ms time in milliseconds to sleep
**/
static void msleep(unsigned long ms)
{
QThread::msleep(ms);
}
/**
* @brief Set a thread to sleep for microseconds
* @param us time in microseconds to sleep
**/
static void usleep(unsigned long us)
{
QThread::usleep(us);
}
};
}
#define QGC_EVENTLOOP_DEBUG 0
......
......@@ -59,7 +59,7 @@ MAVLinkProtocol::MAVLinkProtocol() :
heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
m_heartbeatsEnabled(false),
m_loggingEnabled(false),
m_logfile(NULL),
m_logfile(new QFile(QCoreApplication::applicationDirPath()+"/mavlink.log")),
m_enable_version_check(true),
versionMismatchIgnore(false)
{
......@@ -99,7 +99,7 @@ void MAVLinkProtocol::run()
QString MAVLinkProtocol::getLogfileName()
{
return QCoreApplication::applicationDirPath()+"/mavlink.log";
return m_logfile->fileName();
}
/**
......@@ -391,21 +391,27 @@ void MAVLinkProtocol::enableLogging(bool enabled)
{
if (enabled && !m_loggingEnabled)
{
m_logfile = new QFile(getLogfileName());
if (m_logfile->isOpen()) m_logfile->close();
m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
}
else
else if (!enabled)
{
m_logfile->close();
delete m_logfile;
m_logfile = NULL;
}
m_loggingEnabled = enabled;
emit loggingChanged(enabled);
}
void MAVLinkProtocol::setLogfileName(const QString& filename)
{
m_logfile->close();
m_logfile->setFileName(filename);
}
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
m_enable_version_check = enabled;
emit versionCheckChanged(enabled);
}
bool MAVLinkProtocol::heartbeatsEnabled(void)
......
......@@ -70,8 +70,10 @@ public:
bool loggingEnabled(void);
/** @brief Get protocol version check state */
bool versionCheckEnabled(void);
/** @brief Get the protocol version */
int getVersion() { return MAVLINK_VERSION; }
/** @brief Get the name of the packet log file */
static QString getLogfileName();
QString getLogfileName();
public slots:
/** @brief Receive bytes from a communication interface */
......@@ -89,6 +91,9 @@ public slots:
/** @brief Enable/disable binary packet logging */
void enableLogging(bool enabled);
/** @brief Set log file name */
void setLogfileName(const QString& filename);
/** @brief Enable / disable version check */
void enableVersionCheck(bool enabled);
......
......@@ -97,8 +97,8 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
LinkManager::instance()->add(this);
// Open packet log
mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
mavlinkLogFile->open(QIODevice::ReadOnly);
mavlinkLogFile = new QFile();
//mavlinkLogFile->open(QIODevice::ReadOnly);
}
MAVLinkSimulationLink::~MAVLinkSimulationLink()
......@@ -141,7 +141,7 @@ void MAVLinkSimulationLink::run()
}
last = MG::TIME::getGroundTimeNow();
}
MG::SLEEP::msleep(2);
MG::SLEEP::msleep(3);
}
}
......
......@@ -108,8 +108,9 @@ void SerialLink::loadSettings()
setPortName(settings.value("SERIALLINK_COMM_PORT").toString());
setBaudRateType(settings.value("SERIALLINK_COMM_BAUD").toInt());
setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt());
setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt());
setDataBitsType(settings.value("SERIALLINK_COMM_DATABITS").toInt());
setStopBits(settings.value("SERIALLINK_COMM_STOPBITS").toInt());
setDataBits(settings.value("SERIALLINK_COMM_DATABITS").toInt());
setFlowType(settings.value("SERIALLINK_COMM_FLOW_CONTROL").toInt());
}
}
......@@ -120,8 +121,9 @@ void SerialLink::writeSettings()
settings.setValue("SERIALLINK_COMM_PORT", this->porthandle);
settings.setValue("SERIALLINK_COMM_BAUD", getBaudRateType());
settings.setValue("SERIALLINK_COMM_PARITY", getParityType());
settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType());
settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType());
settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBits());
settings.setValue("SERIALLINK_COMM_DATABITS", getDataBits());
settings.setValue("SERIALLINK_COMM_FLOW_CONTROL", getFlowType());
settings.sync();
}
......@@ -316,10 +318,10 @@ bool SerialLink::hardwareConnect()
{
emit connected();
emit connected(true);
writeSettings();
}
writeSettings();
return connectionUp;
}
......@@ -531,6 +533,48 @@ int SerialLink::getStopBitsType()
return stopBits;
}
int SerialLink::getDataBits()
{
int ret;
switch (dataBits)
{
case DATA_5:
ret = 5;
break;
case DATA_6:
ret = 6;
break;
case DATA_7:
ret = 7;
break;
case DATA_8:
ret = 8;
break;
default:
ret = 0;
break;
}
return ret;
}
int SerialLink::getStopBits()
{
int ret;
switch (stopBits)
{
case STOP_1:
ret = 1;
break;
case STOP_2:
ret = 2;
break;
default:
ret = 0;
break;
}
return ret;
}
bool SerialLink::setPortName(QString portName)
{
if(portName.trimmed().length() > 0)
......@@ -845,8 +889,7 @@ bool SerialLink::setParityType(int parity)
}
// FIXME Works not as anticipated by user!
bool SerialLink::setDataBitsType(int dataBits)
bool SerialLink::setDataBits(int dataBits)
{
bool accepted = true;
......@@ -879,12 +922,12 @@ bool SerialLink::setDataBitsType(int dataBits)
return accepted;
}
// FIXME WORKS NOT AS ANTICIPATED BY USER!
bool SerialLink::setStopBitsType(int stopBits)
bool SerialLink::setStopBits(int stopBits)
{
bool reconnect = false;
bool accepted = true;
if(isConnected()) {
if(isConnected())
{
disconnect();
reconnect = true;
}
......@@ -907,3 +950,51 @@ bool SerialLink::setStopBitsType(int stopBits)
if(reconnect) connect();
return accepted;
}
bool SerialLink::setDataBitsType(int dataBits)
{
bool reconnect = false;
bool accepted = false;
if (isConnected())
{
disconnect();
reconnect = true;
}
if (dataBits >= (int)DATA_5 && dataBits <= (int)DATA_8)
{
DataBitsType newBits = (DataBitsType) dataBits;
port->setDataBits(newBits);
if(reconnect)
{
connect();
}
accepted = true;
}
return accepted;
}
bool SerialLink::setStopBitsType(int stopBits)
{
bool reconnect = false;
bool accepted = false;
if(isConnected())
{
disconnect();
reconnect = true;
}
if (stopBits >= (int)STOP_1 && dataBits <= (int)STOP_2)
{
StopBitsType newBits = (StopBitsType) stopBits;
port->setStopBits(newBits);
accepted = true;
}
if(reconnect) connect();
return accepted;
}
......@@ -74,6 +74,10 @@ public:
*/
QString getName();
int getBaudRate();
int getDataBits();
int getStopBits();
// ENUM values
int getBaudRateType();
int getFlowType();
int getParityType();
......@@ -103,6 +107,10 @@ public:
public slots:
bool setPortName(QString portName);
bool setBaudRate(int rate);
bool setDataBits(int dataBits);
bool setStopBits(int stopBits);
// Set ENUM values
bool setBaudRateType(int rateIndex);
bool setFlowType(int flow);
bool setParityType(int parity);
......
......@@ -43,6 +43,9 @@ class SerialLinkInterface : public LinkInterface {
public:
virtual QString getPortName() = 0;
virtual int getBaudRate() = 0;
virtual int getDataBits() = 0;
virtual int getStopBits() = 0;
virtual int getBaudRateType() = 0;
virtual int getFlowType() = 0;
virtual int getParityType() = 0;
......
......@@ -49,7 +49,7 @@ UDPLink::UDPLink(QHostAddress host, quint16 port)
// Set unique ID and add link to the list of links
this->id = getNextLinkId();
this->name = tr("UDP link ") + QString::number(getId());
this->name = tr("UDP Link (port:%1)").arg(14550);
LinkManager::instance()->add(this);
}
......
......@@ -663,12 +663,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_nav_filter_bias_t bias;
mavlink_msg_nav_filter_bias_decode(&message, &bias);
quint64 time = MG::TIME::getGroundTimeNow();
emit valueChanged(uasId, "b_f[0]", bias.accel_0, time);
emit valueChanged(uasId, "b_f[1]", bias.accel_1, time);
emit valueChanged(uasId, "b_f[2]", bias.accel_2, time);
emit valueChanged(uasId, "b_w[0]", bias.gyro_0, time);
emit valueChanged(uasId, "b_w[1]", bias.gyro_1, time);
emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time);
emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time);
emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time);
emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time);
emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time);
emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time);
emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time);
}
break;
case MAVLINK_MSG_ID_RADIO_CALIBRATION:
......
......@@ -55,6 +55,7 @@ void UASWaypointManager::timeout()
{
protocol_timer.start(PROTOCOL_TIMEOUT_MS);
current_retries--;
emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries));
qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
if (current_state == WP_GETLIST)
{
......@@ -517,7 +518,7 @@ void UASWaypointManager::sendWaypointClearAll()
wpca.target_system = uas.getUASID();
wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
emit updateStatusString(QString("clearing waypoint list..."));
emit updateStatusString(QString("Clearing waypoint list..."));
mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
uas.sendMessage(message);
......@@ -554,7 +555,7 @@ void UASWaypointManager::sendWaypointCount()
wpc.count = current_count;
qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
emit updateStatusString(QString("start transmitting waypoints..."));
emit updateStatusString(QString("Starting to transmit waypoints..."));
mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
uas.sendMessage(message);
......@@ -571,7 +572,7 @@ void UASWaypointManager::sendWaypointRequestList()
wprl.target_system = uas.getUASID();
wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
emit updateStatusString(QString("requesting waypoint list..."));
emit updateStatusString(QString("Requesting waypoint list..."));
mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
uas.sendMessage(message);
......@@ -591,7 +592,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
wpr.seq = seq;
emit updateStatusString(QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
uas.sendMessage(message);
......@@ -612,7 +613,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
wp->target_system = uas.getUASID();
wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;
emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
uas.sendMessage(message);
......
......@@ -106,7 +106,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox);
layout->addWidget(conf);
ui.linkGroupBox->setLayout(layout);
ui.linkGroupBox->setTitle(tr("serial link"));
ui.linkGroupBox->setTitle(tr("Serial Link"));
//ui.linkGroupBox->setTitle(link->getName());
//connect(link, SIGNAL(nameChanged(QString)), ui.linkGroupBox, SLOT(setTitle(QString)));
}
......@@ -142,13 +142,13 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
// Open details pane for MAVLink if necessary
MAVLinkProtocol* mavlink = dynamic_cast<MAVLinkProtocol*>(protocol);
if(mavlink != 0)
if (mavlink != 0)
{
QWidget* conf = new MAVLinkSettingsWidget(mavlink, this);
QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.protocolGroupBox);
layout->addWidget(conf);
ui.protocolGroupBox->setLayout(layout);
ui.protocolGroupBox->setTitle(protocol->getName());
ui.protocolGroupBox->setTitle(protocol->getName()+" (Global Settings)");
}
else
{
......
......@@ -105,6 +105,8 @@ DebugConsole::DebugConsole(QWidget *parent) :
connect(m_ui->holdButton, SIGNAL(toggled(bool)), this, SLOT(hold(bool)));
// Connect connect button
connect(m_ui->connectButton, SIGNAL(clicked()), this, SLOT(handleConnectButton()));
// Connect the special chars combo box
connect(m_ui->specialComboBox, SIGNAL(activated(QString)), this, SLOT(appendSpecialSymbol(QString)));
this->setVisible(false);
}
......@@ -327,8 +329,78 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes)
}
}
QByteArray DebugConsole::symbolNameToBytes(const QString& text)
{
QByteArray b;
if (text == "LF")
{
b.append(static_cast<char>(0x0A));
}
else if (text == "FF")
{
b.append(static_cast<char>(0x0C));
}
else if (text == "CR")
{
b.append(static_cast<char>(0x0D));
}
else if (text == "CR+LF")
{
b.append(static_cast<char>(0x0D));
b.append(static_cast<char>(0x0A));
}
else if (text == "TAB")
{
b.append(static_cast<char>(0x09));
}
else if (text == "NUL")
{
b.append(static_cast<char>(0x00));
}
else if (text == "ESC")
{
b.append(static_cast<char>(0x1B));
}
else if (text == "~")
{
b.append(static_cast<char>(0x7E));
}
else if (text == "<Space>")
{
b.append(static_cast<char>(0x20));
}
return b;
}
void DebugConsole::appendSpecialSymbol(const QString& text)
{
QString line = m_ui->sendText->text();
QByteArray symbols = symbolNameToBytes(text);
// The text is appended to the enter field
if (convertToAscii)
{
line.append(symbols);
}
else
{
for (int i = 0; i < symbols.size(); i++)
{
QString add(" 0x%1");
line.append(add.arg(static_cast<char>(symbols.at(i)), 2, 16, QChar('0')));
}
}
m_ui->sendText->setText(line);
}
void DebugConsole::sendBytes()
{
if (!currLink->isConnected())
{
m_ui->sentText->setText(tr("Nothing sent. The link %1 is unconnected. Please connect first.").arg(currLink->getName()));
return;
}
QByteArray transmit;
QString feedback;
bool ok = true;
......@@ -382,8 +454,16 @@ void DebugConsole::sendBytes()
if (ok && m_ui->sendText->text().toLatin1().size() > 0)
{
// Transmit only if conversion succeeded
currLink->writeBytes(transmit, transmit.size());
m_ui->sentText->setText(tr("Sent: ") + feedback);
// int transmitted =
currLink->writeBytes(transmit, transmit.size());
// if (transmit.size() == transmitted)
// {
m_ui->sentText->setText(tr("Sent: ") + feedback);
// }
// else
// {
// m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size()));
// }
}
else if (m_ui->sendText->text().toLatin1().size() > 0)
{
......@@ -403,6 +483,8 @@ void DebugConsole::hexModeEnabled(bool mode)
{
convertToAscii = !mode;
m_ui->receiveText->clear();
m_ui->sendText->clear();
m_ui->sentText->clear();
}
/**
......
......@@ -83,6 +83,8 @@ public slots:
void setAutoHold(bool hold);
/** @brief Receive plain text message to output to the user */
void receiveTextMessage(int id, int component, int severity, QString text);
/** @brief Append a special symbol */
void appendSpecialSymbol(const QString& text);
protected slots:
/** @brief Draw information overlay */
......@@ -92,6 +94,7 @@ public slots:
protected:
void changeEvent(QEvent *e);
QByteArray symbolNameToBytes(const QString& symbol);
QList<LinkInterface*> links;
LinkInterface* currLink;
......
......@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>463</width>
<height>159</height>
<width>447</width>
<height>181</height>
</rect>
</property>
<property name="windowTitle">
......@@ -106,24 +106,74 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLineEdit" name="sendText">
<property name="minimumSize">
<size>
<width>80</width>
<height>0</height>
</size>
</property>
<property name="toolTip">
<string>Type the bytes to send here, use 0xAA format for HEX (Check HEX checkbox above)</string>
</property>
</widget>
</item>
<item row="3" column="1">
<layout class="QHBoxLayout" name="horizontalLayout" stretch="10,10,10,0">
<item row="4" column="1">
<layout class="QHBoxLayout" name="horizontalLayout" stretch="0,10,10,10,0">
<property name="spacing">
<number>5</number>
</property>
<item>
<widget class="QComboBox" name="specialComboBox">
<property name="maxVisibleItems">
<number>10</number>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property>
<property name="minimumContentsLength">
<number>1</number>
</property>
<item>
<property name="text">
<string>Add..</string>
</property>
</item>
<item>
<property name="text">
<string>CR+LF</string>
</property>
</item>
<item>
<property name="text">
<string>LF</string>
</property>
</item>
<item>
<property name="text">
<string>FF</string>
</property>
</item>
<item>
<property name="text">
<string>CR</string>
</property>
</item>
<item>
<property name="text">
<string>TAB</string>
</property>
</item>
<item>
<property name="text">
<string>NUL</string>
</property>
</item>
<item>
<property name="text">
<string>ESC</string>
</property>
</item>
<item>
<property name="text">
<string>~</string>
</property>
</item>
<item>
<property name="text">
<string>&lt;Space&gt;</string>
</property>
</item>
</widget>
</item>
<item>
<widget class="QPushButton" name="transmitButton">
<property name="toolTip">
......@@ -164,6 +214,19 @@
</item>
</layout>
</item>
<item row="3" column="0" colspan="2">
<widget class="QLineEdit" name="sendText">
<property name="minimumSize">
<size>
<width>80</width>
<height>0</height>
</size>
</property>
<property name="toolTip">
<string>Type the bytes to send here, use 0xAA format for HEX (Check HEX checkbox above)</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources>
......
......@@ -812,6 +812,7 @@ float HDDisplay::refLineWidthToPen(float line)
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec)
{
Q_UNUSED(uasId);
Q_UNUSED(unit);
// Update mean
const float oldMean = valuesMean.value(name, 0.0f);
const int meanCount = valuesCount.value(name, 0);
......