diff --git a/src/Core.cc b/src/Core.cc index a2c0138577cfc6fb6d59093e5771a4eea2f075b8..068ea54d414bb6f8269aadc11231e4bd33e87ff1 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -62,7 +62,7 @@ This file is part of the PIXHAWK project Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) { this->setApplicationName("QGroundControl"); - this->setApplicationVersion("v. 0.7.0 (Beta)"); + this->setApplicationVersion("v. 0.7.5 (Beta)"); this->setOrganizationName(QLatin1String("OpenMAV Association")); this->setOrganizationDomain("http://qgroundcontrol.org"); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 215a4a61366399a8ed3e2e53727f281d17dd4e53..06c1d218069786b1c02ba96717e8eeef72d3c6f2 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -170,16 +170,42 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) { case MAV_AUTOPILOT_GENERIC: uas = new UAS(this, message.sysid); + // Connect this robot to the UAS object + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); break; case MAV_AUTOPILOT_PIXHAWK: + { // Fixme differentiate between quadrotor and coaxial here - uas = new PxQuadMAV(this, message.sysid); + PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; case MAV_AUTOPILOT_SLUGS: - uas = new SlugsMAV(this, message.sysid); + { + SlugsMAV* mav = new SlugsMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; case MAV_AUTOPILOT_ARDUPILOT: - uas = new ArduPilotMAV(this, message.sysid); + { + ArduPilotMAV* mav = new ArduPilotMAV(this, message.sysid); + // Connect this robot to the UAS object + // it is IMPORTANT here to use the right object type, + // else the slot of the parent object is called (and thus the special + // packets never reach their goal) + connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } break; default: uas = new UAS(this, message.sysid); @@ -189,8 +215,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) // Make UAS aware that this link can be used to communicate with the actual robot uas->addLink(link); - // Connect this robot to the UAS object - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); } diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index f153e77f2928ea1827ace450da02c7db20e1ac6d..d1a0f3a8140ca7a4a2d7bdd2ad237361a12d8b83 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -120,6 +120,22 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit UAS::valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); } break; + case MAVLINK_MSG_ID_CONTROL_STATUS: + { + mavlink_control_status_t status; + mavlink_msg_control_status_decode(&message, &status); + // Emit control status vector + emit attitudeControlEnabled(static_cast(status.control_att)); + emit positionXYControlEnabled(static_cast(status.control_pos_xy)); + emit positionZControlEnabled(static_cast(status.control_pos_z)); + emit positionYawControlEnabled(static_cast(status.control_pos_yaw)); + + // Emit localization status vector + emit localizationChanged(this, status.position_fix); + emit visionLocalizationChanged(this, status.vision_fix); + emit gpsLocalizationChanged(this, status.gps_fix); + } + break; default: // Do nothing break; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 3662f6b93171bfa26eaef969e72b14f7bbcc9ca2..3ebcbdb2ce7196039456dc497ebb0525e167d5c8 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -316,6 +316,30 @@ signals: void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); /** @brief Emit the new system type */ void systemTypeSet(UASInterface* uas, unsigned int type); + /** @brief Attitude control enabled/disabled */ + void attitudeControlEnabled(bool enabled); + /** @brief Position 2D control enabled/disabled */ + void positionXYControlEnabled(bool enabled); + /** @brief Altitude control enabled/disabled */ + void positionZControlEnabled(bool enabled); + /** @brief Heading control enabled/disabled */ + void positionYawControlEnabled(bool enabled); + + /** + * @brief Localization quality changed + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ + void localizationChanged(UASInterface* uas, int fix); + /** + * @brief GPS localization quality changed + * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization + */ + void gpsLocalizationChanged(UASInterface* uas, int fix); + /** + * @brief Vision localization quality changed + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ + void visionLocalizationChanged(UASInterface* uas, int fix); }; Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0"); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 7c5468c64d010c9fbc7c87e0b0168678d9549d1b..c758e24fbe47a70725bbbacff437e35b431a6492 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -84,6 +84,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) : attControlEnabled(false), xyControlEnabled(false), zControlEnabled(false), + yawControlEnabled(false), + positionFix(0), + gpsFix(0), + visionFix(0), + laserFix(0), mavInitialized(false) { connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); @@ -130,31 +135,16 @@ void HSIDisplay::paintDisplay() // Draw background painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); - // Draw status indicators - QColor statusColor(255, 255, 255); - QString lockStatus; - QString xyContrStatus; - QString zContrStatus; - QString attContrStatus; + // Draw status flags + drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, painter); + drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, painter); + drawStatusFlag(44, 1, tr("PZ"), zControlEnabled, painter); - QColor lockStatusColor; + // Draw position lock indicators + drawPositionLock(2, 5, tr("POS"), positionFix, painter); + drawPositionLock(22, 5, tr("VIS"), positionFix, painter); + drawPositionLock(44, 5, tr("GPS"), positionFix, painter); - if (positionLock) - { - lockStatus = tr("LOCK"); - lockStatusColor = QColor(20, 255, 20); - } - else - { - lockStatus = tr("NO"); - lockStatusColor = QColor(255, 20, 20); - } - - paintText(tr("POS"), QGC::colorCyan, 1.8f, 2.0f, 2.5f, &painter); - painter.setBrush(lockStatusColor); - painter.setPen(Qt::NoPen); - painter.drawRect(QRect(refToScreenX(9.5f), refToScreenY(2.0f), refToScreenX(7.0f), refToScreenY(4.0f))); - paintText(lockStatus, statusColor, 2.8f, 10.0f, 2.0f, &painter); // Draw base instrument // ---------------------- @@ -245,6 +235,61 @@ void HSIDisplay::paintDisplay() // bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet; } +void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter) +{ + paintText(label, QGC::colorCyan, 2.6f, x, y+0.35f, &painter); + QColor statusColor(250, 250, 250); + if(status) + { + painter.setBrush(QGC::colorRed); + } + else + { + painter.setBrush(QGC::colorRed); + } + painter.setPen(Qt::NoPen); + painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); + paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.35f, &painter); +} + +void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, QPainter& painter) +{ + paintText(label, QGC::colorCyan, 2.6f, x, y+0.35f, &painter); + QColor negStatusColor(200, 20, 20); + QColor posStatusColor(20, 200, 20); + QColor statusColor(250, 250, 250); + if(status > 0 && status < 4) + { + painter.setBrush(posStatusColor); + } + else + { + painter.setBrush(negStatusColor); + } + + // Lock text + QString lockText; + switch (status) + { + case 1: + lockText = tr("LOC"); + break; + case 2: + lockText = tr("2D"); + break; + case 3: + lockText = tr("3D"); + break; + default: + lockText = tr("NO"); + break; + } + + painter.setPen(Qt::NoPen); + painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); + paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.35f, &painter); +} + void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock) { Q_UNUSED(uas); @@ -372,6 +417,15 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(UASInterface*,bool))); + connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(UASInterface*,bool))); + connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(UASInterface*,bool))); + connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(UASInterface*,bool))); + + connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int))); + connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); + connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); + // Now connect the new UAS //if (this->uas != uas) @@ -488,6 +542,36 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float az } } +void HSIDisplay::updatePositionYawControllerEnabled(bool enabled) +{ + yawControlEnabled = enabled; +} + +/** + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ +void HSIDisplay::updateLocalization(UASInterface* uas, int fix) +{ + Q_UNUSED(uas); + positionFix = fix; +} +/** + * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization + */ +void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix) +{ + Q_UNUSED(uas); + gpsFix = fix; +} +/** + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ +void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix) +{ + Q_UNUSED(uas); + visionFix = fix; +} + QColor HSIDisplay::getColorForSNR(float snr) { QColor color; @@ -701,6 +785,19 @@ void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, cons // paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); } +/** + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ +void localizationChanged(UASInterface* uas, int fix); +/** + * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization + */ +void gpsLocalizationChanged(UASInterface* uas, int fix); +/** + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ +void visionLocalizationChanged(UASInterface* uas, int fix); + void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat) { @@ -710,3 +807,5 @@ void HSIDisplay::pressKey(int key) { } + + diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 18e7a89523b10b0eb42ec49052901aa274a3d05e..28f7654402d57b1be734bc3280d2a1f2dcd8b58e 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -62,6 +62,24 @@ public slots: void updateAttitudeControllerEnabled(UASInterface* uas, bool enabled); void updatePositionXYControllerEnabled(UASInterface* uas, bool enabled); void updatePositionZControllerEnabled(UASInterface* uas, bool enabled); + /** @brief Heading control enabled/disabled */ + void updatePositionYawControllerEnabled(bool enabled); + + /** + * @brief Localization quality changed + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ + void updateLocalization(UASInterface* uas, int localization); + /** + * @brief GPS localization quality changed + * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization + */ + void updateGpsLocalization(UASInterface* uas, int localization); + /** + * @brief Vision localization quality changed + * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization + */ + void updateVisionLocalization(UASInterface* uas, int localization); void paintEvent(QPaintEvent * event); /** @brief Update state from joystick */ @@ -75,6 +93,8 @@ protected slots: void drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); void drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); void drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); + void drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter); + void drawPositionLock(float x, float y, QString label, int status, QPainter& painter); void setBodySetpointCoordinateXY(double x, double y); void setBodySetpointCoordinateZ(double z); /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */ @@ -195,6 +215,11 @@ protected: bool attControlEnabled; bool xyControlEnabled; bool zControlEnabled; + bool yawControlEnabled; + int positionFix; + int gpsFix; + int visionFix; + int laserFix; bool mavInitialized; ///< The MAV is initialized once the setpoint has been received private: diff --git a/standalone/qgroundcontrol-server/qgroundcontrol-server.pro b/standalone/qgroundcontrol-server/qgroundcontrol-server.pro new file mode 100644 index 0000000000000000000000000000000000000000..8ab537f1f8261ec96c292cf1781967e55fe82523 --- /dev/null +++ b/standalone/qgroundcontrol-server/qgroundcontrol-server.pro @@ -0,0 +1,28 @@ +QT += net + +TEMPLATE = app +TARGET = qgroundcontrol-server + +BASEDIR = ../.. +BUILDDIR = $$BASEDIR/build/qgroundcontrol-server +LANGUAGE = C++ + +CONFIG += release +CONFIG -= debug + +OBJECTS_DIR = $$BUILDDIR/qgroundcontrol-server/obj +MOC_DIR = $$BUILDDIR/qgroundcontrol-server/moc + +macx:DESTDIR = $$BASEDIR/bin/mac + +INCLUDEPATH += $$BASEDIR/. \ + $$BASEDIR/src \ + $$BASEDIR/src/comm \ + $$BASEDIR/standalone/qgroundcontrol-server/src + +HEADERS += src/QGroundControlServer.h \ + $$BASEDIR/src/comm/MAVLinkProtocol.h +SOURCES += src/main.cc \ + src/QGroundControlServer.cc \ + $$BASEDIR/src/comm/MAVLinkProtocol.cc +RESOURCES = $$BASEDIR/mavground.qrc diff --git a/standalone/qgroundcontrol-server/src/QGroundControlServer.cc b/standalone/qgroundcontrol-server/src/QGroundControlServer.cc new file mode 100644 index 0000000000000000000000000000000000000000..86c06d47f3cf932da916989a665b4c3a76d14003 --- /dev/null +++ b/standalone/qgroundcontrol-server/src/QGroundControlServer.cc @@ -0,0 +1,93 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + PIXHAWK is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class MAVLinkGen + * + * @author Lorenz Meier + * + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MAVLinkGen.h" +#include "XMLCommProtocolWidget.h" + + +/** + * @brief Constructor for the main application. + * + * This constructor initializes and starts the whole application. It takes standard + * command-line parameters + * + * @param argc The number of command-line parameters + * @param argv The string array of parameters + **/ + +MAVLinkGen::MAVLinkGen(int &argc, char* argv[]) : QApplication(argc, argv) +{ + this->setApplicationName("MAVLink Generator"); + this->setApplicationVersion("v. 0.1.0 (Beta)"); + this->setOrganizationName(QLatin1String("OpenMAV Association")); + this->setOrganizationDomain("http://qgroundcontrol.org"); + + QSettings::setDefaultFormat(QSettings::IniFormat); + // Exit main application when last window is closed + connect(this, SIGNAL(lastWindowClosed()), this, SLOT(quit())); + + // Set application font + QFontDatabase fontDatabase = QFontDatabase(); + const QString fontFileName = ":/general/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app + const QString fontFamilyName = "Bitstream Vera Sans"; + if(!QFile::exists(fontFileName)) printf("ERROR! font file: %s DOES NOT EXIST!\n", fontFileName.toStdString().c_str()); + fontDatabase.addApplicationFont(fontFileName); + setFont(fontDatabase.font(fontFamilyName, "Roman", 12)); + + // Create main window + QMainWindow* window = new QMainWindow(); + window->setCentralWidget(new XMLCommProtocolWidget(window)); + window->setWindowTitle(applicationName() + " " + applicationVersion()); + window->show(); +} + +/** + * @brief Destructor for the groundstation. It destroys all loaded instances. + * + **/ +MAVLinkGen::~MAVLinkGen() +{ +} + diff --git a/standalone/qgroundcontrol-server/src/QGroundControlServer.h b/standalone/qgroundcontrol-server/src/QGroundControlServer.h new file mode 100644 index 0000000000000000000000000000000000000000..518fed03d6e587b80de55db958e8db7f9714b9c7 --- /dev/null +++ b/standalone/qgroundcontrol-server/src/QGroundControlServer.h @@ -0,0 +1,58 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + PIXHAWK is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of class MAVLinkGen + * + * @author Lorenz Meier + * + */ + + +#ifndef MAVLINKGEN_H +#define MAVLINKGEN_H + +#include + +/** + * @brief The main application and management class. + * + * This class is started by the main method and provides + * the central management unit of the groundstation application. + * + **/ +class MAVLinkGen : public QApplication +{ + Q_OBJECT + +public: + MAVLinkGen(int &argc, char* argv[]); + ~MAVLinkGen(); + +protected: + +private: +}; + +#endif /* MAVLINKGEN_H */ diff --git a/standalone/qgroundcontrol-server/src/main.cc b/standalone/qgroundcontrol-server/src/main.cc new file mode 100644 index 0000000000000000000000000000000000000000..4a57b36902f237b23f4801f2a2e7ec75ab93e164 --- /dev/null +++ b/standalone/qgroundcontrol-server/src/main.cc @@ -0,0 +1,46 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + PIXHAWK is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Main executable + * @author Lorenz Meier + * + */ + +#include +#include "MAVLinkGen.h" + +/** + * @brief Starts the application + * + * @param argc Number of commandline arguments + * @param argv Commandline arguments + * @return exit code, 0 for normal exit and !=0 for error cases + */ +int main(int argc, char *argv[]) +{ + + MAVLinkGen gen(argc, argv); + return gen.exec(); +}