From fe5ec39975805cf7f0bbb7451736dd27a6f8f795 Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Thu, 3 Feb 2011 18:30:47 +0100
Subject: [PATCH] Added angle conversion testing, added battery settings
option, added option to set MAVLink ID of groundstation
---
qgcunittest/UASUnitTest.cc | 236 ++++++++++++++++++++++++++++++----------
qgcunittest/UASUnitTest.h | 57 ++++++----
qgroundcontrol-plugins.pro | 45 --------
src/QGC.cc | 18 ++-
src/QGC.h | 7 +-
src/comm/MAVLinkProtocol.cc | 106 +++++++++++++++---
src/comm/MAVLinkProtocol.h | 9 ++
src/uas/UAS.cc | 80 ++++++++------
src/uas/UAS.h | 12 +-
src/uas/UASInterface.h | 6 +-
src/ui/MAVLinkSettingsWidget.cc | 2 +
src/ui/MAVLinkSettingsWidget.ui | 49 +++++++--
src/ui/MainWindow.cc | 3 +-
src/ui/uas/UASView.cc | 16 +++
src/ui/uas/UASView.h | 3 +
15 files changed, 455 insertions(+), 194 deletions(-)
delete mode 100644 qgroundcontrol-plugins.pro
diff --git a/qgcunittest/UASUnitTest.cc b/qgcunittest/UASUnitTest.cc
index 7aa8537..8b4c6c1 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<int>(UASInterface::COMM_DISCONNECTED));
+ // Verify that upon construction the Comm status is disconnected
+ QCOMPARE(uas->getCommunicationStatus(), static_cast<int>(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 67f1eb6..7bd98bd 100644
--- a/qgcunittest/UASUnitTest.h
+++ b/qgcunittest/UASUnitTest.h
@@ -6,6 +6,7 @@
#include <QtTest/QtTest>
#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 5f00ac2..0000000
--- a/qgroundcontrol-plugins.pro
+++ /dev/null
@@ -1,45 +0,0 @@
-# -------------------------------------------------
-# QGroundControl - Micro Air Vehicle Groundstation
-# Please see our website at <http://qgroundcontrol.org>
-# Author:
-# Lorenz Meier <mavteam@student.ethz.ch>
-# (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 <http://www.gnu.org/licenses/>.
-# -------------------------------------------------
-# 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 04735eb..867be31 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 <qmath.h>
+#include <float.h>
namespace QGC {
@@ -36,7 +37,22 @@ quint64 groundTimeUsecs()
return static_cast<quint64>(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 b777115..0a75b75 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 32c9b50..2a937c4 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -14,8 +14,10 @@
#include <QTime>
#include <QApplication>
#include <QMessageBox>
+#include <QSettings>
+#include <QDesktopServices>
-#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 af1ba18..d99034c 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 6f79c3b..e537208 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 d65d319..77bea1f 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 cf7f65b..15d774e 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 7cb49ec..b63a871 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 5f96e68..1f5a911 100644
--- a/src/ui/MAVLinkSettingsWidget.ui
+++ b/src/ui/MAVLinkSettingsWidget.ui
@@ -6,29 +6,29 @@
<rect>
<x>0</x>
<y>0</y>
- <width>361</width>
- <height>145</height>
+ <width>431</width>
+ <height>243</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="1,100,1">
- <item row="0" column="0" colspan="3">
+ <item row="1" column="0" colspan="3">
<widget class="QCheckBox" name="heartbeatCheckBox">
<property name="text">
<string>Emit heartbeat</string>
</property>
</widget>
</item>
- <item row="1" column="0" colspan="3">
+ <item row="2" column="0" colspan="3">
<widget class="QCheckBox" name="loggingCheckBox">
<property name="text">
<string>Log all MAVLink packets</string>
</property>
</widget>
</item>
- <item row="3" column="0">
+ <item row="4" column="0">
<spacer name="logFileSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -44,14 +44,14 @@
</property>
</spacer>
</item>
- <item row="4" column="0" colspan="3">
+ <item row="5" column="0" colspan="3">
<widget class="QCheckBox" name="versionCheckBox">
<property name="text">
<string>Only accept MAVs with same protocol version</string>
</property>
</widget>
</item>
- <item row="5" column="0">
+ <item row="6" column="0">
<spacer name="versionSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -64,27 +64,56 @@
</property>
</spacer>
</item>
- <item row="5" column="1" colspan="2">
+ <item row="6" column="1" colspan="2">
<widget class="QLabel" name="versionLabel">
<property name="text">
<string>MAVLINK_VERSION: </string>
</property>
</widget>
</item>
- <item row="3" column="1">
+ <item row="4" column="1">
<widget class="QLabel" name="logFileLabel">
<property name="text">
<string>Logfile name</string>
</property>
</widget>
</item>
- <item row="3" column="2">
+ <item row="4" column="2">
<widget class="QPushButton" name="logFileButton">
<property name="text">
<string>Select..</string>
</property>
</widget>
</item>
+ <item row="0" column="2">
+ <widget class="QSpinBox" name="systemIdSpinBox">
+ <property name="toolTip">
+ <string>Set the groundstation number</string>
+ </property>
+ <property name="statusTip">
+ <string>Set the groundstation number</string>
+ </property>
+ <property name="minimum">
+ <number>1</number>
+ </property>
+ <property name="maximum">
+ <number>255</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0" colspan="2">
+ <widget class="QLabel" name="systemIdLabel">
+ <property name="toolTip">
+ <string>The system ID is the number the MAV associates with this computer</string>
+ </property>
+ <property name="statusTip">
+ <string>The system ID is the number the MAV associates with this computer</string>
+ </property>
+ <property name="text">
+ <string>Groundstation MAVLink System ID</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 5033718..6300e6e 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 65f7f66..fb922f4 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 1654452..57b4215 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;