From d49b7022c858649fbd9778aef784bd59e54d38d2 Mon Sep 17 00:00:00 2001 From: lm Date: Mon, 7 Jun 2010 18:00:34 +0200 Subject: [PATCH 1/3] Cleanups in GCS --- src/comm/MAVLinkProtocol.cc | 26 +++++++++++++++++++++++++- src/comm/MAVLinkProtocol.h | 16 +++++++++++++--- src/uas/UAS.cc | 5 +++-- src/ui/HSIDisplay.cc | 41 +++++++++++++++++++++++++++++++++++------ src/ui/HSIDisplay.h | 12 ++++++------ src/ui/MAVLinkSettingsWidget.cc | 3 +++ src/ui/MainWindow.cc | 2 +- src/ui/ObjectDetectionView.cc | 6 +++--- 8 files changed, 89 insertions(+), 22 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 18e4b83..8d3abf0 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -35,6 +35,7 @@ This file is part of the PIXHAWK project #include #include +#include #include "MG.h" #include "MAVLinkProtocol.h" @@ -56,7 +57,9 @@ This file is part of the PIXHAWK project MAVLinkProtocol::MAVLinkProtocol() : heartbeatTimer(new QTimer(this)), heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), - m_heartbeatsEnabled(false) + m_heartbeatsEnabled(false), + m_loggingEnabled(false), + m_logfile(NULL) { start(QThread::LowPriority); // Start heartbeat timer, emitting a heartbeat at the configured rate @@ -83,6 +86,7 @@ MAVLinkProtocol::~MAVLinkProtocol() void MAVLinkProtocol::run() { + } /** @@ -316,11 +320,31 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled) emit heartbeatChanged(enabled); } +void MAVLinkProtocol::enableLogging(bool enabled) +{ + if (enabled && !m_loggingEnabled) + { + m_logfile = new QFile(QCoreApplication::applicationDirPath()+"mavlink.log"); + } + else + { + m_logfile->close(); + delete m_logfile; + m_logfile = NULL; + } + m_loggingEnabled = enabled; +} + bool MAVLinkProtocol::heartbeatsEnabled(void) { return m_heartbeatsEnabled; } +bool MAVLinkProtocol::loggingEnabled(void) +{ + return m_loggingEnabled; +} + /** * The default rate is 1 Hertz. * diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 05ba5f7..4a02a16 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -37,6 +37,7 @@ This file is part of the PIXHAWK project #include #include #include +#include #include #include #include "ProtocolInterface.h" @@ -65,6 +66,8 @@ public: int getHeartbeatRate(); /** @brief Get heartbeat state */ bool heartbeatsEnabled(void); + /** @brief Get logging state */ + bool loggingEnabled(void); public slots: /** @brief Receive bytes from a communication interface */ @@ -79,14 +82,19 @@ public slots: /** @brief Enable / disable the heartbeat emission */ void enableHeartbeats(bool enabled); + /** @brief Enable/disable binary packet logging */ + void enableLogging(bool enabled); + /** @brief Send an extra heartbeat to all connected units */ void sendHeartbeat(); protected: - QTimer* heartbeatTimer; ///< Timer to emit heartbeats - int heartbeatRate; ///< Heartbeat rate, controls the timer interval + QTimer* heartbeatTimer; ///< Timer to emit heartbeats + int heartbeatRate; ///< Heartbeat rate, controls the timer interval bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission - QMutex receiveMutex; ///< Mutex to protect receiveBytes function + bool m_loggingEnabled; ///< Enable/disable packet logging + QFile* m_logfile; ///< Logfile + QMutex receiveMutex; ///< Mutex to protect receiveBytes function int lastIndex[256][256]; int totalReceiveCounter; int totalLossCounter; @@ -98,6 +106,8 @@ signals: void messageReceived(LinkInterface* link, mavlink_message_t message); /** @brief Emitted if heartbeat emission mode is changed */ void heartbeatChanged(bool heartbeats); + /** @brief Emitted if logging is started / stopped */ + void loggingChanged(bool enabled); }; #endif // MAVLINKPROTOCOL_H_ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8d7fe46..b4d8601 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -338,7 +338,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "lon", pos.lon, time); emit valueChanged(uasId, "alt", pos.alt, time); emit valueChanged(uasId, "speed", pos.v, time); - qDebug() << "GOT GPS RAW"; + //qDebug() << "GOT GPS RAW"; emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); } break; @@ -346,7 +346,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_gps_status_t pos; mavlink_msg_gps_status_decode(&message, &pos); - for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++) + //qDebug() << "GOT GPS STATUS FOR "<< pos.satellites_visible << "SATELLITES"; + for(int i = 0; i < (int)pos.satellites_visible; i++) { emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast(pos.satellite_used[i])); } diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index eb4ba2d..cbb4b00 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -42,7 +42,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : HDDisplay(NULL, parent), gpsSatellites() { - + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); } void HSIDisplay::paintEvent(QPaintEvent * event) @@ -133,6 +133,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); } + connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); + // Now connect the new UAS //if (this->uas != uas) @@ -146,7 +148,14 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used) { Q_UNUSED(uasid); + //qDebug() << "UPDATED SATELLITE"; // If slot is empty, insert object + if (gpsSatellites.size() <= satid) + { + gpsSatellites.resize(satid+1); +// gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used)); + } + if (gpsSatellites.at(satid) == NULL) { gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used)); @@ -160,11 +169,30 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float dire QColor HSIDisplay::getColorForSNR(float snr) { - return QColor(200, 250, 200); + QColor color; + if (snr < 10) + { + color = QColor(250, 200, 200); + } + else if (snr > 20) + { + color = QColor(200, 250, 200); + } + else if (snr > 30) + { + color = QColor(100, 250, 100); + } + else + { + color = QColor(180, 180, 180); + } + return color; } void HSIDisplay::drawGPS() { + float xCenter = vwidth/2.0f; + float yCenter = vwidth/2.0f; QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::HighQualityAntialiasing, true); @@ -172,7 +200,8 @@ void HSIDisplay::drawGPS() // Max satellite circle radius const float margin = 0.2f; // 20% margin of total width on each side - float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f; + float radius = (vwidth - vwidth * 2.0f * margin) / 4.0f; + radius = radius; for (int i = 0; i < gpsSatellites.size(); i++) { @@ -195,10 +224,10 @@ void HSIDisplay::drawGPS() painter.setPen(color); painter.setBrush(brush); - float xPos = sin(sat->direction) * sat->azimuth * radius; - float yPos = cos(sat->direction) * sat->azimuth * radius; + float xPos = xCenter + sin(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; + float yPos = yCenter + cos(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; - drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter); + drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter); } } } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 838013d..4126fc7 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -49,6 +49,7 @@ public: public slots: void setActiveUAS(UASInterface* uas); + void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); void paintEvent(QPaintEvent * event); protected slots: @@ -59,7 +60,6 @@ protected slots: protected: - void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); static QColor getColorForSNR(float snr); /** @@ -69,11 +69,11 @@ protected: { public: GPSSatellite(int id, float azimuth, float direction, float snr, bool used) : - id(id), - azimuth(azimuth), - direction(direction), - snr(snr), - used(used) + id(id), + azimuth(azimuth), + direction(direction), + snr(snr), + used(used) { } diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 05f0bdd..a64fbaa 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -11,9 +11,12 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget // 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))); // Initialize state m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); + m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); } MAVLinkSettingsWidget::~MAVLinkSettingsWidget() diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index fcfab3c..d686607 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -85,7 +85,7 @@ settings() waypoints->setVisible(false); info = new UASInfoWidget(this); info->setVisible(false); - detection = new ObjectDetectionView("test", this); + detection = new ObjectDetectionView("patterns", this); detection->setVisible(false); hud = new HUD(640, 480, this); hud->setVisible(false); diff --git a/src/ui/ObjectDetectionView.cc b/src/ui/ObjectDetectionView.cc index 7b4d9be..3838acf 100644 --- a/src/ui/ObjectDetectionView.cc +++ b/src/ui/ObjectDetectionView.cc @@ -94,7 +94,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i { //qDebug() << "REDETECTED"; - /* QList actions = m_ui->listWidget->actions(); // Find action and update it foreach (QAction* act, actions) @@ -107,7 +106,9 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i act->setText(patternPath + separator + "(#" + QString::number(count) + ")" + separator + QString::number(confidence)); } } - QPixmap image = QPixmap(patternFolder + "/" + patternPath); + QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/").last(); + qDebug() << "Loading:" << filePath; + QPixmap image = QPixmap(filePath); image = image.scaledToWidth(m_ui->imageLabel->width()); m_ui->imageLabel->setPixmap(image); QString patternName = patternPath.split("//").last(); // Remove preceding folder names @@ -115,7 +116,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i // Set name and label m_ui->nameLabel->setText(patternName); - */ } else { From d3b45c527199837cfd75d7f555766e33e6f5f65d Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 7 Jun 2010 21:34:08 +0200 Subject: [PATCH 2/3] Added packet logging --- src/comm/MAVLinkProtocol.cc | 22 +++++++++++++++++++++- src/comm/MAVLinkProtocol.h | 2 ++ src/ui/MAVLinkSettingsWidget.ui | 7 +++++++ 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 8d3abf0..c992411 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -80,6 +80,11 @@ MAVLinkProtocol::MAVLinkProtocol() : MAVLinkProtocol::~MAVLinkProtocol() { + if (m_logfile) + { + m_logfile->close(); + delete m_logfile; + } } @@ -89,6 +94,11 @@ void MAVLinkProtocol::run() } +QString MAVLinkProtocol::getLogfileName() +{ + return QCoreApplication::applicationDirPath()+"/mavlink.log"; +} + /** * The bytes are copied by calling the LinkInterface::readBytes() method. * This method parses all incoming bytes and constructs a MAVLink packet. @@ -115,6 +125,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) if (decodeState == 1) { + // Log data + if (m_loggingEnabled) + { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + mavlink_msg_to_send_buffer(buf, &message); + m_logfile->write((const char*) buf); + qDebug() << "WROTE LOGFILE"; + } + // ORDER MATTERS HERE! // If the matching UAS object does not yet exist, it has to be created // before emitting the packetReceived signal @@ -324,7 +343,8 @@ void MAVLinkProtocol::enableLogging(bool enabled) { if (enabled && !m_loggingEnabled) { - m_logfile = new QFile(QCoreApplication::applicationDirPath()+"mavlink.log"); + m_logfile = new QFile(getLogfileName()); + m_logfile->open(QIODevice::WriteOnly | QIODevice::Append); } else { diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 4a02a16..e2fbafb 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -68,6 +68,8 @@ public: bool heartbeatsEnabled(void); /** @brief Get logging state */ bool loggingEnabled(void); + /** @brief Get the name of the packet log file */ + QString getLogfileName(); public slots: /** @brief Receive bytes from a communication interface */ diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index de9b670..c65627d 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -25,6 +25,13 @@ + + + Log all MAVLink packets + + + + Qt::Vertical From 96522cf70646354c6baa6049f1204a8e564e0ebf Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 7 Jun 2010 21:45:45 +0200 Subject: [PATCH 3/3] Working on real log replay in simulation link --- src/comm/MAVLinkProtocol.h | 2 +- src/comm/MAVLinkSimulationLink.cc | 20 ++++++++++++++++++++ src/comm/MAVLinkSimulationLink.h | 1 + 3 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index e2fbafb..82cf9b1 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -69,7 +69,7 @@ public: /** @brief Get logging state */ bool loggingEnabled(void); /** @brief Get the name of the packet log file */ - QString getLogfileName(); + static QString getLogfileName(); public slots: /** @brief Receive bytes from a communication interface */ diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 8af036e..2049e55 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -38,6 +38,7 @@ This file is part of the PIXHAWK project #include #include "MG.h" #include "LinkManager.h" +#include "MAVLinkProtocol.h" #include "MAVLinkSimulationLink.h" // MAVLINK includes #include @@ -92,6 +93,10 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile maxTimeNoise = 0; this->id = getNextLinkId(); LinkManager::instance()->add(this); + + // Open packet log + mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName()); + mavlinkLogFile->open(QIODevice::ReadOnly); } MAVLinkSimulationLink::~MAVLinkSimulationLink() @@ -121,6 +126,21 @@ void MAVLinkSimulationLink::run() if (_isConnected) { mainloop(); + + // FIXME Hacky code to read from packet log file +// readyBufferMutex.lock(); +// for (int i = 0; i < streampointer; i++) +// { +// readyBuffer.enqueue(*(stream + i)); +// } +// readyBufferMutex.unlock(); + + + + + + + emit bytesReady(this); } last = MG::TIME::getGroundTimeNow(); diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index b6b5564..0b10e37 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -98,6 +98,7 @@ protected: QTimer* timer; /** File which contains the input data (simulated robot messages) **/ QFile* simulationFile; + QFile* mavlinkLogFile; QString simulationHeader; /** File where the commands sent by the groundstation are stored **/ QFile* receiveFile;