diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index db264b60da7371fa874a202c03682d66965baebd..f93cab1095e1b93c44322252928d5e5f992c3214 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -235,7 +235,11 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCMAVLinkInspector.ui \ src/ui/WaypointViewOnlyView.ui \ src/ui/WaypointEditableView.ui \ - src/ui/UnconnectedUASInfoWidget.ui + src/ui/UnconnectedUASInfoWidget.ui \ + src/ui/mavlink/QGCMAVLinkMessageSender.ui \ + src/ui/firmwareupdate/QGCFirmwareUpdateWidget.ui \ + src/ui/QGCPluginHost.ui \ + src/ui/firmwareupdate/QGCPX4FirmwareUpdate.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -353,7 +357,11 @@ HEADERS += src/MG.h \ src/ui/WaypointViewOnlyView.h \ src/ui/WaypointEditableView.h \ src/ui/UnconnectedUASInfoWidget.h \ - src/ui/QGCRGBDView.h + src/ui/QGCRGBDView.h \ + src/ui/mavlink/QGCMAVLinkMessageSender.h \ + src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \ + src/ui/QGCPluginHost.h \ + src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -481,7 +489,11 @@ SOURCES += src/main.cc \ src/ui/WaypointViewOnlyView.cc \ src/ui/WaypointEditableView.cc \ src/ui/UnconnectedUASInfoWidget.cc \ - src/ui/QGCRGBDView.cc + src/ui/QGCRGBDView.cc \ + src/ui/mavlink/QGCMAVLinkMessageSender.cc \ + src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \ + src/ui/QGCPluginHost.cc \ + src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 82fbe2b2cd45822e11063f1cdbed01a285bd13ed..17d19ec6aa1bf9a7f7170d042d3d3da51bcbfd17 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 9c35dc87dd7ef49662e24d0146d48c2fae4e8cf0..ae6a311bea504917fd67e3a3a4ecf3f5a048ee28 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/MainWindow.cc b/src/ui/MainWindow.cc index efd74116d88dec16e165797d86a8223ff6357377..dc29bfe31b7a1a77cd205db5ac861c4078b45eb8 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -51,7 +51,9 @@ This file is part of the QGROUNDCONTROL project #include "QGCSettingsWidget.h" #include "QGCMapTool.h" #include "MAVLinkDecoder.h" +#include "QGCMAVLinkMessageSender.h" #include "QGCRGBDView.h" +#include "QGCFirmwareUpdate.h" #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" @@ -237,7 +239,7 @@ MainWindow::~MainWindow() // Get and delete all dockwidgets and contained // widgets - QObjectList childList( this->children() ); + QObjectList childList(this->children()); QObjectList::iterator i; QDockWidget* dockWidget; @@ -251,12 +253,11 @@ MainWindow::~MainWindow() // delete dockWidget->widget(); delete dockWidget; } - else + else if (dynamic_cast(*i)) { delete dynamic_cast(*i); } } - // Delete all UAS objects } @@ -402,31 +403,42 @@ void MainWindow::buildCommonWidgets() addTool(mavlinkInspectorWidget, tr("MAVLink Inspector"), Qt::RightDockWidgetArea); } + if (!mavlinkSenderWidget) + { + mavlinkSenderWidget = new QDockWidget(tr("MAVLink Message Sender"), this); + mavlinkSenderWidget->setWidget( new QGCMAVLinkMessageSender(mavlink, this) ); + mavlinkSenderWidget->setObjectName("MAVLINK_SENDER_DOCKWIDGET"); + addTool(mavlinkSenderWidget, tr("MAVLink Sender"), Qt::RightDockWidgetArea); + } + //FIXME: memory of acceptList will never be freed again QStringList* acceptList = new QStringList(); - acceptList->append("-105,roll deg,deg,+105,s"); - acceptList->append("-105,pitch deg,deg,+105,s"); - acceptList->append("-105,heading deg,deg,+105,s"); + acceptList->append("-3.3,ATTITUDE.roll,rad,+3.3,s"); + acceptList->append("-3.3,ATTITUDE.pitch,deg,+3.3,s"); + acceptList->append("-3.3,ATTITUDE.yaw,deg,+3.3,s"); //FIXME: memory of acceptList2 will never be freed again QStringList* acceptList2 = new QStringList(); - acceptList2->append("0,abs pressure,hPa,65500"); + acceptList2->append("0,RAW_PRESSURE.pres_abs,hPa,65500"); - if (!parametersDockWidget) { + if (!parametersDockWidget) + { parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); addTool(parametersDockWidget, tr("Calibration and Parameters"), Qt::RightDockWidgetArea); } - if (!hsiDockWidget) { + if (!hsiDockWidget) + { hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); addTool(hsiDockWidget, tr("Horizontal Situation"), Qt::BottomDockWidgetArea); } - if (!headDown1DockWidget) { + if (!headDown1DockWidget) + { headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); HDDisplay* hdDisplay = new HDDisplay(acceptList, "Flight Display", this); hdDisplay->addSource(mavlinkDecoder); @@ -435,7 +447,8 @@ void MainWindow::buildCommonWidgets() addTool(headDown1DockWidget, tr("Flight Display"), Qt::RightDockWidgetArea); } - if (!headDown2DockWidget) { + if (!headDown2DockWidget) + { headDown2DockWidget = new QDockWidget(tr("Actuator Status"), this); HDDisplay* hdDisplay = new HDDisplay(acceptList2, "Actuator Status", this); hdDisplay->addSource(mavlinkDecoder); @@ -444,21 +457,24 @@ void MainWindow::buildCommonWidgets() addTool(headDown2DockWidget, tr("Actuator Status"), Qt::RightDockWidgetArea); } - if (!rcViewDockWidget) { + if (!rcViewDockWidget) + { rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); addTool(rcViewDockWidget, tr("Radio Control"), Qt::BottomDockWidgetArea); } - if (!headUpDockWidget) { + if (!headUpDockWidget) + { headUpDockWidget = new QDockWidget(tr("HUD"), this); headUpDockWidget->setWidget( new HUD(320, 240, this)); headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); addTool(headUpDockWidget, tr("Head Up Display"), Qt::RightDockWidgetArea); } - if (!video1DockWidget) { + if (!video1DockWidget) + { video1DockWidget = new QDockWidget(tr("Video Stream 1"), this); QGCRGBDView* video1 = new QGCRGBDView(160, 120, this); video1->enableHUDInstruments(false); @@ -469,7 +485,8 @@ void MainWindow::buildCommonWidgets() addTool(video1DockWidget, tr("Video Stream 1"), Qt::LeftDockWidgetArea); } - if (!video2DockWidget) { + if (!video2DockWidget) + { video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); QGCRGBDView* video2 = new QGCRGBDView(160, 120, this); video2->enableHUDInstruments(false); @@ -518,6 +535,12 @@ void MainWindow::buildCommonWidgets() addCentralWidget(protocolWidget, "Mavlink Generator"); } + if (!firmwareUpdateWidget) + { + firmwareUpdateWidget = new QGCFirmwareUpdate(this); + addCentralWidget(firmwareUpdateWidget, "Firmware Update"); + } + if (!hudWidget) { hudWidget = new HUD(320, 240, this); addCentralWidget(hudWidget, tr("Head Up Display")); @@ -608,7 +631,8 @@ void MainWindow::closeEvent(QCloseEvent *event) */ void MainWindow::connectCommonWidgets() { - if (infoDockWidget && infoDockWidget->widget()) { + if (infoDockWidget && infoDockWidget->widget()) + { connect(mavlink, SIGNAL(receiveLossChanged(int, float)), infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); } @@ -846,7 +870,8 @@ void MainWindow::selectStylesheet() // Let user select style sheet styleFileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), styleFileName, tr("CSS Stylesheet (*.css);;")); - if (!styleFileName.endsWith(".css")) { + if (!styleFileName.endsWith(".css")) + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Information); msgBox.setText(tr("QGroundControl did lot load a new style")); @@ -865,14 +890,18 @@ void MainWindow::reloadStylesheet() { // Load style sheet QFile* styleSheet = new QFile(styleFileName); - if (!styleSheet->exists()) { + if (!styleSheet->exists()) + { styleSheet = new QFile(":/images/style-mission.css"); } - if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) { + if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) + { QString style = QString(styleSheet->readAll()); style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); qApp->setStyleSheet(style); - } else { + } + else + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Information); msgBox.setText(tr("QGroundControl did lot load a new style")); @@ -940,6 +969,7 @@ void MainWindow::connectCommonActions() perspectives->addAction(ui.actionMavlinkView); perspectives->addAction(ui.actionPilotsView); perspectives->addAction(ui.actionOperatorsView); + perspectives->addAction(ui.actionFirmwareUpdateView); perspectives->addAction(ui.actionUnconnectedView); perspectives->setExclusive(true); @@ -948,6 +978,7 @@ void MainWindow::connectCommonActions() if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); + if (currentView == VIEW_FIRMWAREUPDATE) ui.actionFirmwareUpdateView->setChecked(true); if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true); // The UAS actions are not enabled without connection to system @@ -978,7 +1009,9 @@ void MainWindow::connectCommonActions() connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView())); + connect(ui.actionFirmwareUpdateView, SIGNAL(triggered()), this, SLOT(loadFirmwareUpdateView())); connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(ui.actionReloadStylesheet, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); connect(ui.actionSelectStylesheet, SIGNAL(triggered()), this, SLOT(selectStylesheet())); @@ -1012,7 +1045,8 @@ void MainWindow::connectCommonActions() void MainWindow::showHelp() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/start"))) { + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/start"))) + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Could not open help in browser"); @@ -1025,7 +1059,8 @@ void MainWindow::showHelp() void MainWindow::showCredits() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits"))) { + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits"))) + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Could not open credits in browser"); @@ -1038,7 +1073,8 @@ void MainWindow::showCredits() void MainWindow::showRoadMap() { - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/dev/roadmap"))) { + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/dev/roadmap"))) + { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Could not open roadmap in browser"); @@ -1051,8 +1087,10 @@ void MainWindow::showRoadMap() void MainWindow::configure() { - if (!joystickWidget) { - if (!joystick->isRunning()) { + if (!joystickWidget) + { + if (!joystick->isRunning()) + { joystick->start(); } joystickWidget = new JoystickWidget(joystick); @@ -1080,8 +1118,10 @@ void MainWindow::addLink() const int32_t& linkIndex(LinkManager::instance()->getLinks().indexOf(link)); const int32_t& linkID(LinkManager::instance()->getLinks()[linkIndex]->getId()); - foreach (QAction* act, actions) { - if (act->data().toInt() == linkID) { // LinkManager::instance()->getLinks().indexOf(link) + foreach (QAction* act, actions) + { + if (act->data().toInt() == linkID) + { // LinkManager::instance()->getLinks().indexOf(link) act->trigger(); break; } @@ -1105,15 +1145,18 @@ void MainWindow::addLink(LinkInterface *link) const int32_t& linkIndex(LinkManager::instance()->getLinks().indexOf(link)); const int32_t& linkID(LinkManager::instance()->getLinks()[linkIndex]->getId()); - foreach (QAction* act, actions) { - if (act->data().toInt() == linkID) { // LinkManager::instance()->getLinks().indexOf(link) + foreach (QAction* act, actions) + { + if (act->data().toInt() == linkID) + { // LinkManager::instance()->getLinks().indexOf(link) found = true; } } //UDPLink* udp = dynamic_cast(link); - if (!found) { // || udp + if (!found) + { // || udp CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); QAction* action = commWidget->getAction(); ui.menuNetwork->addAction(action); @@ -1274,13 +1317,17 @@ void MainWindow::UASCreated(UASInterface* uas) if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) { int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt(); - switch (view) { + switch (view) + { case VIEW_ENGINEER: loadEngineerView(); break; case VIEW_MAVLINK: loadMAVLinkView(); break; + case VIEW_FIRMWAREUPDATE: + loadFirmwareUpdateView(); + break; case VIEW_PILOT: loadPilotView(); break; @@ -1358,6 +1405,7 @@ void MainWindow::loadViewState() debugConsoleDockWidget->show(); logPlayerDockWidget->show(); mavlinkInspectorWidget->show(); + mavlinkSenderWidget->show(); parametersDockWidget->show(); hsiDockWidget->hide(); headDown1DockWidget->hide(); @@ -1393,7 +1441,27 @@ void MainWindow::loadViewState() infoDockWidget->hide(); debugConsoleDockWidget->hide(); logPlayerDockWidget->hide(); + mavlinkInspectorWidget->show(); + mavlinkSenderWidget->show(); + parametersDockWidget->hide(); + hsiDockWidget->hide(); + headDown1DockWidget->hide(); + headDown2DockWidget->hide(); + rcViewDockWidget->hide(); + headUpDockWidget->hide(); + video1DockWidget->hide(); + video2DockWidget->hide(); + break; + case VIEW_FIRMWAREUPDATE: + centerStack->setCurrentWidget(firmwareUpdateWidget); + controlDockWidget->hide(); + listDockWidget->hide(); + waypointsDockWidget->hide(); + infoDockWidget->hide(); + debugConsoleDockWidget->hide(); + logPlayerDockWidget->hide(); mavlinkInspectorWidget->hide(); + mavlinkSenderWidget->hide(); parametersDockWidget->hide(); hsiDockWidget->hide(); headDown1DockWidget->hide(); @@ -1506,6 +1574,17 @@ void MainWindow::loadMAVLinkView() } } +void MainWindow::loadFirmwareUpdateView() +{ + if (currentView != VIEW_FIRMWAREUPDATE) + { + storeViewState(); + currentView = VIEW_FIRMWAREUPDATE; + ui.actionFirmwareUpdateView->setChecked(true); + loadViewState(); + } +} + void MainWindow::loadDataView(QString fileName) { // Plot is now selected, now load data from file diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 72e0361244331fd37a1d2cda146bda0e1d184795..eccfa4e14705940d91fe362c5857a09d7546dea7 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -55,7 +55,6 @@ This file is part of the QGROUNDCONTROL project #include "JoystickWidget.h" #include "input/JoystickInput.h" #include "DebugConsole.h" -//#include "MapWidget.h" #include "ParameterInterface.h" #include "XMLCommProtocolWidget.h" #include "HDDisplay.h" @@ -80,6 +79,8 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkDecoder.h" class QGCMapTool; +class QGCMAVLinkMessageSender; +class QGCFirmwareUpdate; class QSplashScreen; /** @@ -154,6 +155,8 @@ public slots: void loadOperatorView(); /** @brief Load MAVLink XML generator view */ void loadMAVLinkView(); + /** @brief Load firmware update view */ + void loadFirmwareUpdateView(); /** @brief Show the online help for users */ void showHelp(); @@ -241,13 +244,11 @@ protected: VIEW_OPERATOR, VIEW_PILOT, VIEW_MAVLINK, + VIEW_FIRMWAREUPDATE, VIEW_UNCONNECTED, ///< View in unconnected mode, when no UAS is available VIEW_FULL ///< All widgets shown at once } VIEW_SECTIONS; -// QHash toolsMenuActions; // Holds ptr to the Menu Actions -// QHash dockWidgets; // Holds ptr to the Actual Dock widget - /** * @brief Adds an already instantiated QDockedWidget to the Tools Menu * @@ -314,12 +315,10 @@ protected: #ifdef QGC_OSG_ENABLED QPointer _3DWidget; #endif -#ifdef QGC_OSGEARTH_ENABLED - QPointer _3DMapWidget; -#endif #if (defined _MSC_VER) || (defined Q_OS_MAC) QPointer gEarthWidget; #endif + QPointer firmwareUpdateWidget; // Dock widgets QPointer controlDockWidget; @@ -353,6 +352,7 @@ protected: QPointer mavlinkInspectorWidget; QPointer mavlinkDecoder; + QPointer mavlinkSenderWidget; QGCMAVLinkLogPlayer* logPlayer; // Popup widgets diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index bbc2231f2019a57de3342d25ccda9c03fe018a08..17cb443ab94354102b94f6bc577094a96a57a332 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -51,7 +51,7 @@ 0 0 800 - 25 + 22 @@ -108,7 +108,7 @@ - Widgets + Tool Widgets @@ -129,6 +129,7 @@ + @@ -137,16 +138,22 @@ - Main + Main Widget + + + + + Plugins + - + @@ -466,6 +473,18 @@ Load Custom Widget File + + + + :/images/status/software-update-available.svg:/images/status/software-update-available.svg + + + Firmware Update + + + Update the firmware of one of the connected autopilots + + diff --git a/src/ui/QGCFirmwareUpdate.ui b/src/ui/QGCFirmwareUpdate.ui index 0981fa6333e631fcc42ed45fc04d964351386f81..cd2aa4ef6f2dd5eaebde4b8eb262270c578d829f 100644 --- a/src/ui/QGCFirmwareUpdate.ui +++ b/src/ui/QGCFirmwareUpdate.ui @@ -6,13 +6,21 @@ 0 0 - 400 - 300 + 596 + 343 Form + + + + + + + + diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index a05aeb5c8491f7e52e59727b0288ed4e78d11e25..f7982149e2f201a23f05c84a463dc824ea0c6e43 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 b9850a5fa65a82b07efa6ccd6be1077c34312343..9e2028ca93f71474817593a957a9c4fdaecf9d87 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/QGCPluginHost.cc b/src/ui/QGCPluginHost.cc new file mode 100644 index 0000000000000000000000000000000000000000..91b16d6dbb6acf77c8ceeed4f6a4d1a6b41352e5 --- /dev/null +++ b/src/ui/QGCPluginHost.cc @@ -0,0 +1,14 @@ +#include "QGCPluginHost.h" +#include "ui_QGCPluginHost.h" + +QGCPluginHost::QGCPluginHost(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCPluginHost) +{ + ui->setupUi(this); +} + +QGCPluginHost::~QGCPluginHost() +{ + delete ui; +} diff --git a/src/ui/QGCPluginHost.h b/src/ui/QGCPluginHost.h new file mode 100644 index 0000000000000000000000000000000000000000..fe78e6cad085fe7716c524460878fd048db3468b --- /dev/null +++ b/src/ui/QGCPluginHost.h @@ -0,0 +1,22 @@ +#ifndef QGCPLUGINHOST_H +#define QGCPLUGINHOST_H + +#include + +namespace Ui { +class QGCPluginHost; +} + +class QGCPluginHost : public QWidget +{ + Q_OBJECT + +public: + explicit QGCPluginHost(QWidget *parent = 0); + ~QGCPluginHost(); + +private: + Ui::QGCPluginHost *ui; +}; + +#endif // QGCPLUGINHOST_H diff --git a/src/ui/QGCPluginHost.ui b/src/ui/QGCPluginHost.ui new file mode 100644 index 0000000000000000000000000000000000000000..2554c142ac2143c5e9493f93dce64a76d6214bca --- /dev/null +++ b/src/ui/QGCPluginHost.ui @@ -0,0 +1,68 @@ + + + QGCPluginHost + + + + 0 + 0 + 633 + 329 + + + + Form + + + + + 10 + 30 + 256 + 271 + + + + + + + 280 + 30 + 341 + 271 + + + + true + + + + + + 10 + 10 + 121 + 16 + + + + Loaded Plugins + + + + + + 280 + 10 + 91 + 16 + + + + Plugin Log + + + + + + diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 583c486883f74484ff080db084318185ad4a59ec..00ac31c6d724e676365edf57e211d6e10f8a3674 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); @@ -101,8 +101,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) : void QGCToolBar::setLogPlayer(QGCMAVLinkLogPlayer* player) { this->player = player; - connect(toggleLoggingAction, SIGNAL(triggered(bool)), this, SLOT(playLogFile(bool))); - connect(logReplayAction, SIGNAL(triggered(bool)), this, SLOT(logging(bool))); + connect(toggleLoggingAction, SIGNAL(triggered(bool)), this, SLOT(logging(bool))); + connect(logReplayAction, SIGNAL(triggered(bool)), this, SLOT(playLogFile(bool))); } void QGCToolBar::playLogFile(bool enabled) @@ -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); diff --git a/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc new file mode 100644 index 0000000000000000000000000000000000000000..3b41f422ca9a95f777b6772b4744fb6ea8b1f5ce --- /dev/null +++ b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc @@ -0,0 +1,14 @@ +#include "QGCFirmwareUpdateWidget.h" +#include "ui_QGCFirmwareUpdateWidget.h" + +QGCFirmwareUpdateWidget::QGCFirmwareUpdateWidget(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCFirmwareUpdateWidget) +{ + ui->setupUi(this); +} + +QGCFirmwareUpdateWidget::~QGCFirmwareUpdateWidget() +{ + delete ui; +} diff --git a/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h new file mode 100644 index 0000000000000000000000000000000000000000..44a471c5a95bc0fafc400cf550aae2ae4ddfc992 --- /dev/null +++ b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h @@ -0,0 +1,22 @@ +#ifndef QGCFIRMWAREUPDATEWIDGET_H +#define QGCFIRMWAREUPDATEWIDGET_H + +#include + +namespace Ui { +class QGCFirmwareUpdateWidget; +} + +class QGCFirmwareUpdateWidget : public QWidget +{ + Q_OBJECT + +public: + explicit QGCFirmwareUpdateWidget(QWidget *parent = 0); + ~QGCFirmwareUpdateWidget(); + +private: + Ui::QGCFirmwareUpdateWidget *ui; +}; + +#endif // QGCFIRMWAREUPDATEWIDGET_H diff --git a/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.ui b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.ui new file mode 100644 index 0000000000000000000000000000000000000000..442db35a3aaba2fad44f99117e71749ff710c206 --- /dev/null +++ b/src/ui/firmwareupdate/QGCFirmwareUpdateWidget.ui @@ -0,0 +1,122 @@ + + + QGCFirmwareUpdateWidget + + + + 0 + 0 + 638 + 412 + + + + Form + + + + + + 1) Select Autopilot + + + + + + + + + + true + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:14px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:large; font-weight:600;">Autopilot Selection</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">Select one of the connected autopilots from the list on the left.</span></p></body></html> + + + + + + + true + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:14px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:large; font-weight:600;">AP Firmware v0.9.1</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">This software update...</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">FIXES:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">- Fix1</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">FEATURES:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">- New feature 1</span></p></body></html> + + + + + + + 2) Select Software Version + + + + + + + + + + true + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:14px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:large; font-weight:600;">Software Version Selection</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">Select the software version from the online repository on the left or choose the select file button to load a file from the harddisk. Detail information is shown on the right.</span></p></body></html> + + + + + + + Select File.. + + + + + + + 24 + + + + + + + Flash Firmware + + + + + + + Status + + + + + + + + diff --git a/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc new file mode 100644 index 0000000000000000000000000000000000000000..508de6ace87b83297766f5c3e5902d9c168560e4 --- /dev/null +++ b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc @@ -0,0 +1,14 @@ +#include "QGCPX4FirmwareUpdate.h" +#include "ui_QGCPX4FirmwareUpdate.h" + +QGCPX4FirmwareUpdate::QGCPX4FirmwareUpdate(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCPX4FirmwareUpdate) +{ + ui->setupUi(this); +} + +QGCPX4FirmwareUpdate::~QGCPX4FirmwareUpdate() +{ + delete ui; +} diff --git a/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h new file mode 100644 index 0000000000000000000000000000000000000000..3d7e503718d9f961d5dcf480d5625ebbdb9ecc69 --- /dev/null +++ b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h @@ -0,0 +1,22 @@ +#ifndef QGCPX4FIRMWAREUPDATE_H +#define QGCPX4FIRMWAREUPDATE_H + +#include + +namespace Ui { +class QGCPX4FirmwareUpdate; +} + +class QGCPX4FirmwareUpdate : public QWidget +{ + Q_OBJECT + +public: + explicit QGCPX4FirmwareUpdate(QWidget *parent = 0); + ~QGCPX4FirmwareUpdate(); + +private: + Ui::QGCPX4FirmwareUpdate *ui; +}; + +#endif // QGCPX4FIRMWAREUPDATE_H diff --git a/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.ui b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.ui new file mode 100644 index 0000000000000000000000000000000000000000..1fdcba752418567b2d9edc0ee18aff1468a7a343 --- /dev/null +++ b/src/ui/firmwareupdate/QGCPX4FirmwareUpdate.ui @@ -0,0 +1,143 @@ + + + QGCPX4FirmwareUpdate + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + 2 + + + + Firmware + + + + 6 + + + 6 + + + 6 + + + + + + + + + + + Select File + + + + + + + + Settings + + + + + + + + + Select File + + + + + + + + Flash Firmware + + + + + + Filename + + + + + + + 24 + + + + + + + Status + + + + + + + Flash Firmware + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Prev + + + + + + + Next + + + + + + + Flash + + + + + + + + diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 5fabc6898b7ebab48620bc45e609f372dcb4ee61..c17613393e379056912e709d50a675b58d943942 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -35,8 +35,8 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt minTime(0), lastTime(0), maxTime(100), - plotPosition(0), maxInterval(MAX_STORAGE_INTERVAL), + plotPosition(0), timeScaleStep(DEFAULT_SCALE_INTERVAL), // 10 seconds automaticScrollActive(false), m_active(false), diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index 2cf4fc323c1dcd6eded2dfc3c082419907af32c9..77c0a7c20b8bfd976957a42de2a0d7f9ff8e662c 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -298,8 +298,8 @@ protected: // TODO CHECK THIS!!! int scaling; QwtScaleEngine* yScaleEngine; - quint64 lastTime; ///< Last added timestamp quint64 minTime; ///< The smallest timestamp occured so far + quint64 lastTime; ///< Last added timestamp quint64 maxTime; ///< The biggest timestamp occured so far quint64 maxInterval; quint64 storageInterval; diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index f5bf0a4be484b93910f8790ae74b5e418e157e27..b212e5102a3274ad6270fa8f6bb0008fb9873350 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -663,15 +663,19 @@ void Pixhawk3DWidget::getPosition(double& x, double& y, double& z, QString& utmZone) { - if (uas) { - if (frame == MAV_FRAME_GLOBAL) { + if (uas) + { + if (frame == MAV_FRAME_GLOBAL) + { double latitude = uas->getLatitude(); double longitude = uas->getLongitude(); double altitude = uas->getAltitude(); Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); z = -altitude; - } else if (frame == MAV_FRAME_LOCAL_NED) { + } + else if (frame == MAV_FRAME_LOCAL_NED) + { x = uas->getLocalX(); y = uas->getLocalY(); z = uas->getLocalZ(); @@ -881,7 +885,8 @@ Pixhawk3DWidget::resizeHUD(void) int bottomHUDHeight = 25; osg::Vec3Array* vertices = static_cast(hudBackgroundGeometry->getVertexArray()); - if (vertices == NULL || vertices->size() != 8) { + if (vertices == NULL || vertices->size() != 8) + { osg::ref_ptr newVertices = new osg::Vec3Array(8); hudBackgroundGeometry->setVertexArray(newVertices); @@ -899,7 +904,8 @@ Pixhawk3DWidget::resizeHUD(void) statusText->setPosition(osg::Vec3(10, height() - 15, -1.5)); - if (rgb2DGeode.valid() && depth2DGeode.valid()) { + if (rgb2DGeode.valid() && depth2DGeode.valid()) + { int windowWidth = (width() - 20) / 2; int windowHeight = 3 * windowWidth / 4; rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2, @@ -922,7 +928,8 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, std::ostringstream oss; oss.setf(std::ios::fixed, std::ios::floatfield); oss.precision(2); - if (frame == MAV_FRAME_GLOBAL) { + if (frame == MAV_FRAME_GLOBAL) + { double latitude, longitude; Imagery::UTMtoLL(robotX, robotY, utmZone, latitude, longitude); @@ -943,7 +950,9 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, oss.precision(6); oss << " Cursor [" << cursorLatitude << " " << cursorLongitude << "]"; - } else if (frame == MAV_FRAME_LOCAL_NED) { + } + else if (frame == MAV_FRAME_LOCAL_NED) + { oss << " x = " << robotX << " y = " << robotY << " z = " << robotZ << @@ -957,7 +966,8 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, statusText->setText(oss.str()); bool darkBackground = true; - if (mapNode->getImageryType() == Imagery::GOOGLE_MAP) { + if (mapNode->getImageryType() == Imagery::GOOGLE_MAP) + { darkBackground = false; } @@ -968,34 +978,44 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, void Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ) { - if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f) { + if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f) + { return; } bool addToTrail = false; - if (trail.size() > 0) { + if (trail.size() > 0) + { if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f || fabs(robotY - trail[trail.size() - 1].y()) > 0.01f || - fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f) { + fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f) + { addToTrail = true; } - } else { + } + else + { addToTrail = true; } - if (addToTrail) { + if (addToTrail) + { osg::Vec3d p(robotX, robotY, robotZ); - if (trail.size() == trail.capacity()) { + if (trail.size() == trail.capacity()) + { memcpy(trail.data(), trail.data() + 1, (trail.size() - 1) * sizeof(osg::Vec3d)); trail[trail.size() - 1] = p; - } else { + } + else + { trail.append(p); } } trailVertices->clear(); - for (int i = 0; i < trail.size(); ++i) { + for (int i = 0; i < trail.size(); ++i) + { trailVertices->push_back(osg::Vec3d(trail[i].y() - robotY, trail[i].x() - robotX, -(trail[i].z() - robotZ))); @@ -1010,12 +1030,14 @@ void Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, const QString& zone) { - if (mapNode->getImageryType() == Imagery::BLANK_MAP) { + if (mapNode->getImageryType() == Imagery::BLANK_MAP) + { return; } double viewingRadius = cameraManipulator->getDistance() * 10.0; - if (viewingRadius < 100.0) { + if (viewingRadius < 100.0) + { viewingRadius = 100.0; } @@ -1024,7 +1046,8 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, double maxResolution = 1048576.0; Imagery::ImageryType imageryType = mapNode->getImageryType(); - switch (imageryType) { + switch (imageryType) + { case Imagery::GOOGLE_MAP: minResolution = 0.25; break; @@ -1040,10 +1063,13 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, } double resolution = minResolution; - while (resolution * 2.0 < centerResolution) { + while (resolution * 2.0 < centerResolution) + { resolution *= 2.0; } - if (resolution > maxResolution) { + + if (resolution > maxResolution) + { resolution = maxResolution; } @@ -1057,14 +1083,16 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, zone); // prefetch map tiles - if (resolution / 2.0 >= minResolution) { + if (resolution / 2.0 >= minResolution) + { mapNode->prefetch3D(viewingRadius / 2.0, resolution / 2.0, cameraManipulator->getCenter().y(), cameraManipulator->getCenter().x(), zone); } - if (resolution * 2.0 <= maxResolution) { + if (resolution * 2.0 <= maxResolution) + { mapNode->prefetch3D(viewingRadius * 2.0, resolution * 2.0, cameraManipulator->getCenter().y(), diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.cc b/src/ui/mavlink/QGCMAVLinkMessageSender.cc new file mode 100644 index 0000000000000000000000000000000000000000..630d79af303054fc33378be473ad84320e570000 --- /dev/null +++ b/src/ui/mavlink/QGCMAVLinkMessageSender.cc @@ -0,0 +1,578 @@ +#include "QGCMAVLinkMessageSender.h" +#include "ui_QGCMAVLinkMessageSender.h" +#include "MAVLinkProtocol.h" + +QGCMAVLinkMessageSender::QGCMAVLinkMessageSender(MAVLinkProtocol* mavlink, QWidget *parent) : + QWidget(parent), + protocol(mavlink), + ui(new Ui::QGCMAVLinkMessageSender) +{ + ui->setupUi(this); + mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO; + memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256); + + QStringList header; + header << tr("Name"); + header << tr("Value"); + header << tr("Type"); + ui->treeWidget->setHeaderLabels(header); + createTreeView(); + connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); + refreshTimer.start(1000); // Refresh at 1 Hz interval +} + +void QGCMAVLinkMessageSender::refresh() +{ + unsigned int maxUpdateRate = 0; + // Send messages + foreach (unsigned int i, managementItems.keys()) + { + if (!sendTimers.contains(i)) + { + //sendTimers.insert(i, new QTimer()) + } + } + + // ui->treeWidget->topLevelItem(0)->children(); +} + +bool QGCMAVLinkMessageSender::sendMessage(unsigned int msgid) +{ + bool result = true; + + if (treeWidgetItems.contains(msgid)) + { + // Fill message fields + + mavlink_message_t msg; + QList fields;// = treeWidgetItems.value(msgid)->; + + for (unsigned int i = 0; i < messageInfo[msgid].num_fields; ++i) + { + QTreeWidgetItem* field = fields.at(i); + int fieldid = i; + uint8_t* m = ((uint8_t*)(&msg))+8; + switch (messageInfo[msgid].fields[fieldid].type) + { + case MAVLINK_TYPE_CHAR: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + char* str = (char*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Copy data + QString string = field->data(1, Qt::DisplayRole).toString(); + // Copy string size + int len = qMin((unsigned int)string.length(), messageInfo[msgid].fields[fieldid].array_length); + memcpy(str, string.toStdString().c_str(), len); + // Enforce null termination + str[len-1] = '\0'; + } + else + { + // Single char + char* b = ((char*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + *b = field->data(1, Qt::DisplayRole).toChar().toAscii(); + } + break; + case MAVLINK_TYPE_UINT8_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + uint8_t* nums = m+messageInfo[msgid].fields[fieldid].wire_offset; + for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j) + { + if (field->data(1, Qt::DisplayRole).toString().split(" ").size() > j) + { + nums[j] = field->data(1, Qt::DisplayRole).toString().split(" ").at(j).toInt(); + } + } + } + else + { + // Single value + uint8_t* u = (m+messageInfo[msgid].fields[fieldid].wire_offset); + *u = field->data(1, Qt::DisplayRole).toChar().toAscii(); + } + break; + // case MAVLINK_TYPE_INT8_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // int8_t* nums = (int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // if (field->data(1, DisplayRole).toString().split(" ").size() > j) + // { + // nums[j] = field->data(1, DisplayRole).toString().split(" ").at(j).toInt(); + // } + // } + // item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // int8_t n = *((int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "int8_t"); + // item->setData(1, Qt::DisplayRole, n); + // } + // break; + // case MAVLINK_TYPE_UINT16_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // uint16_t* nums = (uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // uint16_t n = *((uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "uint16_t"); + // item->setData(1, Qt::DisplayRole, n); + // } + // break; + // case MAVLINK_TYPE_INT16_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // int16_t* nums = (int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // int16_t n = *((int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "int16_t"); + // item->setData(1, Qt::DisplayRole, n); + // } + // break; + // case MAVLINK_TYPE_UINT32_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // uint32_t* nums = (uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // float n = *((uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "uint32_t"); + // item->setData(1, Qt::DisplayRole, n); + // } + // break; + // case MAVLINK_TYPE_INT32_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // int32_t* nums = (int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // int32_t n = *((int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "int32_t"); + // item->setData(1, Qt::DisplayRole, n); + // } + // break; + // case MAVLINK_TYPE_FLOAT: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // float* nums = (float*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // float f = *((float*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "float"); + // item->setData(1, Qt::DisplayRole, f); + // } + // break; + // case MAVLINK_TYPE_DOUBLE: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // double* nums = (double*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // double f = *((double*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "double"); + // item->setData(1, Qt::DisplayRole, f); + // } + // break; + // case MAVLINK_TYPE_UINT64_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // uint64_t* nums = (uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // uint64_t n = *((uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "uint64_t"); + // item->setData(1, Qt::DisplayRole, (quint64) n); + // } + // break; + // case MAVLINK_TYPE_INT64_T: + // if (messageInfo[msgid].fields[fieldid].array_length > 0) + // { + // int64_t* nums = (int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // // Enforce null termination + // QString tmp("%1, "); + // QString string; + // for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + // { + // string += tmp.arg(nums[j]); + // } + // item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + // item->setData(1, Qt::DisplayRole, string); + // } + // else + // { + // // Single value + // int64_t n = *((int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + // item->setData(2, Qt::DisplayRole, "int64_t"); + // item->setData(1, Qt::DisplayRole, (qint64) n); + // } + // break; + } + } + } + else + { + result = false; + } + + return result; +} + +QGCMAVLinkMessageSender::~QGCMAVLinkMessageSender() +{ + delete ui; +} + +void QGCMAVLinkMessageSender::createTreeView() +{ + for (int i = 0; i < 256; ++i)//mavlink_message_t msg, receivedMessages) + { + // Update the tree view + QString messageName("%1 (%2 Hz, #%3)"); + float msgHz = messagesHz.value(i, 0); + + // Ignore non-existent messages + if (QString(messageInfo[i].name) == "EMPTY") continue; + + messageName = messageName.arg(messageInfo[i].name).arg(msgHz, 3, 'f', 1).arg(i); + if (!treeWidgetItems.contains(i)) + { + QStringList fields; + fields << messageName; + QTreeWidgetItem* widget = new QTreeWidgetItem(fields); + widget->setFirstColumnSpanned(true); + + for (unsigned int j = 0; j < messageInfo[i].num_fields; ++j) + { + QTreeWidgetItem* field = new QTreeWidgetItem(); + widget->addChild(field); + } + + treeWidgetItems.insert(i, widget); + ui->treeWidget->addTopLevelItem(widget); + + + QTreeWidgetItem* message = widget;//treeWidgetItems.value(msg->msgid); + message->setFirstColumnSpanned(true); + message->setData(0, Qt::DisplayRole, QVariant(messageName)); + for (unsigned int j = 0; j < messageInfo[i].num_fields; ++j) + { + createField(i, j, message->child(j)); + } + // Add management fields, such as update rate and send button + // QTreeWidgetItem* management = new QTreeWidgetItem(); + // widget->addChild(management); + // management->setData(0, Qt::DisplayRole, "Rate:"); + // management->setData(1, Qt::DisplayRole, 0); + // management->setData(2, Qt::DisplayRole, "Hz"); + // managementItems.insert(i, management); + } + } +} + +void QGCMAVLinkMessageSender::createField(int msgid, int fieldid, QTreeWidgetItem* item) +{ + // Add field tree widget item + item->setData(0, Qt::DisplayRole, QVariant(messageInfo[msgid].fields[fieldid].name)); + //uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; + switch (messageInfo[msgid].fields[fieldid].type) + { + case MAVLINK_TYPE_CHAR: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + item->setData(2, Qt::DisplayRole, "char"); + item->setData(1, Qt::DisplayRole, ""); + } + else + { + // Single char + item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, ""); + } + break; + case MAVLINK_TYPE_UINT8_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "uint8_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_INT8_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "int8_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_UINT16_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "uint16_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_INT16_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "int16_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_UINT32_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "uint32_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_INT32_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "int32_t"); + item->setData(1, Qt::DisplayRole, 0); + } + break; + case MAVLINK_TYPE_FLOAT: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0.0f); + } + item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "float"); + item->setData(1, Qt::DisplayRole, 0.0f); + } + break; + case MAVLINK_TYPE_DOUBLE: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "double"); + item->setData(1, Qt::DisplayRole, 0.0); + } + break; + case MAVLINK_TYPE_UINT64_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "uint64_t"); + item->setData(1, Qt::DisplayRole, (quint64) 0); + } + break; + case MAVLINK_TYPE_INT64_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(0); + } + item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + item->setData(2, Qt::DisplayRole, "int64_t"); + item->setData(1, Qt::DisplayRole, (qint64) 0); + } + break; + } +} diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.h b/src/ui/mavlink/QGCMAVLinkMessageSender.h new file mode 100644 index 0000000000000000000000000000000000000000..a9f38703f965509da63f4223638e48d57b8623b5 --- /dev/null +++ b/src/ui/mavlink/QGCMAVLinkMessageSender.h @@ -0,0 +1,48 @@ +#ifndef QGCMAVLINKMESSAGESENDER_H +#define QGCMAVLINKMESSAGESENDER_H + +#include +#include +#include +#include +#include "MAVLinkProtocol.h" + +namespace Ui { +class QGCMAVLinkMessageSender; +} + +class QGCMAVLinkMessageSender : public QWidget +{ + Q_OBJECT + + friend class QTimer; + +public: + explicit QGCMAVLinkMessageSender(MAVLinkProtocol* mavlink, QWidget *parent = 0); + ~QGCMAVLinkMessageSender(); + +protected: + mavlink_message_info_t messageInfo[256]; ///< Meta information about all messages + MAVLinkProtocol* protocol; ///< MAVLink protocol + QMap messagesHz; ///< Used to store update rate in Hz + QTimer refreshTimer; + QMap sendTimers; + QMap managementItems; + QMap treeWidgetItems; ///< Messages + + /** @brief Create the tree view of all messages */ + void createTreeView(); + /** @brief Create one field of one message in the tree view of all messages */ + void createField(int msgid, int fieldid, QTreeWidgetItem* item); + /** @brief Send message with values taken from tree view */ + bool sendMessage(unsigned int id); + +protected slots: + /** @brief Read / display values in UI */ + void refresh(); + +private: + Ui::QGCMAVLinkMessageSender *ui; +}; + +#endif // QGCMAVLINKMESSAGESENDER_H diff --git a/src/ui/mavlink/QGCMAVLinkMessageSender.ui b/src/ui/mavlink/QGCMAVLinkMessageSender.ui new file mode 100644 index 0000000000000000000000000000000000000000..db4aa010c553e90b93ba09a36d128322e66d3b96 --- /dev/null +++ b/src/ui/mavlink/QGCMAVLinkMessageSender.ui @@ -0,0 +1,33 @@ + + + QGCMAVLinkMessageSender + + + + 0 + 0 + 400 + 300 + + + + Form + + + + 6 + + + + + + 1 + + + + + + + + +