diff --git a/qgcunittest/UASUnitTest.cc b/qgcunittest/UASUnitTest.cc index 7aa85377bda8d35197933167ad631be5d1ece3f3..8b4c6c1b795a756737bd261b98af3d6ba1ac7660 100644 --- a/qgcunittest/UASUnitTest.cc +++ b/qgcunittest/UASUnitTest.cc @@ -6,14 +6,15 @@ UASUnitTest::UASUnitTest() void UASUnitTest::initTestCase() { - mav= new MAVLinkProtocol(); - uas=new UAS(mav,UASID); + mav= new MAVLinkProtocol(); + link = new SerialLink(); + uas=new UAS(mav,UASID); } void UASUnitTest::cleanupTestCase() { - delete uas; - delete mav; + delete uas; + delete mav; } @@ -34,123 +35,244 @@ void UASUnitTest::getUASID_test() void UASUnitTest::getUASName_test() { - // Test that the name is build as MAV + ID - QCOMPARE(uas->getUASName(), "MAV 0" + QString::number(UASID)); + // Test that the name is build as MAV + ID + QCOMPARE(uas->getUASName(), "MAV 0" + QString::number(UASID)); } void UASUnitTest::getUpTime_test() { - UAS* uas2 = new UAS(mav); - // Test that the uptime starts at zero to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0); + UAS* uas2 = new UAS(mav); + // Test that the uptime starts at zero to a + // precision of seconds + QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0); - // Sleep for three seconds - QTest::qSleep(3000); + // Sleep for three seconds + QTest::qSleep(3000); - // Test that the up time is computed correctly to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 3.0); + // Test that the up time is computed correctly to a + // precision of seconds + QCOMPARE(floor(uas2->getUptime()/1000.0), 3.0); - delete uas2; + delete uas2; } void UASUnitTest::getCommunicationStatus_test() { - // Verify that upon construction the Comm status is disconnected - QCOMPARE(uas->getCommunicationStatus(), static_cast(UASInterface::COMM_DISCONNECTED)); + // Verify that upon construction the Comm status is disconnected + QCOMPARE(uas->getCommunicationStatus(), static_cast(UASInterface::COMM_DISCONNECTED)); } void UASUnitTest::filterVoltage_test() { float verificar=uas->filterVoltage(0.4f); - // Verify that upon construction the Comm status is disconnected - QCOMPARE(verificar, 8.52f); + // Verify that upon construction the Comm status is disconnected + QCOMPARE(verificar, 8.52f); } void UASUnitTest:: getAutopilotType_test() { - int verificar=uas->getAutopilotType(); - // Verify that upon construction the autopilot is set to -1 - QCOMPARE(verificar, -1); + int type = uas->getAutopilotType(); + // Verify that upon construction the autopilot is set to -1 + QCOMPARE(type, -1); } void UASUnitTest::setAutopilotType_test() { - uas->setAutopilotType(2); - // Verify that the autopilot is set - QCOMPARE(uas->getAutopilotType(), 2); + uas->setAutopilotType(2); + // Verify that the autopilot is set + QCOMPARE(uas->getAutopilotType(), 2); } void UASUnitTest::getStatusForCode_test() { - QString state, desc; - state = ""; - desc = ""; + QString state, desc; + state = ""; + desc = ""; - uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); + uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); + QVERIFY(state == "UNINIT"); - uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); + uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); + QVERIFY(state == "UNINIT"); - uas->getStatusForCode(MAV_STATE_BOOT, state, desc); - QVERIFY(state == "BOOT"); + uas->getStatusForCode(MAV_STATE_BOOT, state, desc); + QVERIFY(state == "BOOT"); - uas->getStatusForCode(MAV_STATE_CALIBRATING, state, desc); - QVERIFY(state == "CALIBRATING"); + uas->getStatusForCode(MAV_STATE_CALIBRATING, state, desc); + QVERIFY(state == "CALIBRATING"); - uas->getStatusForCode(MAV_STATE_ACTIVE, state, desc); - QVERIFY(state == "ACTIVE"); + uas->getStatusForCode(MAV_STATE_ACTIVE, state, desc); + QVERIFY(state == "ACTIVE"); - uas->getStatusForCode(MAV_STATE_STANDBY, state, desc); - QVERIFY(state == "STANDBY"); + uas->getStatusForCode(MAV_STATE_STANDBY, state, desc); + QVERIFY(state == "STANDBY"); - uas->getStatusForCode(MAV_STATE_CRITICAL, state, desc); - QVERIFY(state == "CRITICAL"); + uas->getStatusForCode(MAV_STATE_CRITICAL, state, desc); + QVERIFY(state == "CRITICAL"); - uas->getStatusForCode(MAV_STATE_EMERGENCY, state, desc); - QVERIFY(state == "EMERGENCY"); + uas->getStatusForCode(MAV_STATE_EMERGENCY, state, desc); + QVERIFY(state == "EMERGENCY"); - uas->getStatusForCode(MAV_STATE_POWEROFF, state, desc); - QVERIFY(state == "SHUTDOWN"); + uas->getStatusForCode(MAV_STATE_POWEROFF, state, desc); + QVERIFY(state == "SHUTDOWN"); - uas->getStatusForCode(5325, state, desc); - QVERIFY(state == "UNKNOWN"); + uas->getStatusForCode(5325, state, desc); + QVERIFY(state == "UNKNOWN"); } void UASUnitTest::getLocalX_test() { - QCOMPARE(uas->getLocalX(), 0.0); + QCOMPARE(uas->getLocalX(), 0.0); } void UASUnitTest::getLocalY_test() { - QCOMPARE(uas->getLocalY(), 0.0); + QCOMPARE(uas->getLocalY(), 0.0); } void UASUnitTest::getLocalZ_test() { - QCOMPARE(uas->getLocalZ(), 0.0); + QCOMPARE(uas->getLocalZ(), 0.0); } void UASUnitTest::getLatitude_test() { QCOMPARE(uas->getLatitude(), 0.0); } void UASUnitTest::getLongitude_test() { - QCOMPARE(uas->getLongitude(), 0.0); + QCOMPARE(uas->getLongitude(), 0.0); } void UASUnitTest::getAltitude_test() { - QCOMPARE(uas->getAltitude(), 0.0); + QCOMPARE(uas->getAltitude(), 0.0); } void UASUnitTest::getRoll_test() { - QCOMPARE(uas->getRoll(), 0.0); + QCOMPARE(uas->getRoll(), 0.0); } void UASUnitTest::getPitch_test() { - QCOMPARE(uas->getPitch(), 0.0); + QCOMPARE(uas->getPitch(), 0.0); } void UASUnitTest::getYaw_test() { - QCOMPARE(uas->getYaw(), 0.0); + QCOMPARE(uas->getYaw(), 0.0); +} +void UASUnitTest::attitudeLimitsZero_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + + // Set zero values and encode them + att.roll = 0.0f; + att.pitch = 0.0f; + att.yaw = 0.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QCOMPARE(uas->getRoll(), 0.0); + QCOMPARE(uas->getPitch(), 0.0); + QCOMPARE(uas->getYaw(), 0.0); +} + +void UASUnitTest::attitudeLimitsPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set PI values and encode them + att.roll = M_PI; + att.pitch = M_PI; + att.yaw = M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < M_PI+0.000001) && (uas->getRoll() > M_PI+-0.000001)); + QVERIFY((uas->getPitch() < M_PI+0.000001) && (uas->getPitch() > M_PI+-0.000001)); + QVERIFY((uas->getYaw() < M_PI+0.000001) && (uas->getYaw() > M_PI+-0.000001)); +} + +void UASUnitTest::attitudeLimitsMinusPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set -PI values and encode them + att.roll = -M_PI; + att.pitch = -M_PI; + att.yaw = -M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() > -M_PI-0.000001) && (uas->getRoll() < -M_PI+0.000001)); + QVERIFY((uas->getPitch() > -M_PI-0.000001) && (uas->getPitch() < -M_PI+0.000001)); + QVERIFY((uas->getYaw() > -M_PI-0.000001) && (uas->getYaw() < -M_PI+0.000001)); +} + +void UASUnitTest::attitudeLimitsTwoPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set off-limit values and check + // correct wrapping + // Set 2PI values and encode them + att.roll = 2*M_PI; + att.pitch = 2*M_PI; + att.yaw = 2*M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); + QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); + QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); +} + +void UASUnitTest::attitudeLimitsMinusTwoPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set -2PI values and encode them + att.roll = -2*M_PI; + att.pitch = -2*M_PI; + att.yaw = -2*M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); + QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); + QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); +} + +void UASUnitTest::attitudeLimitsTwoPiOne_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set over 2 PI values and encode them + att.roll = 2*M_PI+1.0f; + att.pitch = 2*M_PI+1.0f; + att.yaw = 2*M_PI+1.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 1.000001) && (uas->getRoll() > 0.999999)); + QVERIFY((uas->getPitch() < 1.000001) && (uas->getPitch() > 0.999999)); + QVERIFY((uas->getYaw() < 1.000001) && (uas->getYaw() > 0.999999)); +} + +void UASUnitTest::attitudeLimitsMinusTwoPiOne_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set 3PI values and encode them + att.roll = -2*M_PI-1.0f; + att.pitch = -2*M_PI-1.0f; + att.yaw = -2*M_PI-1.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() > -1.000001) && (uas->getRoll() < -0.999999)); + QVERIFY((uas->getPitch() > -1.000001) && (uas->getPitch() < -0.999999)); + QVERIFY((uas->getYaw() > -1.000001) && (uas->getYaw() < -0.999999)); } diff --git a/qgcunittest/UASUnitTest.h b/qgcunittest/UASUnitTest.h index 67f1eb6d826276dee0858a9705e20f5a52332aa0..7bd98bd23ed0db1be3e38460e01970aad85d68b2 100644 --- a/qgcunittest/UASUnitTest.h +++ b/qgcunittest/UASUnitTest.h @@ -6,6 +6,7 @@ #include #include "UAS.h" #include "MAVLinkProtocol.h" +#include "SerialLink.h" #include "UASInterface.h" #include "AutoTest.h" @@ -13,36 +14,44 @@ class UASUnitTest : public QObject { Q_OBJECT public: - #define UASID 50 - MAVLinkProtocol* mav; - UAS* uas; - UASUnitTest(); +#define UASID 50 + MAVLinkProtocol* mav; + UAS* uas; + SerialLink* link; + UASUnitTest(); signals: private slots: - void initTestCase(); - void cleanupTestCase(); - void getUASID_test(); - void getUASName_test(); - void getUpTime_test(); - void getCommunicationStatus_test(); - void filterVoltage_test(); - void getAutopilotType_test(); - void setAutopilotType_test(); - void getStatusForCode_test(); - void getLocalX_test(); - void getLocalY_test(); - void getLocalZ_test(); - void getLatitude_test(); - void getLongitude_test(); - void getAltitude_test(); - void getRoll_test(); - void getPitch_test(); - void getYaw_test(); + void initTestCase(); + void cleanupTestCase(); + void getUASID_test(); + void getUASName_test(); + void getUpTime_test(); + void getCommunicationStatus_test(); + void filterVoltage_test(); + void getAutopilotType_test(); + void setAutopilotType_test(); + void getStatusForCode_test(); + void getLocalX_test(); + void getLocalY_test(); + void getLocalZ_test(); + void getLatitude_test(); + void getLongitude_test(); + void getAltitude_test(); + void getRoll_test(); + void getPitch_test(); + void getYaw_test(); + void attitudeLimitsZero_test(); + void attitudeLimitsPi_test(); + void attitudeLimitsMinusPi_test(); + void attitudeLimitsTwoPi_test(); + void attitudeLimitsMinusTwoPi_test(); + void attitudeLimitsTwoPiOne_test(); + void attitudeLimitsMinusTwoPiOne_test(); protected: - UAS *prueba; + UAS *prueba; }; diff --git a/qgroundcontrol-plugins.pro b/qgroundcontrol-plugins.pro deleted file mode 100644 index 5f00ac29488907920180b16eff1210166faa360b..0000000000000000000000000000000000000000 --- a/qgroundcontrol-plugins.pro +++ /dev/null @@ -1,45 +0,0 @@ -# ------------------------------------------------- -# QGroundControl - Micro Air Vehicle Groundstation -# Please see our website at -# Author: -# Lorenz Meier -# (c) 2009-2010 PIXHAWK Team -# This file is part of the mav groundstation project -# QGroundControl 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. -# QGroundControl is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with QGroundControl. If not, see . -# ------------------------------------------------- -# Include QMapControl map library -# prefer version from external directory / -# from http://github.com/pixhawk/qmapcontrol/ -# over bundled version in lib directory -# Version from GIT repository is preferred -# include ( "../qmapcontrol/QMapControl/QMapControl.pri" ) #{ -# Include bundled version if necessary - - CONFIG += designer plugin - TARGET = $$qtLibraryTarget($$TARGET) - TEMPLATE = lib - QTDIR_build:DESTDIR = $$QT_BUILD_TREE/plugins/designer - - FORMS = src/ui/designer/QGCParamSlider.ui - - HEADERS = src/ui/designer/QGCParamSlider.h \ - src/ui/designer/QGCParamSliderPlugin.h - SOURCES = src/ui/designer/QGCParamSlider.cc \ - src/ui/designer/QGCParamSliderPlugin.cc - - # install - target.path = $$[QT_INSTALL_PLUGINS]/designer - sources.files = $$SOURCES $$HEADERS *.pro - sources.path = $$[QT_INSTALL_EXAMPLES]/designer/worldtimeclockplugin - INSTALLS += target - - symbian: include($$QT_SOURCE_TREE/examples/symbianpkgrules.pri) diff --git a/src/QGC.cc b/src/QGC.cc index 04735eb1f3df10553f6646efe151b66a08bc55b7..867be31d32df2043be1b21d18fee4a4d2cf789ea 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -24,6 +24,7 @@ This file is part of the QGROUNDCONTROL project #include "QGC.h" #include +#include namespace QGC { @@ -36,7 +37,22 @@ quint64 groundTimeUsecs() return static_cast(microseconds + (time.time().msec()*1000)); } -double limitAngleToPMPI(double angle) +float limitAngleToPMPIf(float angle) +{ + while (angle > (M_PI+FLT_EPSILON)) + { + angle -= 2 * M_PI; + } + + while (angle <= -(M_PI+FLT_EPSILON)) + { + angle += 2 * M_PI; + } + + return angle; +} + +double limitAngleToPMPId(double angle) { if (angle < -M_PI) { diff --git a/src/QGC.h b/src/QGC.h index b777115a92c845a5e58939bd5bc90ba03041ca6b..0a75b75cef0bafccec9fd67c9b13e0fa72bd7d1f 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -9,6 +9,9 @@ namespace QGC { + const static int defaultSystemId = 255; + const static int defaultComponentId = 0; + const QColor colorCyan(55, 154, 195); const QColor colorRed(154, 20, 20); const QColor colorGreen(20, 200, 20); @@ -19,7 +22,9 @@ namespace QGC /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); /** @brief Returns the angle limited to -pi - pi */ - double limitAngleToPMPI(double angle); + float limitAngleToPMPIf(float angle); + /** @brief Returns the angle limited to -pi - pi */ + double limitAngleToPMPId(double angle); int applicationVersion(); const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 32c9b500585613f1ab7c07eedf20f43b24f8b779..2a937c45ea1ea5169285e24fa44688adba885ca2 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -14,8 +14,10 @@ #include #include #include +#include +#include -#include "MG.h" +//#include "MG.h" #include "MAVLinkProtocol.h" #include "UASInterface.h" #include "UASManager.h" @@ -40,11 +42,13 @@ MAVLinkProtocol::MAVLinkProtocol() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(false), m_loggingEnabled(false), - m_logfile(new QFile(QCoreApplication::applicationDirPath()+"/mavlink_packetlog.mavlink")), + m_logfile(NULL), m_enable_version_check(true), - versionMismatchIgnore(false) + versionMismatchIgnore(false), + systemId(QGC::defaultSystemId) { - start(QThread::LowPriority); + loadSettings(); + //start(QThread::LowPriority); // Start heartbeat timer, emitting a heartbeat at the configured rate connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat())); heartbeatTimer->start(1000/heartbeatRate); @@ -63,8 +67,53 @@ MAVLinkProtocol::MAVLinkProtocol() : emit versionCheckChanged(m_enable_version_check); } +void MAVLinkProtocol::loadSettings() +{ + // Load defaults from settings + QSettings settings; + settings.sync(); + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + m_heartbeatsEnabled = settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool(); + m_loggingEnabled = settings.value("LOGGING_ENABLED", m_loggingEnabled).toBool(); + m_enable_version_check = settings.value("VERION_CHECK_ENABLED", m_enable_version_check).toBool(); + + // Only set logfile if there is a name present in settings + if (settings.contains("LOGFILE_NAME") && m_logfile == NULL) + { + m_logfile = new QFile(settings.value("LOGFILE_NAME").toString()); + } + + // Only set system id if it was valid + int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt(); + if (temp > 0 && temp < 256) + { + systemId = temp; + } + settings.endGroup(); +} + +void MAVLinkProtocol::storeSettings() +{ + // Store settings + QSettings settings; + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled); + settings.setValue("LOGGING_ENABLED", m_loggingEnabled); + settings.setValue("VERION_CHECK_ENABLED", m_enable_version_check); + settings.setValue("GCS_SYSTEM_ID", systemId); + if (m_logfile) + { + // Logfile exists, store the name + settings.setValue("LOGFILE_NAME", m_logfile->fileName()); + } + settings.endGroup(); + settings.sync(); + //qDebug() << "Storing settings!"; +} + MAVLinkProtocol::~MAVLinkProtocol() { + storeSettings(); if (m_logfile) { m_logfile->flush(); @@ -82,7 +131,14 @@ void MAVLinkProtocol::run() QString MAVLinkProtocol::getLogfileName() { - return m_logfile->fileName(); + if (m_logfile) + { + return m_logfile->fileName(); + } + else + { + return QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "qgrouncontrol_packetlog.mavlink"; + } } /** @@ -254,13 +310,18 @@ QString MAVLinkProtocol::getName() /** @return System id of this application */ int MAVLinkProtocol::getSystemId() { - return MG::SYSTEM::ID; + return systemId; +} + +void MAVLinkProtocol::setSystemId(int id) +{ + systemId = id; } /** @return Component id of this application */ int MAVLinkProtocol::getComponentId() { - return MG::SYSTEM::COMPID; + return QGC::defaultComponentId; } /** @@ -329,21 +390,29 @@ void MAVLinkProtocol::enableLogging(bool enabled) if (enabled) { - if (m_logfile->isOpen()) + if (m_logfile && m_logfile->isOpen()) { m_logfile->flush(); m_logfile->close(); } - if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) + if (m_logfile) { - emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file.").arg(m_logfile->fileName())); - qDebug() << "OPENING LOGFILE FAILED!"; + 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.").arg(m_logfile->fileName())); + qDebug() << "OPENING LOGFILE FAILED!"; + } } } else if (!enabled) { - m_logfile->flush(); - m_logfile->close(); + if (m_logfile) + { + m_logfile->flush(); + m_logfile->close(); + delete m_logfile; + m_logfile = NULL; + } } m_loggingEnabled = enabled; if (changed) emit loggingChanged(enabled); @@ -351,8 +420,15 @@ void MAVLinkProtocol::enableLogging(bool enabled) void MAVLinkProtocol::setLogfileName(const QString& filename) { - m_logfile->flush(); - m_logfile->close(); + if (!m_logfile) + { + m_logfile = new QFile(filename); + } + else + { + m_logfile->flush(); + m_logfile->close(); + } m_logfile->setFileName(filename); enableLogging(m_loggingEnabled); } diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index af1ba184078a927a3b468958c88281ec3539ed9b..d99034cf9445d5c2a2849d30858053cb289521f8 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project #include "ProtocolInterface.h" #include "LinkInterface.h" #include "QGCMAVLink.h" +#include "QGC.h" /** * @brief MAVLink micro air vehicle protocol reference implementation. @@ -84,6 +85,8 @@ public slots: void sendMessage(LinkInterface* link, mavlink_message_t message); /** @brief Set the rate at which heartbeats are emitted */ void setHeartbeatRate(int rate); + /** @brief Set the system id of this application */ + void setSystemId(int id); /** @brief Enable / disable the heartbeat emission */ void enableHeartbeats(bool enabled); @@ -100,6 +103,11 @@ public slots: /** @brief Send an extra heartbeat to all connected units */ void sendHeartbeat(); + /** @brief Load protocol settings */ + void loadSettings(); + /** @brief Store protocol settings */ + void storeSettings(); + protected: QTimer* heartbeatTimer; ///< Timer to emit heartbeats int heartbeatRate; ///< Heartbeat rate, controls the timer interval @@ -114,6 +122,7 @@ protected: int currReceiveCounter; int currLossCounter; bool versionMismatchIgnore; + int systemId; signals: /** @brief Message received and directly copied via signal */ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6f79c3b84d613cf676bb5a1de1f405bb0012e5f1..e537208ca9af0d9a6e567c8c31caca4e53306212 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -106,6 +106,7 @@ void UAS::writeSettings() settings.setValue("NAME", this->name); settings.setValue("AIRFRAME", this->airframe); settings.setValue("AP_TYPE", this->autopilot); + settings.setValue("BATTERY_SPECS", getBatterySpecs()); settings.endGroup(); settings.sync(); } @@ -117,6 +118,10 @@ void UAS::readSettings() this->name = settings.value("NAME", this->name).toString(); this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); + if (settings.contains("BATTERY_SPECS")) + { + setBatterySpecs(settings.value("BATTERY_SPECS").toString()); + } settings.endGroup(); } @@ -382,50 +387,28 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixTime(attitude.usec); - roll = attitude.roll; - pitch = attitude.pitch; - yaw = attitude.yaw; - - roll = QGC::limitAngleToPMPI(roll); - pitch = QGC::limitAngleToPMPI(pitch); - yaw = QGC::limitAngleToPMPI(yaw); - -// emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); -// emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); -// emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "roll", "rad", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); + + roll = QGC::limitAngleToPMPIf(attitude.roll); + pitch = QGC::limitAngleToPMPIf(attitude.pitch); + yaw = QGC::limitAngleToPMPIf(attitude.yaw); + + emit valueChanged(uasId, "roll", "rad", roll, time); + emit valueChanged(uasId, "pitch", "rad", pitch, time); + emit valueChanged(uasId, "yaw", "rad", yaw, time); emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time); // Emit in angles - emit valueChanged(uasId, "roll", "deg", (attitude.roll/M_PI)*180.0, time); - emit valueChanged(uasId, "pitch", "deg", (attitude.pitch/M_PI)*180.0, time); - + emit valueChanged(uasId, "roll", "deg", (roll/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch", "deg", (pitch/M_PI)*180.0, time); + emit valueChanged(uasId, "yaw", "deg", (yaw/M_PI)*180.0, time); emit valueChanged(uasId, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); - - // Force yaw to 180 deg range - double yaw = ((attitude.yaw/M_PI)*180.0); - double sign = 1.0; - if (yaw < 0) - { - sign = -1.0; - yaw = -yaw; - } - while (yaw > 180.0) - { - yaw -= 180.0; - } - - yaw *= sign; - - emit valueChanged(uasId, "yaw", "deg", yaw, time); emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); - emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); + emit attitudeChanged(this, roll, pitch, yaw, time); + emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION: @@ -1774,6 +1757,33 @@ void UAS::setBattery(BatteryType type, int cells) } } +void UAS::setBatterySpecs(const QString& specs) +{ + QString stringList = specs; + stringList = stringList.remove("V"); + stringList = stringList.remove("v"); + QStringList parts = stringList.split(","); + if (parts.length() == 3) + { + float temp; + bool ok; + // Get the empty voltage + temp = parts.at(0).toFloat(&ok); + if (ok) emptyVoltage = temp; + // Get the warning voltage + temp = parts.at(1).toFloat(&ok); + if (ok) warnVoltage = temp; + // Get the full voltage + temp = parts.at(2).toFloat(&ok); + if (ok) fullVoltage = temp; + } +} + +QString UAS::getBatterySpecs() +{ + return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); +} + int UAS::calculateTimeRemaining() { quint64 dt = MG::TIME::getGroundTimeNow() - startTime; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d65d319f00aa942e83f870e9d064983c22fab823..77bea1f89a2dac19e8d1342514be7e4ecdcab870 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -120,9 +120,10 @@ protected: //COMMENTS FOR TEST UNIT double thrustMax; ///< Maximum forward/up thrust of this vehicle, in Newtons // Battery stats - double fullVoltage; ///< Voltage of the fully charged battery (100%) - double emptyVoltage; ///< Voltage of the empty battery (0%) - double startVoltage; ///< Voltage at system start + float fullVoltage; ///< Voltage of the fully charged battery (100%) + float emptyVoltage; ///< Voltage of the empty battery (0%) + float warnVoltage; ///< Voltage where QGC will start to warn about low battery + float startVoltage; ///< Voltage at system start double currentVoltage; ///< Voltage currently measured float lpVoltage; ///< Low-pass filtered voltage int timeRemaining; ///< Remaining time calculated based on previous and current @@ -172,7 +173,6 @@ public: /** @brief Check if vehicle is in autonomous mode */ bool isAuto(); -public: UASWaypointManager* getWaypointManager() { return &waypointManager; } int getSystemType(); int getAutopilotType() {return autopilot;} @@ -188,6 +188,10 @@ public slots: void setUASName(const QString& name); /** @brief Executes an action **/ void setAction(MAV_ACTION action); + /** @brief Set the current battery type and voltages */ + void setBatterySpecs(const QString& specs); + /** @brief Get the current battery type and specs */ + QString getBatterySpecs(); /** @brief Launches the system **/ void launch(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index cf7f65b4891e69ebf0cedb5a359510bdd9129126..15d774ed4cf35b783a290680162bf62a9d314ede 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -269,7 +269,10 @@ public slots: virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; - + /** @brief Set the current battery type and voltages */ + virtual void setBatterySpecs(const QString& specs) = 0; + /** @brief Get the current battery type and specs */ + virtual QString getBatterySpecs() = 0; protected: QColor color; @@ -382,6 +385,7 @@ signals: void thrustChanged(UASInterface*, double thrust); void heartbeat(UASInterface* uas); void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); + void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec); void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 7cb49ec6d639341bb0d004ae507de37c5d982c27..b63a8715ff21e73c86e085ba33e90e9b4c73403d 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -48,6 +48,7 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); + m_ui->systemIdSpinBox->setValue(protocol->getSystemId()); // Connect actions connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool))); @@ -57,6 +58,7 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionCheckBox, SLOT(setChecked(bool))); connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableVersionCheck(bool))); connect(m_ui->logFileButton, SIGNAL(clicked()), this, SLOT(chooseLogfileName())); + connect(m_ui->systemIdSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setSystemId(int))); // Update values m_ui->versionLabel->setText(tr("MAVLINK_VERSION: %1").arg(protocol->getVersion())); diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index 5f96e685ae3652ee9413b42bddcc58f8d3e84ed7..1f5a9117e940982bcf6ddd6f52132d1cdb51fec9 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -6,29 +6,29 @@ 0 0 - 361 - 145 + 431 + 243 Form - + Emit heartbeat - + Log all MAVLink packets - + Qt::Horizontal @@ -44,14 +44,14 @@ - + Only accept MAVs with same protocol version - + Qt::Horizontal @@ -64,27 +64,56 @@ - + MAVLINK_VERSION: - + Logfile name - + Select.. + + + + Set the groundstation number + + + Set the groundstation number + + + 1 + + + 255 + + + + + + + The system ID is the number the MAV associates with this computer + + + The system ID is the number the MAV associates with this computer + + + Groundstation MAVLink System ID + + + diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 50337188cac5d97d665b12437cb439036fcfa69c..6300e6ecb4abd6a03fcefa32caa527ee2a4fee66 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -159,7 +159,7 @@ MainWindow::MainWindow(QWidget *parent): MainWindow::~MainWindow() { - + delete mavlink; } /** @@ -896,6 +896,7 @@ void MainWindow::closeEvent(QCloseEvent *event) settings.setValue(getWindowGeometryKey(), saveGeometry()); //settings.setValue("windowState", saveState()); aboutToCloseFlag = true; + mavlink->storeSettings(); // Save the last current view in any case settings.setValue("CURRENT_VIEW", currentView); // Save the current window state, but only if a system is connected (else no real number of widgets would be present) diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 65f7f66a1e71b29af4e51b9a52f038b4a624fc17..fb922f469b171f37c0a256ca763b3bbe4e18eb68 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -68,6 +68,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : renameAction(new QAction("Rename..", this)), selectAction(new QAction("Control this system", this )), selectAirframeAction(new QAction("Choose Airframe", this)), + setBatterySpecsAction(new QAction("Set Battery Options", this)), m_ui(new Ui::UASView) { m_ui->setupUi(this); @@ -106,6 +107,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); + connect(setBatterySpecsAction, SIGNAL(triggered()), this, SLOT(setBatterySpecs())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); // Name changes @@ -423,9 +425,23 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) menu.addAction(removeAction); } menu.addAction(selectAirframeAction); + menu.addAction(setBatterySpecsAction); menu.exec(event->globalPos()); } +void UASView::setBatterySpecs() +{ + if (uas) + { + bool ok; + QString newName = QInputDialog::getText(this, tr("Set Battery Specifications for %1").arg(uas->getUASName()), + tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V)"), QLineEdit::Normal, + uas->getBatterySpecs(), &ok); + + if (ok && !newName.isEmpty()) uas->setBatterySpecs(newName); + } +} + void UASView::rename() { if (uas) diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 1654452da510a3ff1724c58881be1bf7b891f288..57b4215e7151b8878b644d8a21126d4077c7bf5b 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -84,6 +84,8 @@ public slots: void rename(); /** @brief Select airframe for this vehicle */ void selectAirframe(); + /** @brief Select the battery type */ + void setBatterySpecs(); protected: void changeEvent(QEvent *e); @@ -114,6 +116,7 @@ protected: QAction* renameAction; QAction* selectAction; QAction* selectAirframeAction; + QAction* setBatterySpecsAction; static const int updateInterval = 300;