diff --git a/QGCApplication.pro b/QGCApplication.pro
index 06ecd51971046d7f4cb277f224bcb765d2282419..aa4f0c27a9082017506c5e4bb6bf6140363439a9 100644
--- a/QGCApplication.pro
+++ b/QGCApplication.pro
@@ -356,6 +356,7 @@ HEADERS += \
src/ViewWidgets/CustomCommandWidgetController.h \
src/ViewWidgets/ViewWidgetController.h \
src/Waypoint.h \
+ src/QmlControls/QGCMavManager.h
!AndroidBuild {
HEADERS += \
@@ -485,6 +486,7 @@ SOURCES += \
src/ViewWidgets/CustomCommandWidgetController.cc \
src/ViewWidgets/ViewWidgetController.cc \
src/Waypoint.cc \
+ src/QmlControls/QGCMavManager.cc
!AndroidBuild {
SOURCES += \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 3d7bab682cbb7c65678411303839e77a18e31b7f..de4df1b972f541d700737bea9d49133e123e34c1 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -118,6 +118,8 @@
src/ui/qmlcommon/QGCWaypointEditor.qml
src/ui/qmlcommon/QGCMapToolButton.qml
src/ui/qmlcommon/QGCArtificialHorizon.qml
+
+ src/ui/qmlcommon/QGCWaypoint.qml
src/ui/qmlcommon/compass.svg
src/ui/qmlcommon/compassNeedle.svg
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index 7c6e65fd2d7d4cd09961fbde544e8a187bb1f2b3..e9a4f0e1ccac75b1aef1bee42d6b42fd820d671b 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -58,12 +58,14 @@
#include "QGCTemporaryFile.h"
#include "QGCFileDialog.h"
#include "QGCPalette.h"
-#include "ScreenTools.h"
#include "QGCLoggingCategory.h"
#include "ViewWidgetController.h"
#include "ParameterEditorController.h"
#include "CustomCommandWidgetController.h"
+#include "ScreenTools.h"
+#include "QGCMavManager.h"
+
#ifdef QGC_RTLAB_ENABLED
#include "OpalLink.h"
#endif
@@ -96,6 +98,18 @@ static QObject* screenToolsSingletonFactory(QQmlEngine*, QJSEngine*)
return screenTools;
}
+/**
+ * @brief MavManager creation callback
+ *
+ * This is called by the QtQuick engine for creating the singleton
+**/
+
+static QObject* mavManagerSingletonFactory(QQmlEngine*, QJSEngine*)
+{
+ MavManager* mavManager = new MavManager;
+ return mavManager;
+}
+
/**
* @brief Constructor for the main application.
*
@@ -287,9 +301,11 @@ void QGCApplication::_initCommon(void)
qmlRegisterType("QGroundControl.Controllers", 1, 0, "ViewWidgetController");
qmlRegisterType("QGroundControl.Controllers", 1, 0, "ParameterEditorController");
qmlRegisterType("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController");
-
//-- Create QML Singleton Interfaces
qmlRegisterSingletonType("QGroundControl.ScreenTools", 1, 0, "ScreenTools", screenToolsSingletonFactory);
+ qmlRegisterSingletonType("QGroundControl.MavManager", 1, 0, "MavManager", mavManagerSingletonFactory);
+ //-- Register Waypoint Interface
+ qmlRegisterInterface("Waypoint");
}
bool QGCApplication::_initForNormalAppBoot(void)
diff --git a/src/QmlControls/QGCMavManager.cc b/src/QmlControls/QGCMavManager.cc
new file mode 100644
index 0000000000000000000000000000000000000000..684271347e534841875c17f584b89e5ed6127775
--- /dev/null
+++ b/src/QmlControls/QGCMavManager.cc
@@ -0,0 +1,626 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2015 QGROUNDCONTROL PROJECT
+
+This file is part of the QGROUNDCONTROL 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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief QGC Mav Manager (QML Bindings)
+ * @author Gus Grubba
+ */
+
+#include "MainWindow.h"
+#include "UASManager.h"
+#include "Waypoint.h"
+#include "QGCMavManager.h"
+
+#define UPDATE_TIMER 50
+#define DEFAULT_LAT 38.965767f
+#define DEFAULT_LON -120.083923f
+
+MavManager::MavManager(QObject *parent)
+ : QObject(parent)
+ , _mav(NULL)
+ , _roll(0.0f)
+ , _pitch(0.0f)
+ , _heading(0.0f)
+ , _altitudeAMSL(0.0f)
+ , _altitudeWGS84(0.0f)
+ , _altitudeRelative(0.0f)
+ , _groundSpeed(0.0f)
+ , _airSpeed(0.0f)
+ , _climbRate(0.0f)
+ , _navigationAltitudeError(0.0f)
+ , _navigationSpeedError(0.0f)
+ , _navigationCrosstrackError(0.0f)
+ , _navigationTargetBearing(0.0f)
+ , _latitude(DEFAULT_LAT)
+ , _longitude(DEFAULT_LON)
+ , _refreshTimer(new QTimer(this))
+ , _batteryVoltage(0.0)
+ , _batteryPercent(0.0)
+ , _systemArmed(false)
+ , _currentHeartbeatTimeout(0)
+ , _waypointDistance(0.0)
+ , _currentWaypoint(0)
+ , _satelliteCount(-1)
+ , _satelliteLock(0)
+ , _wpm(NULL)
+ , _updateCount(0)
+{
+ // Connect with UAS signal
+ _setActiveUAS(UASManager::instance()->getActiveUAS());
+ connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_forgetUAS(UASInterface*)));
+ connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*)));
+ // Refresh timer
+ connect(_refreshTimer, SIGNAL(timeout()), this, SLOT(_checkUpdate()));
+ _refreshTimer->setInterval(UPDATE_TIMER);
+ _refreshTimer->start(UPDATE_TIMER);
+ emit heartbeatTimeoutChanged();
+}
+
+MavManager::~MavManager()
+{
+ _refreshTimer->stop();
+}
+
+void MavManager::saveSetting(const QString &name, const QString& value)
+{
+ QSettings settings;
+ settings.setValue(name, value);
+}
+
+QString MavManager::loadSetting(const QString &name, const QString& defaultValue)
+{
+ QSettings settings;
+ return settings.value(name, defaultValue).toString();
+}
+
+void MavManager::_forgetUAS(UASInterface* uas)
+{
+ if (_mav != NULL && _mav == uas) {
+ // Disconnect any previously connected active MAV
+ disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
+ disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
+ disconnect(_mav, SIGNAL(speedChanged (UASInterface*, double, double, quint64)), this, SLOT(_updateSpeed(UASInterface*, double, double, quint64)));
+ disconnect(_mav, SIGNAL(altitudeChanged (UASInterface*, double, double, double, double, quint64)), this, SLOT(_updateAltitude(UASInterface*, double, double, double, double, quint64)));
+ disconnect(_mav, SIGNAL(navigationControllerErrorsChanged (UASInterface*, double, double, double)), this, SLOT(_updateNavigationControllerErrors(UASInterface*, double, double, double)));
+ disconnect(_mav, SIGNAL(statusChanged (UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*,QString,QString)));
+ disconnect(_mav, SIGNAL(armingChanged (bool)), this, SLOT(_updateArmingState(bool)));
+ disconnect(_mav, &UASInterface::NavigationControllerDataChanged, this, &MavManager::_updateNavigationControllerData);
+ disconnect(_mav, &UASInterface::heartbeatTimeout, this, &MavManager::_heartbeatTimeout);
+ disconnect(_mav, &UASInterface::batteryChanged, this, &MavManager::_updateBatteryRemaining);
+ disconnect(_mav, &UASInterface::modeChanged, this, &MavManager::_updateMode);
+ disconnect(_mav, &UASInterface::nameChanged, this, &MavManager::_updateName);
+ disconnect(_mav, &UASInterface::systemTypeSet, this, &MavManager::_setSystemType);
+ disconnect(_mav, &UASInterface::localizationChanged, this, &MavManager::_setSatLoc);
+ if (_wpm) {
+ disconnect(_wpm, &UASWaypointManager::currentWaypointChanged, this, &MavManager::_updateCurrentWaypoint);
+ disconnect(_wpm, &UASWaypointManager::waypointDistanceChanged, this, &MavManager::_updateWaypointDistance);
+ disconnect(_wpm, SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(_waypointViewOnlyListChanged(void)));
+ disconnect(_wpm, SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(_updateWaypointViewOnly(int,Waypoint*)));
+ }
+ UAS* pUas = dynamic_cast(_mav);
+ if(pUas) {
+ disconnect(pUas, &UAS::satelliteCountChanged, this, &MavManager::_setSatelliteCount);
+ }
+ _mav = NULL;
+ _satelliteCount = -1;
+ emit mavPresentChanged();
+ emit satelliteCountChanged();
+ }
+}
+
+void MavManager::_setActiveUAS(UASInterface* uas)
+{
+ if (uas == _mav)
+ return;
+ // Disconnect the previous one (if any)
+ if(_mav) {
+ _forgetUAS(_mav);
+ }
+ if (uas) {
+ // Reset satellite count (no GPS)
+ _satelliteCount = -1;
+ emit satelliteCountChanged();
+ // Reset connection lost (if any)
+ _currentHeartbeatTimeout = 0;
+ emit heartbeatTimeoutChanged();
+ // Set new UAS
+ _mav = uas;
+ // Now connect the new UAS
+ connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
+ connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
+ connect(_mav, SIGNAL(speedChanged (UASInterface*, double, double, quint64)), this, SLOT(_updateSpeed(UASInterface*, double, double, quint64)));
+ connect(_mav, SIGNAL(altitudeChanged (UASInterface*, double, double, double, double, quint64)), this, SLOT(_updateAltitude(UASInterface*, double, double, double, double, quint64)));
+ connect(_mav, SIGNAL(navigationControllerErrorsChanged (UASInterface*, double, double, double)), this, SLOT(_updateNavigationControllerErrors(UASInterface*, double, double, double)));
+ connect(_mav, SIGNAL(statusChanged (UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*, QString,QString)));
+ connect(_mav, SIGNAL(armingChanged (bool)), this, SLOT(_updateArmingState(bool)));
+ connect(_mav, &UASInterface::NavigationControllerDataChanged, this, &MavManager::_updateNavigationControllerData);
+ connect(_mav, &UASInterface::heartbeatTimeout, this, &MavManager::_heartbeatTimeout);
+ connect(_mav, &UASInterface::batteryChanged, this, &MavManager::_updateBatteryRemaining);
+ connect(_mav, &UASInterface::modeChanged, this, &MavManager::_updateMode);
+ connect(_mav, &UASInterface::nameChanged, this, &MavManager::_updateName);
+ connect(_mav, &UASInterface::systemTypeSet, this, &MavManager::_setSystemType);
+ connect(_mav, &UASInterface::localizationChanged, this, &MavManager::_setSatLoc);
+ _wpm = _mav->getWaypointManager();
+ if (_wpm) {
+ connect(_wpm, &UASWaypointManager::currentWaypointChanged, this, &MavManager::_updateCurrentWaypoint);
+ connect(_wpm, &UASWaypointManager::waypointDistanceChanged, this, &MavManager::_updateWaypointDistance);
+ connect(_wpm, SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(_waypointViewOnlyListChanged(void)));
+ connect(_wpm, SIGNAL(waypointViewOnlyChanged(int,Waypoint*)),this, SLOT(_updateWaypointViewOnly(int,Waypoint*)));
+ _wpm->readWaypoints(true);
+ }
+ UAS* pUas = dynamic_cast(_mav);
+ if(pUas) {
+ _setSatelliteCount(pUas->getSatelliteCount(), QString(""));
+ connect(pUas, &UAS::satelliteCountChanged, this, &MavManager::_setSatelliteCount);
+ }
+ _setSystemType(_mav, _mav->getSystemType());
+ _updateArmingState(_mav->isArmed());
+ }
+ emit mavPresentChanged();
+}
+
+void MavManager::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
+{
+ if (isinf(roll)) {
+ _roll = std::numeric_limits::quiet_NaN();
+ } else {
+ float rolldeg = _oneDecimal(roll * (180.0 / M_PI));
+ if (fabs(roll - rolldeg) > 1.0) {
+ _roll = rolldeg;
+ if(_refreshTimer->isActive()) {
+ emit rollChanged();
+ }
+ }
+ if(_roll != rolldeg) {
+ _roll = rolldeg;
+ _addChange(ROLL_CHANGED);
+ }
+ }
+ if (isinf(pitch)) {
+ _pitch = std::numeric_limits::quiet_NaN();
+ } else {
+ float pitchdeg = _oneDecimal(pitch * (180.0 / M_PI));
+ if (fabs(pitch - pitchdeg) > 1.0) {
+ _pitch = pitchdeg;
+ if(_refreshTimer->isActive()) {
+ emit pitchChanged();
+ }
+ }
+ if(_pitch != pitchdeg) {
+ _pitch = pitchdeg;
+ _addChange(PITCH_CHANGED);
+ }
+ }
+ if (isinf(yaw)) {
+ _heading = std::numeric_limits::quiet_NaN();
+ } else {
+ yaw = _oneDecimal(yaw * (180.0 / M_PI));
+ if (yaw < 0) yaw += 360;
+ if (fabs(_heading - yaw) > 1.0) {
+ _heading = yaw;
+ if(_refreshTimer->isActive()) {
+ emit headingChanged();
+ }
+ }
+ if(_heading != yaw) {
+ _heading = yaw;
+ _addChange(HEADING_CHANGED);
+ }
+ }
+}
+
+void MavManager::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp)
+{
+ _updateAttitude(uas, roll, pitch, yaw, timestamp);
+}
+
+void MavManager::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64)
+{
+ groundSpeed = _oneDecimal(groundSpeed);
+ if (fabs(_groundSpeed - groundSpeed) > 0.5) {
+ _groundSpeed = groundSpeed;
+ if(_refreshTimer->isActive()) {
+ emit groundSpeedChanged();
+ }
+ }
+ airSpeed = _oneDecimal(airSpeed);
+ if (fabs(_airSpeed - airSpeed) > 1.0) {
+ _airSpeed = airSpeed;
+ if(_refreshTimer->isActive()) {
+ emit airSpeedChanged();
+ }
+ }
+ if(_groundSpeed != groundSpeed) {
+ _groundSpeed = groundSpeed;
+ _addChange(GROUNDSPEED_CHANGED);
+ }
+ if(_airSpeed != airSpeed) {
+ _airSpeed = airSpeed;
+ _addChange(AIRSPEED_CHANGED);
+ }
+}
+
+void MavManager::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeWGS84, double altitudeRelative, double climbRate, quint64) {
+ altitudeAMSL = _oneDecimal(altitudeAMSL);
+ if (fabs(_altitudeAMSL - altitudeAMSL) > 0.5) {
+ _altitudeAMSL = altitudeAMSL;
+ if(_refreshTimer->isActive()) {
+ emit altitudeAMSLChanged();
+ }
+ }
+ altitudeWGS84 = _oneDecimal(altitudeWGS84);
+ if (fabs(_altitudeWGS84 - altitudeWGS84) > 0.5) {
+ _altitudeWGS84 = altitudeWGS84;
+ if(_refreshTimer->isActive()) {
+ emit altitudeWGS84Changed();
+ }
+ }
+ altitudeRelative = _oneDecimal(altitudeRelative);
+ if (fabs(_altitudeRelative - altitudeRelative) > 0.5) {
+ _altitudeRelative = altitudeRelative;
+ if(_refreshTimer->isActive()) {
+ emit altitudeRelativeChanged();
+ }
+ }
+ climbRate = _oneDecimal(climbRate);
+ if (fabs(_climbRate - climbRate) > 0.5) {
+ _climbRate = climbRate;
+ if(_refreshTimer->isActive()) {
+ emit climbRateChanged();
+ }
+ }
+ if(_altitudeAMSL != altitudeAMSL) {
+ _altitudeAMSL = altitudeAMSL;
+ _addChange(ALTITUDEAMSL_CHANGED);
+ }
+ if(_altitudeWGS84 != altitudeWGS84) {
+ _altitudeWGS84 = altitudeWGS84;
+ _addChange(ALTITUDEWGS84_CHANGED);
+ }
+ if(_altitudeRelative != altitudeRelative) {
+ _altitudeRelative = altitudeRelative;
+ _addChange(ALTITUDERELATIVE_CHANGED);
+ }
+ if(_climbRate != climbRate) {
+ _climbRate = climbRate;
+ _addChange(CLIMBRATE_CHANGED);
+ }
+}
+
+void MavManager::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) {
+ _navigationAltitudeError = altitudeError;
+ _navigationSpeedError = speedError;
+ _navigationCrosstrackError = xtrackError;
+}
+
+void MavManager::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) {
+ if (_mav == uas) {
+ _navigationTargetBearing = targetBearing;
+ }
+}
+
+/*
+ * Internal
+ */
+
+bool MavManager::_isAirplane() {
+ if (_mav)
+ return _mav->isAirplane();
+ return false;
+}
+
+void MavManager::_addChange(int id)
+{
+ if(!_changes.contains(id)) {
+ _changes.append(id);
+ }
+}
+
+float MavManager::_oneDecimal(float value)
+{
+ int i = (value * 10);
+ return (float)i / 10.0;
+}
+
+void MavManager::_checkUpdate()
+{
+ // Update current location
+ if(_mav) {
+ if(_latitude != _mav->getLatitude()) {
+ _latitude = _mav->getLatitude();
+ emit latitudeChanged();
+ }
+ if(_longitude != _mav->getLongitude()) {
+ _longitude = _mav->getLongitude();
+ emit longitudeChanged();
+ }
+ }
+ // The timer rate is 20Hz for the coordinates above. These below we only check
+ // twice a second.
+ if(++_updateCount > 9) {
+ _updateCount = 0;
+ // Check for changes
+ // Significant changes, that is, whole number changes, are updated immediatelly.
+ // For every message however, we set a flag for what changed and this timer updates
+ // them to bring everything up-to-date. This prevents an avalanche of UI updates.
+ foreach(int i, _changes) {
+ switch (i) {
+ case ROLL_CHANGED:
+ emit rollChanged();
+ break;
+ case PITCH_CHANGED:
+ emit pitchChanged();
+ break;
+ case HEADING_CHANGED:
+ emit headingChanged();
+ break;
+ case GROUNDSPEED_CHANGED:
+ emit groundSpeedChanged();
+ break;
+ case AIRSPEED_CHANGED:
+ emit airSpeedChanged();
+ break;
+ case CLIMBRATE_CHANGED:
+ emit climbRateChanged();
+ break;
+ case ALTITUDERELATIVE_CHANGED:
+ emit altitudeRelativeChanged();
+ break;
+ case ALTITUDEWGS84_CHANGED:
+ emit altitudeWGS84Changed();
+ break;
+ case ALTITUDEAMSL_CHANGED:
+ emit altitudeAMSLChanged();
+ break;
+ default:
+ break;
+ }
+ }
+ _changes.clear();
+ }
+}
+
+QString MavManager::getMavIconColor()
+{
+ // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
+ if(_mav)
+ return _mav->getColor().name();
+ else
+ return QString("black");
+}
+
+void MavManager::_updateArmingState(bool armed)
+{
+ if(_systemArmed != armed) {
+ _systemArmed = armed;
+ emit systemArmedChanged();
+ }
+}
+
+void MavManager::_updateBatteryRemaining(UASInterface*, double voltage, double, double percent, int)
+{
+
+ if(percent < 0.0) {
+ percent = 0.0;
+ }
+ if(voltage < 0.0) {
+ voltage = 0.0;
+ }
+ if (_batteryVoltage != voltage) {
+ _batteryVoltage = voltage;
+ emit batteryVoltageChanged();
+ }
+ if (_batteryPercent != percent) {
+ _batteryPercent = percent;
+ emit batteryPercentChanged();
+ }
+}
+
+void MavManager::_updateState(UASInterface*, QString name, QString)
+{
+ if (_currentState != name) {
+ _currentState = name;
+ emit currentStateChanged();
+ }
+}
+
+void MavManager::_updateMode(int, QString name, QString)
+{
+ if (name.size()) {
+ QString shortMode = name;
+ shortMode = shortMode.replace("D|", "");
+ shortMode = shortMode.replace("A|", "");
+ if (_currentMode != shortMode) {
+ _currentMode = shortMode;
+ emit currentModeChanged();
+ }
+ }
+}
+
+void MavManager::_updateName(const QString& name)
+{
+ if (_systemName != name) {
+ _systemName = name;
+ emit systemNameChanged();
+ }
+}
+
+/**
+ * The current system type is represented through the system icon.
+ *
+ * @param uas Source system, has to be the same as this->uas
+ * @param systemType type ID, following the MAVLink system type conventions
+ * @see http://pixhawk.ethz.ch/software/mavlink
+ */
+void MavManager::_setSystemType(UASInterface*, unsigned int systemType)
+{
+ _systemPixmap = "qrc:/res/mavs/";
+ switch (systemType) {
+ case MAV_TYPE_GENERIC:
+ _systemPixmap += "Generic";
+ break;
+ case MAV_TYPE_FIXED_WING:
+ _systemPixmap += "FixedWing";
+ break;
+ case MAV_TYPE_QUADROTOR:
+ _systemPixmap += "QuadRotor";
+ break;
+ case MAV_TYPE_COAXIAL:
+ _systemPixmap += "Coaxial";
+ break;
+ case MAV_TYPE_HELICOPTER:
+ _systemPixmap += "Helicopter";
+ break;
+ case MAV_TYPE_ANTENNA_TRACKER:
+ _systemPixmap += "AntennaTracker";
+ break;
+ case MAV_TYPE_GCS:
+ _systemPixmap += "Groundstation";
+ break;
+ case MAV_TYPE_AIRSHIP:
+ _systemPixmap += "Airship";
+ break;
+ case MAV_TYPE_FREE_BALLOON:
+ _systemPixmap += "FreeBalloon";
+ break;
+ case MAV_TYPE_ROCKET:
+ _systemPixmap += "Rocket";
+ break;
+ case MAV_TYPE_GROUND_ROVER:
+ _systemPixmap += "GroundRover";
+ break;
+ case MAV_TYPE_SURFACE_BOAT:
+ _systemPixmap += "SurfaceBoat";
+ break;
+ case MAV_TYPE_SUBMARINE:
+ _systemPixmap += "Submarine";
+ break;
+ case MAV_TYPE_HEXAROTOR:
+ _systemPixmap += "HexaRotor";
+ break;
+ case MAV_TYPE_OCTOROTOR:
+ _systemPixmap += "OctoRotor";
+ break;
+ case MAV_TYPE_TRICOPTER:
+ _systemPixmap += "TriCopter";
+ break;
+ case MAV_TYPE_FLAPPING_WING:
+ _systemPixmap += "FlappingWing";
+ break;
+ case MAV_TYPE_KITE:
+ _systemPixmap += "Kite";
+ break;
+ default:
+ _systemPixmap += "Unknown";
+ break;
+ }
+ emit systemPixmapChanged();
+}
+
+void MavManager::_heartbeatTimeout(bool timeout, unsigned int ms)
+{
+ unsigned int elapsed = ms;
+ if (!timeout)
+ {
+ elapsed = 0;
+ }
+ if(elapsed != _currentHeartbeatTimeout) {
+ _currentHeartbeatTimeout = elapsed;
+ emit heartbeatTimeoutChanged();
+ }
+}
+
+void MavManager::_setSatelliteCount(double val, QString)
+{
+ // I'm assuming that a negative value or over 99 means there is no GPS
+ if(val < 0.0) val = -1.0;
+ if(val > 99.0) val = -1.0;
+ if(_satelliteCount != (int)val) {
+ _satelliteCount = (int)val;
+ emit satelliteCountChanged();
+ }
+}
+
+void MavManager::_setSatLoc(UASInterface*, int fix)
+{
+ // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock
+ if(_satelliteLock != fix) {
+ _satelliteLock = fix;
+ emit satelliteLockChanged();
+ }
+}
+
+void MavManager::_updateWaypointDistance(double distance)
+{
+ if (_waypointDistance != distance) {
+ _waypointDistance = distance;
+ emit waypointDistanceChanged();
+ }
+}
+
+void MavManager::_updateCurrentWaypoint(quint16 id)
+{
+ if (_currentWaypoint != id) {
+ _currentWaypoint = id;
+ emit currentWaypointChanged();
+ }
+}
+
+void MavManager::_updateWaypointViewOnly(int, Waypoint* /*wp*/)
+{
+ /*
+ bool changed = false;
+ for(int i = 0; i < _waypoints.count(); i++) {
+ if(_waypoints[i].getId() == wp->getId()) {
+ _waypoints[i] = *wp;
+ changed = true;
+ break;
+ }
+ }
+ if(changed) {
+ emit waypointListChanged();
+ }
+ */
+}
+
+void MavManager::_waypointViewOnlyListChanged()
+{
+ if(_wpm) {
+ const QList &waypoints = _wpm->getWaypointViewOnlyList();
+ _waypoints.clear();
+ for(int i = 0; i < waypoints.count(); i++) {
+ Waypoint* wp = waypoints[i];
+ _waypoints.append(new Waypoint(*wp));
+ }
+ emit waypointsChanged();
+ /*
+ if(_longitude == DEFAULT_LON && _latitude == DEFAULT_LAT && _waypoints.length()) {
+ _longitude = _waypoints[0]->getLongitude();
+ _latitude = _waypoints[0]->getLatitude();
+ emit longitudeChanged();
+ emit latitudeChanged();
+ }
+ */
+ }
+}
diff --git a/src/QmlControls/QGCMavManager.h b/src/QmlControls/QGCMavManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..56b25a3f0a90afa92d07096697b6ddc615596515
--- /dev/null
+++ b/src/QmlControls/QGCMavManager.h
@@ -0,0 +1,215 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2015 QGROUNDCONTROL PROJECT
+
+This file is part of the QGROUNDCONTROL 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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief QGC Mav Manager (QML Bindings)
+ * @author Gus Grubba
+ */
+
+#ifndef MAVMANAGER_H
+#define MAVMANAGER_H
+
+#include
+#include
+#include
+#include "Waypoint.h"
+
+class UASInterface;
+class UASWaypointManager;
+
+class MavManager : public QObject
+{
+ Q_OBJECT
+ Q_ENUMS(MessageType_t)
+public:
+ explicit MavManager(QObject *parent = 0);
+ ~MavManager();
+
+ enum {
+ ROLL_CHANGED,
+ PITCH_CHANGED,
+ HEADING_CHANGED,
+ GROUNDSPEED_CHANGED,
+ AIRSPEED_CHANGED,
+ CLIMBRATE_CHANGED,
+ ALTITUDERELATIVE_CHANGED,
+ ALTITUDEWGS84_CHANGED,
+ ALTITUDEAMSL_CHANGED
+ };
+
+ Q_INVOKABLE QString getMavIconColor();
+ Q_INVOKABLE void saveSetting (const QString &key, const QString& value);
+ Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue);
+
+ Q_PROPERTY(float roll READ roll NOTIFY rollChanged)
+ Q_PROPERTY(float pitch READ pitch NOTIFY pitchChanged)
+ Q_PROPERTY(float heading READ heading NOTIFY headingChanged)
+ Q_PROPERTY(float groundSpeed READ groundSpeed NOTIFY groundSpeedChanged)
+ Q_PROPERTY(float airSpeed READ airSpeed NOTIFY airSpeedChanged)
+ Q_PROPERTY(float climbRate READ climbRate NOTIFY climbRateChanged)
+ Q_PROPERTY(float altitudeRelative READ altitudeRelative NOTIFY altitudeRelativeChanged)
+ Q_PROPERTY(float altitudeWGS84 READ altitudeWGS84 NOTIFY altitudeWGS84Changed)
+ Q_PROPERTY(float altitudeAMSL READ altitudeAMSL NOTIFY altitudeAMSLChanged)
+ Q_PROPERTY(float latitude READ latitude NOTIFY latitudeChanged)
+ Q_PROPERTY(float longitude READ longitude NOTIFY longitudeChanged)
+ Q_PROPERTY(bool mavPresent READ mavPresent NOTIFY mavPresentChanged)
+ Q_PROPERTY(double batteryVoltage READ batteryVoltage NOTIFY batteryVoltageChanged)
+ Q_PROPERTY(double batteryPercent READ batteryPercent NOTIFY batteryPercentChanged)
+ Q_PROPERTY(bool systemArmed READ systemArmed NOTIFY systemArmedChanged)
+ Q_PROPERTY(QString currentMode READ currentMode NOTIFY currentModeChanged)
+ Q_PROPERTY(QString systemPixmap READ systemPixmap NOTIFY systemPixmapChanged)
+ Q_PROPERTY(int satelliteCount READ satelliteCount NOTIFY satelliteCountChanged)
+ Q_PROPERTY(QString currentState READ currentState NOTIFY currentStateChanged)
+ Q_PROPERTY(QString systemName READ systemName NOTIFY systemNameChanged)
+ Q_PROPERTY(int satelliteLock READ satelliteLock NOTIFY satelliteLockChanged)
+ Q_PROPERTY(double waypointDistance READ waypointDistance NOTIFY waypointDistanceChanged)
+ Q_PROPERTY(uint16_t currentWaypoint READ currentWaypoint NOTIFY currentWaypointChanged)
+ Q_PROPERTY(unsigned int heartbeatTimeout READ heartbeatTimeout NOTIFY heartbeatTimeoutChanged)
+ Q_PROPERTY(QQmlListProperty waypoints READ waypoints NOTIFY waypointsChanged)
+
+ float roll () { return _roll; }
+ float pitch () { return _pitch; }
+ float heading () { return _heading; }
+ float groundSpeed () { return _groundSpeed; }
+ float airSpeed () { return _airSpeed; }
+ float climbRate () { return _climbRate; }
+ float altitudeRelative () { return _altitudeRelative; }
+ float altitudeWGS84 () { return _altitudeWGS84; }
+ float altitudeAMSL () { return _altitudeAMSL; }
+ float latitude () { return _latitude; }
+ float longitude () { return _longitude; }
+ bool mavPresent () { return _mav != NULL; }
+ int satelliteCount () { return _satelliteCount; }
+ double batteryVoltage () { return _batteryVoltage; }
+ double batteryPercent () { return _batteryPercent; }
+ bool systemArmed () { return _systemArmed; }
+ QString currentMode () { return _currentMode; }
+ QString systemPixmap () { return _systemPixmap; }
+ QString currentState () { return _currentState; }
+ QString systemName () { return _systemName; }
+ int satelliteLock () { return _satelliteLock; }
+ double waypointDistance () { return _waypointDistance; }
+ uint16_t currentWaypoint () { return _currentWaypoint; }
+ unsigned int heartbeatTimeout () { return _currentHeartbeatTimeout; }
+
+ QQmlListProperty waypoints() {return QQmlListProperty(this, _waypoints); }
+
+signals:
+ void rollChanged ();
+ void pitchChanged ();
+ void headingChanged ();
+ void groundSpeedChanged ();
+ void airSpeedChanged ();
+ void climbRateChanged ();
+ void altitudeRelativeChanged();
+ void altitudeWGS84Changed ();
+ void altitudeAMSLChanged ();
+ void latitudeChanged ();
+ void longitudeChanged ();
+ void mavPresentChanged ();
+ void batteryVoltageChanged ();
+ void batteryPercentChanged ();
+ void systemArmedChanged ();
+ void heartbeatTimeoutChanged();
+ void currentModeChanged ();
+ void currentConfigChanged ();
+ void systemPixmapChanged ();
+ void satelliteCountChanged ();
+ void currentStateChanged ();
+ void systemNameChanged ();
+ void satelliteLockChanged ();
+ void waypointDistanceChanged();
+ void currentWaypointChanged ();
+ void waypointsChanged ();
+
+private slots:
+ /** @brief Attitude from main autopilot / system state */
+ void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
+ /** @brief Attitude from one specific component / redundant autopilot */
+ void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
+ /** @brief Speed */
+ void _updateSpeed (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
+ /** @brief Altitude */
+ void _updateAltitude (UASInterface* uas, double _altitudeAMSL, double _altitudeWGS84, double _altitudeRelative, double _climbRate, quint64 timestamp);
+ void _updateNavigationControllerErrors (UASInterface* uas, double altitudeError, double speedError, double xtrackError);
+ void _updateNavigationControllerData (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance);
+ void _forgetUAS (UASInterface* uas);
+ void _setActiveUAS (UASInterface* uas);
+ void _checkUpdate ();
+ void _updateBatteryRemaining (UASInterface*, double voltage, double, double percent, int);
+ void _updateArmingState (bool armed);
+ void _updateState (UASInterface* system, QString name, QString description);
+ void _updateMode (int system, QString name, QString description);
+ void _updateName (const QString& name);
+ void _setSystemType (UASInterface* uas, unsigned int systemType);
+ void _heartbeatTimeout (bool timeout, unsigned int ms);
+ void _updateCurrentWaypoint (quint16 id);
+ void _updateWaypointDistance (double distance);
+ void _setSatelliteCount (double val, QString name);
+ void _setSatLoc (UASInterface* uas, int fix);
+ void _updateWaypointViewOnly (int uas, Waypoint* wp);
+ void _waypointViewOnlyListChanged ();
+
+private:
+ bool _isAirplane ();
+ void _addChange (int id);
+ float _oneDecimal (float value);
+
+private:
+ UASInterface* _mav;
+ float _roll;
+ float _pitch;
+ float _heading;
+ float _altitudeAMSL;
+ float _altitudeWGS84;
+ float _altitudeRelative;
+ float _groundSpeed;
+ float _airSpeed;
+ float _climbRate;
+ float _navigationAltitudeError;
+ float _navigationSpeedError;
+ float _navigationCrosstrackError;
+ float _navigationTargetBearing;
+ float _latitude;
+ float _longitude;
+ QTimer* _refreshTimer;
+ QList _changes;
+ double _batteryVoltage;
+ double _batteryPercent;
+ bool _systemArmed;
+ QString _currentState;
+ QString _currentMode;
+ QString _systemName;
+ QString _systemPixmap;
+ unsigned int _currentHeartbeatTimeout;
+ double _waypointDistance;
+ quint16 _currentWaypoint;
+ int _satelliteCount;
+ int _satelliteLock;
+ UASWaypointManager* _wpm;
+ int _updateCount;
+ QList_waypoints;
+};
+
+#endif // MAVMANAGER_H
diff --git a/src/QmlControls/ScreenTools.h b/src/QmlControls/ScreenTools.h
index e3908e478f399493b3b4cf1a9dfd2fd622455168..d9948d528b5e83f36defa4e1f56b70d6a2519e45 100644
--- a/src/QmlControls/ScreenTools.h
+++ b/src/QmlControls/ScreenTools.h
@@ -45,6 +45,10 @@ class ScreenTools : public QObject
public:
ScreenTools();
+ Q_PROPERTY(bool isAndroid READ isAndroid CONSTANT)
+ Q_PROPERTY(bool isiOS READ isiOS CONSTANT)
+ Q_PROPERTY(bool isMobile READ isMobile CONSTANT)
+
//! Returns the global mouse X position
Q_PROPERTY(int mouseX READ mouseX)
//! Returns the global mouse Y position
@@ -110,6 +114,16 @@ public:
double pixelSizeFactor ();
double defaultFontPointSize(void);
+#if defined (__android__)
+ bool isAndroid () { return true; }
+ bool isiOS () { return false; }
+ bool isMobile () { return true; }
+#else
+ bool isAndroid () { return false; }
+ bool isiOS () { return false; }
+ bool isMobile () { return false; }
+#endif
+
signals:
void repaintRequestedChanged();
void pixelSizeFactorChanged();
diff --git a/src/Waypoint.cc b/src/Waypoint.cc
index 4a96042c9fe5cb4c867ba53bd5580d2e22081975..6c3d2f8ef8fec02c436fdcb2c5537b120b32016b 100644
--- a/src/Waypoint.cc
+++ b/src/Waypoint.cc
@@ -34,43 +34,85 @@ This file is part of the QGROUNDCONTROL project
#include "Waypoint.h"
-Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _param1, double _param2, double _param3, double _param4,
- bool _autocontinue, bool _current, MAV_FRAME _frame, MAV_CMD _action, const QString& _description)
- : id(_id),
- x(_x),
- y(_y),
- z(_z),
- yaw(_param4),
- frame(_frame),
- action(_action),
- autocontinue(_autocontinue),
- current(_current),
- orbit(_param3),
- param1(_param1),
- param2(_param2),
- name(QString("WP%1").arg(id, 2, 10, QChar('0'))),
- description(_description),
- reachedTime(0)
+Waypoint::Waypoint(
+ QObject *parent,
+ quint16 id,
+ double x,
+ double y,
+ double z,
+ double param1,
+ double param2,
+ double param3,
+ double param4,
+ bool autocontinue,
+ bool current,
+ int frame,
+ int action,
+ const QString& description
+ )
+ : QObject(parent)
+ , _id(id)
+ , _x(x)
+ , _y(y)
+ , _z(z)
+ , _yaw(param4)
+ , _frame(frame)
+ , _action(action)
+ , _autocontinue(autocontinue)
+ , _current(current)
+ , _orbit(param3)
+ , _param1(param1)
+ , _param2(param2)
+ , _name(QString("WP%1").arg(id, 2, 10, QChar('0')))
+ , _description(description)
+ , _reachedTime(0)
{
+
+}
+
+Waypoint::Waypoint(const Waypoint& other)
+ : QObject(NULL)
+{
+ *this = other;
}
Waypoint::~Waypoint()
{
}
+const Waypoint& Waypoint::operator=(const Waypoint& other)
+{
+ _id = other.getId();
+ _x = other.getX();
+ _y = other.getY();
+ _z = other.getZ();
+ _yaw = other.getYaw();
+ _frame = other.getFrame();
+ _action = other.getAction();
+ _autocontinue = other.getAutoContinue();
+ _current = other.getCurrent();
+ _orbit = other.getParam3();
+ _param1 = other.getParam1();
+ _param2 = other.getParam2();
+ _name = other.getName();
+ _description = other.getDescription();
+ _reachedTime = other.getReachedTime();
+ return *this;
+}
+
bool Waypoint::isNavigationType()
{
- return (action < MAV_CMD_NAV_LAST);
+ return (_action < MAV_CMD_NAV_LAST);
}
void Waypoint::save(QTextStream &saveStream)
{
QString position("%1\t%2\t%3");
- position = position.arg(x, 0, 'g', 18);
- position = position.arg(y, 0, 'g', 18);
- position = position.arg(z, 0, 'g', 18);
+ position = position.arg(_x, 0, 'g', 18);
+ position = position.arg(_y, 0, 'g', 18);
+ position = position.arg(_z, 0, 'g', 18);
QString parameters("%1\t%2\t%3\t%4");
- parameters = parameters.arg(param1, 0, 'g', 18).arg(param2, 0, 'g', 18).arg(orbit, 0, 'g', 18).arg(yaw, 0, 'g', 18);
+ parameters = parameters.arg(_param1, 0, 'g', 18).arg(_param2, 0, 'g', 18).arg(_orbit, 0, 'g', 18).arg(_yaw, 0, 'g', 18);
// FORMAT:
// as documented here: http://qgroundcontrol.org/waypoint_protocol
saveStream << this->getId() << "\t" << this->getCurrent() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << parameters << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n";
@@ -80,19 +122,19 @@ bool Waypoint::load(QTextStream &loadStream)
{
const QStringList &wpParams = loadStream.readLine().split("\t");
if (wpParams.size() == 12) {
- this->id = wpParams[0].toInt();
- this->current = (wpParams[1].toInt() == 1 ? true : false);
- this->frame = (MAV_FRAME) wpParams[2].toInt();
- this->action = (MAV_CMD) wpParams[3].toInt();
- this->param1 = wpParams[4].toDouble();
- this->param2 = wpParams[5].toDouble();
- this->orbit = wpParams[6].toDouble();
- this->yaw = wpParams[7].toDouble();
- this->x = wpParams[8].toDouble();
- this->y = wpParams[9].toDouble();
- this->z = wpParams[10].toDouble();
- this->autocontinue = (wpParams[11].toInt() == 1 ? true : false);
- //this->description = wpParams[12];
+ _id = wpParams[0].toInt();
+ _current = (wpParams[1].toInt() == 1 ? true : false);
+ _frame = (MAV_FRAME) wpParams[2].toInt();
+ _action = (MAV_CMD) wpParams[3].toInt();
+ _param1 = wpParams[4].toDouble();
+ _param2 = wpParams[5].toDouble();
+ _orbit = wpParams[6].toDouble();
+ _yaw = wpParams[7].toDouble();
+ _x = wpParams[8].toDouble();
+ _y = wpParams[9].toDouble();
+ _z = wpParams[10].toDouble();
+ _autocontinue = (wpParams[11].toInt() == 1 ? true : false);
+ //_description = wpParams[12];
return true;
}
return false;
@@ -101,129 +143,120 @@ bool Waypoint::load(QTextStream &loadStream)
void Waypoint::setId(quint16 id)
{
- this->id = id;
- this->name = QString("WP%1").arg(id, 2, 10, QChar('0'));
+ _id = id;
+ _name = QString("WP%1").arg(id, 2, 10, QChar('0'));
emit changed(this);
}
void Waypoint::setX(double x)
{
- if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
+ if (!isinf(x) && !isnan(x) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
- this->x = x;
+ _x = x;
emit changed(this);
}
}
void Waypoint::setY(double y)
{
- if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
+ if (!isinf(y) && !isnan(y) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
- this->y = y;
+ _y = y;
emit changed(this);
}
}
void Waypoint::setZ(double z)
{
- if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
+ if (!isinf(z) && !isnan(z) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
- this->z = z;
+ _z = z;
emit changed(this);
}
}
void Waypoint::setLatitude(double lat)
{
- if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
+ if (_x != lat && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
- this->x = lat;
+ _x = lat;
emit changed(this);
}
}
void Waypoint::setLongitude(double lon)
{
- if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
+ if (_y != lon && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
- this->y = lon;
+ _y = lon;
emit changed(this);
}
}
void Waypoint::setAltitude(double altitude)
{
- if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
+ if (_z != altitude && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
- this->z = altitude;
+ _z = altitude;
emit changed(this);
}
}
void Waypoint::setYaw(int yaw)
{
- if (this->yaw != yaw)
+ if (_yaw != yaw)
{
- this->yaw = yaw;
+ _yaw = yaw;
emit changed(this);
}
}
void Waypoint::setYaw(double yaw)
{
- if (this->yaw != yaw)
- {
- this->yaw = yaw;
- emit changed(this);
- }
-}
-
-void Waypoint::setAction(int action)
-{
- if (this->action != (MAV_CMD)action)
+ if (_yaw != yaw)
{
- this->action = (MAV_CMD)action;
+ _yaw = yaw;
emit changed(this);
}
}
-void Waypoint::setAction(MAV_CMD action)
+void Waypoint::setAction(int /*MAV_CMD*/ action)
{
- if (this->action != action) {
- this->action = action;
+ if (_action != action) {
+ _action = action;
// Flick defaults according to WP type
- if (this->action == MAV_CMD_NAV_TAKEOFF) {
+ if (_action == MAV_CMD_NAV_TAKEOFF) {
// We default to 15 degrees minimum takeoff pitch
- this->param1 = 15.0;
+ _param1 = 15.0;
}
emit changed(this);
}
}
-void Waypoint::setFrame(MAV_FRAME frame)
+void Waypoint::setFrame(int /*MAV_FRAME*/ frame)
{
- if (this->frame != frame) {
- this->frame = frame;
+ if (_frame != frame) {
+ _frame = frame;
emit changed(this);
}
}
void Waypoint::setAutocontinue(bool autoContinue)
{
- if (this->autocontinue != autoContinue) {
- this->autocontinue = autoContinue;
+ if (_autocontinue != autoContinue) {
+ _autocontinue = autoContinue;
emit changed(this);
}
}
void Waypoint::setCurrent(bool current)
{
- if (this->current != current)
+ if (_current != current)
{
- this->current = current;
+ _current = current;
// The current waypoint index is handled by the list
// and not part of the individual waypoint update state
}
@@ -231,9 +264,9 @@ void Waypoint::setCurrent(bool current)
void Waypoint::setAcceptanceRadius(double radius)
{
- if (this->param2 != radius)
+ if (_param2 != radius)
{
- this->param2 = radius;
+ _param2 = radius;
emit changed(this);
}
}
@@ -242,90 +275,90 @@ void Waypoint::setParam1(double param1)
{
//// // qDebug() << "SENDER:" << QObject::sender();
//// // qDebug() << "PARAM1 SET REQ:" << param1;
- if (this->param1 != param1)
+ if (_param1 != param1)
{
- this->param1 = param1;
+ _param1 = param1;
emit changed(this);
}
}
void Waypoint::setParam2(double param2)
{
- if (this->param2 != param2)
+ if (_param2 != param2)
{
- this->param2 = param2;
+ _param2 = param2;
emit changed(this);
}
}
void Waypoint::setParam3(double param3)
{
- if (this->orbit != param3) {
- this->orbit = param3;
+ if (_orbit != param3) {
+ _orbit = param3;
emit changed(this);
}
}
void Waypoint::setParam4(double param4)
{
- if (this->yaw != param4) {
- this->yaw = param4;
+ if (_yaw != param4) {
+ _yaw = param4;
emit changed(this);
}
}
void Waypoint::setParam5(double param5)
{
- if (this->x != param5) {
- this->x = param5;
+ if (_x != param5) {
+ _x = param5;
emit changed(this);
}
}
void Waypoint::setParam6(double param6)
{
- if (this->y != param6) {
- this->y = param6;
+ if (_y != param6) {
+ _y = param6;
emit changed(this);
}
}
void Waypoint::setParam7(double param7)
{
- if (this->z != param7) {
- this->z = param7;
+ if (_z != param7) {
+ _z = param7;
emit changed(this);
}
}
void Waypoint::setLoiterOrbit(double orbit)
{
- if (this->orbit != orbit) {
- this->orbit = orbit;
+ if (_orbit != orbit) {
+ _orbit = orbit;
emit changed(this);
}
}
void Waypoint::setHoldTime(int holdTime)
{
- if (this->param1 != holdTime) {
- this->param1 = holdTime;
+ if (_param1 != holdTime) {
+ _param1 = holdTime;
emit changed(this);
}
}
void Waypoint::setHoldTime(double holdTime)
{
- if (this->param1 != holdTime) {
- this->param1 = holdTime;
+ if (_param1 != holdTime) {
+ _param1 = holdTime;
emit changed(this);
}
}
void Waypoint::setTurns(int turns)
{
- if (this->param1 != turns) {
- this->param1 = turns;
+ if (_param1 != turns) {
+ _param1 = turns;
emit changed(this);
}
}
diff --git a/src/Waypoint.h b/src/Waypoint.h
index 35999a264872665659cdb099048c28ff0fc205ed..c2618ff7c1f68a21e80a1d1502d916b84c2126dd 100644
--- a/src/Waypoint.h
+++ b/src/Waypoint.h
@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
#include
#include
+#include
#include
#include "QGCMAVLink.h"
#include "QGC.h"
@@ -42,157 +43,193 @@ This file is part of the PIXHAWK project
class Waypoint : public QObject
{
Q_OBJECT
-
public:
- Waypoint(quint16 id = 0, double x = 0.0, double y = 0.0, double z = 0.0, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0,
- bool autocontinue = true, bool current = false, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_CMD action=MAV_CMD_NAV_WAYPOINT, const QString& description=QString(""));
+ Waypoint(
+ QObject *parent = 0,
+ quint16 id = 0,
+ double x = 0.0,
+ double y = 0.0,
+ double z = 0.0,
+ double param1 = 0.0,
+ double param2 = 0.0,
+ double param3 = 0.0,
+ double param4 = 0.0,
+ bool autocontinue = true,
+ bool current = false,
+ int frame = MAV_FRAME_GLOBAL,
+ int action = MAV_CMD_NAV_WAYPOINT,
+ const QString& description=QString(""));
+
+ Waypoint(const Waypoint& other);
~Waypoint();
+ const Waypoint& operator=(const Waypoint& other);
+
+ Q_PROPERTY(double longitude READ longitude NOTIFY longitudeChanged)
+ Q_PROPERTY(double latitude READ latitude NOTIFY latitudeChanged)
+ Q_PROPERTY(double altitude READ altitude NOTIFY altitudeChanged)
+ Q_PROPERTY(quint16 id READ id CONSTANT)
+
+ double latitude() { return _x; }
+ double longitude() { return _y; }
+ double altitude() { return _z; }
+ quint16 id() { return _id; }
+
quint16 getId() const {
- return id;
+ return _id;
}
double getX() const {
- return x;
+ return _x;
}
double getY() const {
- return y;
+ return _y;
}
double getZ() const {
- return z;
+ return _z;
}
double getLatitude() const {
- return x;
+ return _x;
}
double getLongitude() const {
- return y;
+ return _y;
}
double getAltitude() const {
- return z;
+ return _z;
}
double getYaw() const {
- return yaw;
+ return _yaw;
}
bool getAutoContinue() const {
- return autocontinue;
+ return _autocontinue;
}
bool getCurrent() const {
- return current;
+ return _current;
}
double getLoiterOrbit() const {
- return orbit;
+ return _orbit;
}
double getAcceptanceRadius() const {
- return param2;
+ return _param2;
}
double getHoldTime() const {
- return param1;
+ return _param1;
}
double getParam1() const {
- return param1;
+ return _param1;
}
double getParam2() const {
- return param2;
+ return _param2;
}
double getParam3() const {
- return orbit;
+ return _orbit;
}
double getParam4() const {
- return yaw;
+ return _yaw;
}
double getParam5() const {
- return x;
+ return _x;
}
double getParam6() const {
- return y;
+ return _y;
}
double getParam7() const {
- return z;
+ return _z;
}
int getTurns() const {
- return param1;
+ return _param1;
}
- MAV_FRAME getFrame() const {
- return frame;
+ // MAV_FRAME
+ int getFrame() const {
+ return _frame;
}
- MAV_CMD getAction() const {
- return action;
+ // MAV_CMD
+ int getAction() const {
+ return _action;
}
const QString& getName() const {
- return name;
+ return _name;
}
const QString& getDescription() const {
- return description;
+ return _description;
}
/** @brief Returns true if x, y, z contain reasonable navigation data */
bool isNavigationType();
+ /** @brief Get the time this waypoint was reached */
+ quint64 getReachedTime() const { return _reachedTime; }
+
void save(QTextStream &saveStream);
bool load(QTextStream &loadStream);
protected:
- quint16 id;
- double x;
- double y;
- double z;
- double yaw;
- MAV_FRAME frame;
- MAV_CMD action;
- bool autocontinue;
- bool current;
- double orbit;
- double param1;
- double param2;
- int turns;
- QString name;
- QString description;
- quint64 reachedTime;
-
-public slots:
- void setId(quint16 id);
- void setX(double x);
- void setY(double y);
- void setZ(double z);
- void setLatitude(double lat);
- void setLongitude(double lon);
- void setAltitude(double alt);
+ quint16 _id;
+ double _x;
+ double _y;
+ double _z;
+ double _yaw;
+ int _frame;
+ int _action;
+ bool _autocontinue;
+ bool _current;
+ double _orbit;
+ double _param1;
+ double _param2;
+ int _turns;
+ QString _name;
+ QString _description;
+ quint64 _reachedTime;
+
+public:
+ void setId (quint16 _id);
+ void setX (double _x);
+ void setY (double _y);
+ void setZ (double _z);
+ void setLatitude (double lat);
+ void setLongitude (double lon);
+ void setAltitude (double alt);
/** @brief Yaw angle in COMPASS DEGREES: 0-360 */
- void setYaw(int yaw);
+ void setYaw (int _yaw);
/** @brief Yaw angle in COMPASS DEGREES: 0-360 */
- void setYaw(double yaw);
+ void setYaw (double _yaw);
/** @brief Set the waypoint action */
- void setAction(int action);
- void setAction(MAV_CMD action);
- void setFrame(MAV_FRAME frame);
+ void setAction (int _action);
+ void setFrame (int _frame);
void setAutocontinue(bool autoContinue);
- void setCurrent(bool current);
- void setLoiterOrbit(double orbit);
- void setParam1(double param1);
- void setParam2(double param2);
- void setParam3(double param3);
- void setParam4(double param4);
- void setParam5(double param5);
- void setParam6(double param6);
- void setParam7(double param7);
+ void setCurrent (bool _current);
+ void setLoiterOrbit (double _orbit);
+ void setParam1 (double _param1);
+ void setParam2 (double _param2);
+ void setParam3 (double param3);
+ void setParam4 (double param4);
+ void setParam5 (double param5);
+ void setParam6 (double param6);
+ void setParam7 (double param7);
void setAcceptanceRadius(double radius);
- void setHoldTime(int holdTime);
- void setHoldTime(double holdTime);
+ void setHoldTime (int holdTime);
+ void setHoldTime (double holdTime);
/** @brief Number of turns for loiter waypoints */
- void setTurns(int turns);
+ void setTurns (int _turns);
/** @brief Set waypoint as reached */
- void setReached() { reachedTime = QGC::groundTimeMilliseconds(); }
+ void setReached () { _reachedTime = QGC::groundTimeMilliseconds(); }
/** @brief Wether this waypoint has been reached yet */
- bool isReached() { return (reachedTime > 0); }
- /** @brief Get the time this waypoint was reached */
- quint64 getReachedTime() { return reachedTime; }
+ bool isReached () { return (_reachedTime > 0); }
+
void setChanged() {
emit changed(this);
}
signals:
/** @brief Announces a change to the waypoint data */
- void changed(Waypoint* wp);
+ void changed(Waypoint* wp);
+
+ void latitudeChanged ();
+ void longitudeChanged ();
+ void altitudeChanged ();
+
};
+QML_DECLARE_TYPE(Waypoint)
+
#endif // WAYPOINT_H
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 5a05ec7031295e3195d71d1b72b5abb16bdd6653..3b5206dbc6d42fcaae45c1f60a03398d409de749 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -179,12 +179,37 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id) {
- Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
- addWaypointViewOnly(lwp_vo);
+ Waypoint *lwp_vo = new Waypoint(
+ NULL,
+ wp->seq, wp->x,
+ wp->y,
+ wp->z,
+ wp->param1,
+ wp->param2,
+ wp->param3,
+ wp->param4,
+ wp->autocontinue,
+ wp->current,
+ (MAV_FRAME) wp->frame,
+ (MAV_CMD) wp->command);
+ addWaypointViewOnly(lwp_vo);
if (read_to_edit == true) {
- Waypoint *lwp_ed = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
+ Waypoint *lwp_ed = new Waypoint(
+ NULL,
+ wp->seq,
+ wp->x,
+ wp->y,
+ wp->z,
+ wp->param1,
+ wp->param2,
+ wp->param3,
+ wp->param4,
+ wp->autocontinue,
+ wp->current,
+ (MAV_FRAME) wp->frame,
+ (MAV_CMD) wp->command);
addWaypointEditable(lwp_ed, false);
if (wp->current == 1) currentWaypointEditable = lwp_ed;
}
@@ -216,7 +241,20 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
&& wp->seq < waypointsViewOnly.size() && waypointsViewOnly[wp->seq]->getAction()) {
// accept single sent waypoints because they can contain updates about remaining DO_JUMP repetitions
// but only update view only side
- Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
+ Waypoint *lwp_vo = new Waypoint(
+ NULL,
+ wp->seq,
+ wp->x,
+ wp->y,
+ wp->z,
+ wp->param1,
+ wp->param2,
+ wp->param3,
+ wp->param4,
+ wp->autocontinue,
+ wp->current,
+ (MAV_FRAME) wp->frame,
+ (MAV_CMD) wp->command);
waypointsViewOnly.replace(wp->seq, lwp_vo);
emit waypointViewOnlyListChanged();
diff --git a/src/ui/WaypointEditableView.cc b/src/ui/WaypointEditableView.cc
index d7bf35e9df8e18637183e634ec0b87e0c714dad4..a965dbef2d760ddd783d03854e8695109e3bd229 100644
--- a/src/ui/WaypointEditableView.cc
+++ b/src/ui/WaypointEditableView.cc
@@ -417,14 +417,14 @@ void WaypointEditableView::updateValues()
// update frame
- MAV_FRAME frame = wp->getFrame();
+ MAV_FRAME frame = (MAV_FRAME)wp->getFrame();
int frame_index = m_ui->comboBox_frame->findData(frame);
if (m_ui->comboBox_frame->currentIndex() != frame_index) {
m_ui->comboBox_frame->setCurrentIndex(frame_index);
}
// Update action
- MAV_CMD action = wp->getAction();
+ MAV_CMD action = (MAV_CMD)wp->getAction();
int action_index = m_ui->comboBox_action->findData(action);
if (m_ui->comboBox_action->currentIndex() != action_index)
{
@@ -441,7 +441,7 @@ void WaypointEditableView::updateValues()
}
emit commandBroadcast(wp->getAction());
- emit frameBroadcast(wp->getFrame());
+ emit frameBroadcast((MAV_FRAME)wp->getFrame());
emit param1Broadcast(wp->getParam1());
emit param2Broadcast(wp->getParam2());
emit param3Broadcast(wp->getParam3());
diff --git a/src/ui/flightdisplay/FlightDisplay.qml b/src/ui/flightdisplay/FlightDisplay.qml
index d697af98dc200c03de92ea5be23672dee4c1ec42..074f9a253031523fbb0dddb98fec8653f840465a 100644
--- a/src/ui/flightdisplay/FlightDisplay.qml
+++ b/src/ui/flightdisplay/FlightDisplay.qml
@@ -33,6 +33,7 @@ import QtQuick.Controls.Styles 1.2
import QtQuick.Dialogs 1.2
import QGroundControl.FlightControls 1.0
+import QGroundControl.MavManager 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0
@@ -42,8 +43,8 @@ Item {
property var __qgcPal: QGCPalette { colorGroupEnabled: enabled }
- property real roll: isNaN(flightDisplay.roll) ? 0 : flightDisplay.roll
- property real pitch: isNaN(flightDisplay.pitch) ? 0 : flightDisplay.pitch
+ property real roll: isNaN(MavManager.roll) ? 0 : MavManager.roll
+ property real pitch: isNaN(MavManager.pitch) ? 0 : MavManager.pitch
property bool showPitchIndicator: true
property bool showAttitudeIndicator: true
@@ -67,6 +68,7 @@ Item {
Component.onCompleted:
{
mapBackground.visible = getBool(flightDisplay.loadSetting("showMapBackground", "0"));
+ mapBackground.showWaypoints = getBool(flightDisplay.loadSetting("mapShowWaypoints", "0"));
mapBackground.alwaysNorth = getBool(flightDisplay.loadSetting("mapAlwaysPointsNorth", "0"));
showAttitudeIndicator = getBool(flightDisplay.loadSetting("showAttitudeIndicator", "1"));
showPitchIndicator = getBool(flightDisplay.loadSetting("showPitchIndicator", "1"));
@@ -77,7 +79,8 @@ Item {
currentSpeed.showGroundSpeed = getBool(flightDisplay.loadSetting("showCurrentGroundSpeed", "1"));
currentAltitude.showClimbRate = getBool(flightDisplay.loadSetting("showCurrentClimbRate", "1"));
currentAltitude.showAltitude = getBool(flightDisplay.loadSetting("showCurrentAltitude", "1"));
- mapTypeMenu.update();
+ // Insert Map Type menu before separator
+ contextMenu.insertItem(2, mapBackground.mapMenu);
}
// TODO: This is to replace the context menu but it is not working. Not only the buttons don't show,
@@ -110,6 +113,8 @@ Item {
flightDisplay.saveSetting("showMapBackground", setBool(mapBackground.visible));
mapBackground.alwaysNorth = false;
flightDisplay.saveSetting("mapAlwaysPointsNorth", setBool(mapBackground.alwaysNorth));
+ mapBackground.showWaypoints = false
+ flightDisplay.saveSetting("mapShowWaypoints", setBool(mapBackground.showWaypoints));
}
contentItem: Rectangle {
color: __qgcPal.window
@@ -134,6 +139,15 @@ Item {
flightDisplay.saveSetting("showMapBackground", setBool(mapBackground.visible));
}
}
+ QGCCheckBox {
+ text: "Map Show Waypoints"
+ checked: mapBackground.showWaypoints
+ onClicked:
+ {
+ mapBackground.showWaypoints = !mapBackground.showWaypoints;
+ flightDisplay.saveSetting("mapShowWaypoints", setBool(mapBackground.showWaypoints));
+ }
+ }
QGCCheckBox {
text: "Pitch Indicator"
checked: showPitchIndicator
@@ -291,6 +305,17 @@ Item {
}
}
+ MenuItem {
+ text: "Map Show Waypoints"
+ checkable: true
+ checked: mapBackground.showWaypoints
+ onTriggered:
+ {
+ mapBackground.showWaypoints = !mapBackground.showWaypoints;
+ flightDisplay.saveSetting("mapShowWaypoints", setBool(mapBackground.showWaypoints));
+ }
+ }
+
/*
MenuItem {
text: "Options Dialog"
@@ -314,42 +339,6 @@ Item {
}
*/
- Menu {
- id: mapTypeMenu
- title: "Map Type..."
- ExclusiveGroup { id: currMapType }
- function setCurrentMap(map) {
- for (var i = 0; i < mapBackground.mapItem.supportedMapTypes.length; i++) {
- if (map === mapBackground.mapItem.supportedMapTypes[i].name) {
- mapBackground.mapItem.activeMapType = mapBackground.mapItem.supportedMapTypes[i]
- flightDisplay.saveSetting("currentMapType", map);
- return;
- }
- }
- }
- function addMap(map, checked) {
- var mItem = mapTypeMenu.addItem(map);
- mItem.checkable = true
- mItem.checked = checked
- mItem.exclusiveGroup = currMapType
- var menuSlot = function() {setCurrentMap(map);};
- mItem.triggered.connect(menuSlot);
- }
- function update() {
- clear()
- var map = ''
- if (mapBackground.mapItem.supportedMapTypes.length > 0)
- map = mapBackground.mapItem.activeMapType.name;
- map = flightDisplay.loadSetting("currentMapType", map);
- for (var i = 0; i < mapBackground.mapItem.supportedMapTypes.length; i++) {
- var name = mapBackground.mapItem.supportedMapTypes[i].name;
- addMap(name, map === name);
- }
- if(map != '')
- setCurrentMap(map);
- }
- }
-
MenuSeparator {}
MenuItem {
@@ -479,6 +468,8 @@ Item {
flightDisplay.saveSetting("showMapBackground", setBool(mapBackground.visible));
mapBackground.alwaysNorth = false;
flightDisplay.saveSetting("mapAlwaysPointsNorth", setBool(mapBackground.alwaysNorth));
+ mapBackground.showWaypoints = false
+ flightDisplay.saveSetting("mapShowWaypoints", setBool(mapBackground.showWaypoints));
}
}
@@ -487,10 +478,12 @@ Item {
QGCMapBackground {
id: mapBackground
anchors.fill: parent
- heading: 0 // isNaN(flightDisplay.heading) ? 0 : flightDisplay.heading
- latitude: mapBackground.visible ? ((flightDisplay.latitude === 0) ? 37.803784 : flightDisplay.latitude) : 37.803784
- longitude: mapBackground.visible ? ((flightDisplay.longitude === 0) ? -122.462276 : flightDisplay.longitude) : -122.462276
- interactive: !flightDisplay.mavPresent
+ mapName: 'MainFlightDisplay'
+ heading: 0 // isNaN(MavManager.heading) ? 0 : MavManager.heading
+ latitude: mapBackground.visible ? ((MavManager.latitude === 0) ? 37.803784 : MavManager.latitude) : 37.803784
+ longitude: mapBackground.visible ? ((MavManager.longitude === 0) ? -122.462276 : MavManager.longitude) : -122.462276
+ readOnly: true
+ //interactive: !MavManager.mavPresent
z: 10
}
@@ -499,7 +492,7 @@ Item {
y: ScreenTools.pixelSizeFactor * (5)
x: ScreenTools.pixelSizeFactor * (85)
size: ScreenTools.pixelSizeFactor * (160)
- heading: isNaN(flightDisplay.heading) ? 0 : flightDisplay.heading
+ heading: isNaN(MavManager.heading) ? 0 : MavManager.heading
visible: mapBackground.visible && showCompass
z: mapBackground.z + 1
onResetRequested: {
@@ -567,7 +560,7 @@ Item {
anchors.right: parent.right
width: ScreenTools.pixelSizeFactor * (60)
height: parent.height * 0.65 > ScreenTools.pixelSizeFactor * (280) ? ScreenTools.pixelSizeFactor * (280) : parent.height * 0.65
- altitude: flightDisplay.altitudeWGS84
+ altitude: MavManager.altitudeWGS84
z: 30
}
@@ -576,7 +569,7 @@ Item {
anchors.left: parent.left
width: ScreenTools.pixelSizeFactor * (60)
height: parent.height * 0.65 > ScreenTools.pixelSizeFactor * (280) ? ScreenTools.pixelSizeFactor * (280) : parent.height * 0.65
- speed: flightDisplay.groundSpeed
+ speed: MavManager.groundSpeed
z: 40
}
@@ -584,8 +577,8 @@ Item {
id: currentSpeed
anchors.left: parent.left
width: ScreenTools.pixelSizeFactor * (75)
- airspeed: flightDisplay.airSpeed
- groundspeed: flightDisplay.groundSpeed
+ airspeed: MavManager.airSpeed
+ groundspeed: MavManager.groundSpeed
showAirSpeed: true
showGroundSpeed: true
visible: (currentSpeed.showGroundSpeed || currentSpeed.showAirSpeed)
@@ -596,8 +589,8 @@ Item {
id: currentAltitude
anchors.right: parent.right
width: ScreenTools.pixelSizeFactor * (75)
- altitude: flightDisplay.altitudeWGS84
- vertZ: flightDisplay.climbRate
+ altitude: MavManager.altitudeWGS84
+ vertZ: MavManager.climbRate
showAltitude: true
showClimbRate: true
visible: (currentAltitude.showAltitude || currentAltitude.showClimbRate)
@@ -610,7 +603,7 @@ Item {
x: root.width * 0.5 - ScreenTools.pixelSizeFactor * (60)
width: ScreenTools.pixelSizeFactor * (120)
height: ScreenTools.pixelSizeFactor * (120)
- heading: isNaN(flightDisplay.heading) ? 0 : flightDisplay.heading
+ heading: isNaN(MavManager.heading) ? 0 : MavManager.heading
visible: !mapBackground.visible && showCompass
z: 70
}
@@ -618,6 +611,7 @@ Item {
//- Context Menu
MouseArea {
anchors.fill: parent
+ z: 1000
acceptedButtons: Qt.RightButton
onClicked: {
if (mouse.button == Qt.RightButton)
diff --git a/src/ui/flightdisplay/QGCFlightDisplay.cc b/src/ui/flightdisplay/QGCFlightDisplay.cc
index bdd40b9b1dbbd0c28670d36684494bdf961c7dc1..cd480c50e976ce3887db5136401ca163b5c6fc9d 100644
--- a/src/ui/flightdisplay/QGCFlightDisplay.cc
+++ b/src/ui/flightdisplay/QGCFlightDisplay.cc
@@ -35,29 +35,10 @@ This file is part of the QGROUNDCONTROL project
#include "QGCFlightDisplay.h"
#include "UASManager.h"
-#define UPDATE_TIMER 500
-
const char* kMainFlightDisplayGroup = "MainFlightDisplay";
QGCFlightDisplay::QGCFlightDisplay(QWidget *parent)
: QGCQmlWidgetHolder(parent)
- , _mav(NULL)
- , _roll(0.0f)
- , _pitch(0.0f)
- , _heading(0.0f)
- , _altitudeAMSL(0.0f)
- , _altitudeWGS84(0.0f)
- , _altitudeRelative(0.0f)
- , _groundSpeed(0.0f)
- , _airSpeed(0.0f)
- , _climbRate(0.0f)
- , _navigationAltitudeError(0.0f)
- , _navigationSpeedError(0.0f)
- , _navigationCrosstrackError(0.0f)
- , _navigationTargetBearing(0.0f)
- , _latitude(37.803784f)
- , _longitude(-122.462276f)
- , _refreshTimer(new QTimer(this))
{
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
setObjectName("MainFlightDisplay");
@@ -73,18 +54,10 @@ QGCFlightDisplay::QGCFlightDisplay(QWidget *parent)
setContextPropertyObject("flightDisplay", this);
setSource(QUrl::fromUserInput("qrc:/qml/FlightDisplay.qml"));
setVisible(true);
- // Connect with UAS signal
- _setActiveUAS(UASManager::instance()->getActiveUAS());
- connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_forgetUAS(UASInterface*)));
- connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*)));
- // Refresh timer
- _refreshTimer->setInterval(UPDATE_TIMER);
- connect(_refreshTimer, SIGNAL(timeout()), this, SLOT(_checkUpdate()));
}
QGCFlightDisplay::~QGCFlightDisplay()
{
- _refreshTimer->stop();
}
void QGCFlightDisplay::saveSetting(const QString &name, const QString& value)
@@ -102,279 +75,3 @@ QString QGCFlightDisplay::loadSetting(const QString &name, const QString& defaul
key += "/" + name;
return settings.value(key, defaultValue).toString();
}
-
-void QGCFlightDisplay::_forgetUAS(UASInterface* uas)
-{
- if (_mav != NULL && _mav == uas) {
- // Disconnect any previously connected active MAV
- disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
- disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
- disconnect(_mav, SIGNAL(speedChanged (UASInterface*, double, double, quint64)), this, SLOT(_updateSpeed(UASInterface*, double, double, quint64)));
- disconnect(_mav, SIGNAL(altitudeChanged (UASInterface*, double, double, double, double, quint64)), this, SLOT(_updateAltitude(UASInterface*, double, double, double, double, quint64)));
- disconnect(_mav, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(_updateNavigationControllerErrors(UASInterface*, double, double, double)));
- disconnect(_mav, &UASInterface::NavigationControllerDataChanged, this, &QGCFlightDisplay::_updateNavigationControllerData);
- }
- _mav = NULL;
- emit mavPresentChanged();
-}
-
-void QGCFlightDisplay::_setActiveUAS(UASInterface* uas)
-{
- if (uas == _mav)
- return;
- // Disconnect the previous one (if any)
- if(_mav) {
- _forgetUAS(_mav);
- }
- if (uas) {
- // Now connect the new UAS
- // Setup communication
- connect(uas, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
- connect(uas, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
- connect(uas, SIGNAL(speedChanged (UASInterface*, double, double, quint64)), this, SLOT(_updateSpeed(UASInterface*, double, double, quint64)));
- connect(uas, SIGNAL(altitudeChanged (UASInterface*, double, double, double, double, quint64)), this, SLOT(_updateAltitude(UASInterface*, double, double, double, double, quint64)));
- connect(uas, SIGNAL(navigationControllerErrorsChanged (UASInterface*, double, double, double)), this, SLOT(_updateNavigationControllerErrors(UASInterface*, double, double, double)));
- connect(uas, &UASInterface::NavigationControllerDataChanged, this, &QGCFlightDisplay::_updateNavigationControllerData);
- // Set new UAS
- _mav = uas;
- }
- emit mavPresentChanged();
-}
-
-void QGCFlightDisplay::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
-{
- if (isinf(roll)) {
- _roll = std::numeric_limits::quiet_NaN();
- } else {
- float rolldeg = _oneDecimal(roll * (180.0 / M_PI));
- if (fabs(roll - rolldeg) > 1.0) {
- _roll = rolldeg;
- if(_refreshTimer->isActive()) {
- emit rollChanged();
- }
- }
- if(_roll != rolldeg) {
- _roll = rolldeg;
- _addChange(ROLL_CHANGED);
- }
- }
- if (isinf(pitch)) {
- _pitch = std::numeric_limits::quiet_NaN();
- } else {
- float pitchdeg = _oneDecimal(pitch * (180.0 / M_PI));
- if (fabs(pitch - pitchdeg) > 1.0) {
- _pitch = pitchdeg;
- if(_refreshTimer->isActive()) {
- emit pitchChanged();
- }
- }
- if(_pitch != pitchdeg) {
- _pitch = pitchdeg;
- _addChange(PITCH_CHANGED);
- }
- }
- if (isinf(yaw)) {
- _heading = std::numeric_limits::quiet_NaN();
- } else {
- yaw = _oneDecimal(yaw * (180.0 / M_PI));
- if (yaw < 0) yaw += 360;
- if (fabs(_heading - yaw) > 1.0) {
- _heading = yaw;
- if(_refreshTimer->isActive()) {
- emit headingChanged();
- }
- }
- if(_heading != yaw) {
- _heading = yaw;
- _addChange(HEADING_CHANGED);
- }
- }
-}
-
-void QGCFlightDisplay::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp)
-{
- _updateAttitude(uas, roll, pitch, yaw, timestamp);
-}
-
-void QGCFlightDisplay::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64)
-{
- groundSpeed = _oneDecimal(groundSpeed);
- if (fabs(_groundSpeed - groundSpeed) > 0.5) {
- _groundSpeed = groundSpeed;
- if(_refreshTimer->isActive()) {
- emit groundSpeedChanged();
- }
- }
- airSpeed = _oneDecimal(airSpeed);
- if (fabs(_airSpeed - airSpeed) > 1.0) {
- _airSpeed = airSpeed;
- if(_refreshTimer->isActive()) {
- emit airSpeedChanged();
- }
- }
- if(_groundSpeed != groundSpeed) {
- _groundSpeed = groundSpeed;
- _addChange(GROUNDSPEED_CHANGED);
- }
- if(_airSpeed != airSpeed) {
- _airSpeed = airSpeed;
- _addChange(AIRSPEED_CHANGED);
- }
-}
-
-void QGCFlightDisplay::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeWGS84, double altitudeRelative, double climbRate, quint64) {
- altitudeAMSL = _oneDecimal(altitudeAMSL);
- if (fabs(_altitudeAMSL - altitudeAMSL) > 0.5) {
- _altitudeAMSL = altitudeAMSL;
- if(_refreshTimer->isActive()) {
- emit altitudeAMSLChanged();
- }
- }
- altitudeWGS84 = _oneDecimal(altitudeWGS84);
- if (fabs(_altitudeWGS84 - altitudeWGS84) > 0.5) {
- _altitudeWGS84 = altitudeWGS84;
- if(_refreshTimer->isActive()) {
- emit altitudeWGS84Changed();
- }
- }
- altitudeRelative = _oneDecimal(altitudeRelative);
- if (fabs(_altitudeRelative - altitudeRelative) > 0.5) {
- _altitudeRelative = altitudeRelative;
- if(_refreshTimer->isActive()) {
- emit altitudeRelativeChanged();
- }
- }
- climbRate = _oneDecimal(climbRate);
- if (fabs(_climbRate - climbRate) > 0.5) {
- _climbRate = climbRate;
- if(_refreshTimer->isActive()) {
- emit climbRateChanged();
- }
- }
- if(_altitudeAMSL != altitudeAMSL) {
- _altitudeAMSL = altitudeAMSL;
- _addChange(ALTITUDEAMSL_CHANGED);
- }
- if(_altitudeWGS84 != altitudeWGS84) {
- _altitudeWGS84 = altitudeWGS84;
- _addChange(ALTITUDEWGS84_CHANGED);
- }
- if(_altitudeRelative != altitudeRelative) {
- _altitudeRelative = altitudeRelative;
- _addChange(ALTITUDERELATIVE_CHANGED);
- }
- if(_climbRate != climbRate) {
- _climbRate = climbRate;
- _addChange(CLIMBRATE_CHANGED);
- }
-}
-
-void QGCFlightDisplay::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) {
- _navigationAltitudeError = altitudeError;
- _navigationSpeedError = speedError;
- _navigationCrosstrackError = xtrackError;
-}
-
-void QGCFlightDisplay::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) {
- if (_mav == uas) {
- _navigationTargetBearing = targetBearing;
- }
-}
-
-/*
- * Internal
- */
-
-bool QGCFlightDisplay::_isAirplane() {
- if (_mav)
- return _mav->isAirplane();
- return false;
-}
-
-// TODO: Implement. Should return true when navigating.
-// That would be (PX4) in AUTO and RTL modes.
-// This could forward to a virtual on UAS bool isNavigatingAutonomusly() or whatever.
-bool QGCFlightDisplay::_shouldDisplayNavigationData() {
- return true;
-}
-
-void QGCFlightDisplay::showEvent(QShowEvent* event)
-{
- // Enable updates
- QWidget::showEvent(event);
- _refreshTimer->start(UPDATE_TIMER);
-}
-
-void QGCFlightDisplay::hideEvent(QHideEvent* event)
-{
- // Disable updates
- _refreshTimer->stop();
- QWidget::hideEvent(event);
-}
-
-void QGCFlightDisplay::_addChange(int id)
-{
- if(!_changes.contains(id)) {
- _changes.append(id);
- }
-}
-
-float QGCFlightDisplay::_oneDecimal(float value)
-{
- int i = (value * 10);
- return (float)i / 10.0;
-}
-
-void QGCFlightDisplay::_checkUpdate()
-{
- // Update current location
- if(_mav) {
- if(_latitude != _mav->getLatitude()) {
- _latitude = _mav->getLatitude();
- emit latitudeChanged();
- }
- if(_longitude != _mav->getLongitude()) {
- _longitude = _mav->getLongitude();
- emit longitudeChanged();
- }
- }
- // Check for changes
- // Significant changes, that is, whole number changes, are updated immediatelly.
- // For every message however, we set a flag for what changed and this timer updates
- // them to bring everything up-to-date. This prevents an avalanche of UI updates.
- foreach(int i, _changes) {
- switch (i) {
- case ROLL_CHANGED:
- emit rollChanged();
- break;
- case PITCH_CHANGED:
- emit pitchChanged();
- break;
- case HEADING_CHANGED:
- emit headingChanged();
- break;
- case GROUNDSPEED_CHANGED:
- emit groundSpeedChanged();
- break;
- case AIRSPEED_CHANGED:
- emit airSpeedChanged();
- break;
- case CLIMBRATE_CHANGED:
- emit climbRateChanged();
- break;
- case ALTITUDERELATIVE_CHANGED:
- emit altitudeRelativeChanged();
- break;
- case ALTITUDEWGS84_CHANGED:
- emit altitudeWGS84Changed();
- break;
- case ALTITUDEAMSL_CHANGED:
- emit altitudeAMSLChanged();
- break;
- default:
- break;
- }
- }
- _changes.clear();
-}
-
diff --git a/src/ui/flightdisplay/QGCFlightDisplay.h b/src/ui/flightdisplay/QGCFlightDisplay.h
index 41103c6fe316e458077bb8fc4aad2dfa0233b811..07f069b7be6f0b57e3e8f893c4d9ae8b8bd92bf8 100644
--- a/src/ui/flightdisplay/QGCFlightDisplay.h
+++ b/src/ui/flightdisplay/QGCFlightDisplay.h
@@ -41,120 +41,15 @@ public:
QGCFlightDisplay(QWidget* parent = NULL);
~QGCFlightDisplay();
- enum {
- ROLL_CHANGED,
- PITCH_CHANGED,
- HEADING_CHANGED,
- GROUNDSPEED_CHANGED,
- AIRSPEED_CHANGED,
- CLIMBRATE_CHANGED,
- ALTITUDERELATIVE_CHANGED,
- ALTITUDEWGS84_CHANGED,
- ALTITUDEAMSL_CHANGED
- };
-
/// @brief Invokes the Flight Display Options menu
void showOptionsMenu() { emit showOptionsMenuChanged(); }
- Q_PROPERTY(float roll READ roll NOTIFY rollChanged)
- Q_PROPERTY(float pitch READ pitch NOTIFY pitchChanged)
- Q_PROPERTY(float heading READ heading NOTIFY headingChanged)
- Q_PROPERTY(float groundSpeed READ groundSpeed NOTIFY groundSpeedChanged)
- Q_PROPERTY(float airSpeed READ airSpeed NOTIFY airSpeedChanged)
- Q_PROPERTY(float climbRate READ climbRate NOTIFY climbRateChanged)
- Q_PROPERTY(float altitudeRelative READ altitudeRelative NOTIFY altitudeRelativeChanged)
- Q_PROPERTY(float altitudeWGS84 READ altitudeWGS84 NOTIFY altitudeWGS84Changed)
- Q_PROPERTY(float altitudeAMSL READ altitudeAMSL NOTIFY altitudeAMSLChanged)
- Q_PROPERTY(bool repaintRequested READ repaintRequested NOTIFY repaintRequestedChanged)
- Q_PROPERTY(float latitude READ latitude NOTIFY latitudeChanged)
- Q_PROPERTY(float longitude READ longitude NOTIFY longitudeChanged)
- Q_PROPERTY(bool mavPresent READ mavPresent NOTIFY mavPresentChanged)
-
Q_INVOKABLE void saveSetting (const QString &key, const QString& value);
Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue);
- float roll () { return _roll; }
- float pitch () { return _pitch; }
- float heading () { return _heading; }
- float groundSpeed () { return _groundSpeed; }
- float airSpeed () { return _airSpeed; }
- float climbRate () { return _climbRate; }
- float altitudeRelative () { return _altitudeRelative; }
- float altitudeWGS84 () { return _altitudeWGS84; }
- float altitudeAMSL () { return _altitudeAMSL; }
- float latitude () { return _latitude; }
- float longitude () { return _longitude; }
- bool repaintRequested () { return true; }
- bool mavPresent () { return _mav != NULL; }
-
- /** @brief Start updating widget */
- void showEvent(QShowEvent* event);
- /** @brief Stop updating widget */
- void hideEvent(QHideEvent* event);
-
signals:
- void rollChanged ();
- void pitchChanged ();
- void headingChanged ();
- void groundSpeedChanged ();
- void airSpeedChanged ();
- void climbRateChanged ();
- void altitudeRelativeChanged();
- void altitudeWGS84Changed ();
- void altitudeAMSLChanged ();
- void repaintRequestedChanged();
- void latitudeChanged ();
- void longitudeChanged ();
- void mavPresentChanged ();
void showOptionsMenuChanged ();
-private slots:
- /** @brief Attitude from main autopilot / system state */
- void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
- /** @brief Attitude from one specific component / redundant autopilot */
- void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
- /** @brief Speed */
- void _updateSpeed (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
- /** @brief Altitude */
- void _updateAltitude (UASInterface* uas, double _altitudeAMSL, double _altitudeWGS84, double _altitudeRelative, double _climbRate, quint64 timestamp);
- void _updateNavigationControllerErrors (UASInterface* uas, double altitudeError, double speedError, double xtrackError);
- void _updateNavigationControllerData (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance);
- void _forgetUAS (UASInterface* uas);
- void _setActiveUAS (UASInterface* uas);
- void _checkUpdate ();
-
-private:
- bool _isAirplane ();
- bool _shouldDisplayNavigationData ();
- void _addChange (int id);
- float _oneDecimal (float value);
-
-private:
- UASInterface* _mav;
-
- float _roll;
- float _pitch;
- float _heading;
-
- float _altitudeAMSL;
- float _altitudeWGS84;
- float _altitudeRelative;
-
- float _groundSpeed;
- float _airSpeed;
- float _climbRate;
-
- float _navigationAltitudeError;
- float _navigationSpeedError;
- float _navigationCrosstrackError;
- float _navigationTargetBearing;
-
- float _latitude;
- float _longitude;
-
- QTimer* _refreshTimer;
- QList _changes;
};
-
#endif // QGCFLIGHTDISPLAY_H
diff --git a/src/ui/mapdisplay/MapDisplay.qml b/src/ui/mapdisplay/MapDisplay.qml
index bbf608ea7056e7f104c7641a5f54c156c161ca7a..9cf57920d234e7ddcd5414940b67449d2a91f2f8 100644
--- a/src/ui/mapdisplay/MapDisplay.qml
+++ b/src/ui/mapdisplay/MapDisplay.qml
@@ -45,56 +45,6 @@ Rectangle {
property real pitch: isNaN(mapEngine.pitch) ? 0 : mapEngine.pitch
property bool showWaypointEditor: true
- function getBool(value) {
- return value === '0' ? false : true;
- }
-
- function setBool(value) {
- return value ? "1" : "0";
- }
-
- Component.onCompleted:
- {
- mapTypeMenu.update();
- }
-
- Menu {
- id: mapTypeMenu
- title: "Map Type..."
- ExclusiveGroup { id: currentMapType }
- function setCurrentMap(map) {
- for (var i = 0; i < mapBackground.mapItem.supportedMapTypes.length; i++) {
- if (map === mapBackground.mapItem.supportedMapTypes[i].name) {
- mapBackground.mapItem.activeMapType = mapBackground.mapItem.supportedMapTypes[i]
- mapEngine.saveSetting("currentMapType", map);
- return;
- }
- }
- }
- function addMap(map, checked) {
- var mItem = mapTypeMenu.addItem(map);
- mItem.checkable = true
- mItem.checked = checked
- mItem.exclusiveGroup = currentMapType
- var menuSlot = function() {setCurrentMap(map);};
- mItem.triggered.connect(menuSlot);
- }
- function update() {
- clear()
- var map = ''
- if (mapBackground.mapItem.supportedMapTypes.length > 0)
- map = mapBackground.mapItem.activeMapType.name;
- map = mapEngine.loadSetting("currentMapType", map);
- for (var i = 0; i < mapBackground.mapItem.supportedMapTypes.length; i++) {
- var name = mapBackground.mapItem.supportedMapTypes[i].name;
- addMap(name, map === name);
- }
- if(map != '')
- setCurrentMap(map);
- }
-
- }
-
SplitView {
id: splitView
anchors.fill: parent
@@ -117,6 +67,9 @@ Rectangle {
heading: isNaN(mapEngine.heading) ? 0 : mapEngine.heading
latitude: 37.803784 // mapEngine.latitude
longitude: -122.462276 // mapEngine.longitude
+ showWaypoints: true
+ mapName: 'MainMapDisplay'
+
// Chevron button at upper right corner of Map Display
Item {
id: openWaypoints
@@ -164,7 +117,7 @@ Rectangle {
onClicked: {
if (mouse.button == Qt.RightButton)
{
- mapTypeMenu.popup()
+ mapBackground.mapMenu.popup()
}
}
z: splitView.z + 5
diff --git a/src/ui/qmlcommon/QGCMapBackground.qml b/src/ui/qmlcommon/QGCMapBackground.qml
index 30e906df78554f5280344618b18606c6d8475ced..98b5a92428c4204bbed28019144c7f82f07b7335 100644
--- a/src/ui/qmlcommon/QGCMapBackground.qml
+++ b/src/ui/qmlcommon/QGCMapBackground.qml
@@ -28,25 +28,74 @@ This file is part of the QGROUNDCONTROL project
*/
import QtQuick 2.4
-import QtPositioning 5.3
+import QtQuick.Controls 1.3
import QtLocation 5.3
+import QtPositioning 5.3
import QGroundControl.Controls 1.0
import QGroundControl.FlightControls 1.0
import QGroundControl.ScreenTools 1.0
+import QGroundControl.MavManager 1.0
+
+Item {
+ id: root
+ clip: true
-Rectangle {
- id: root
- property real latitude: 37.803784
- property real longitude : -122.462276
- property real zoomLevel: 18
- property real heading: 0
- property bool alwaysNorth: true
- property bool interactive: true
- property alias mapItem: map
+ property real latitude: 0
+ property real longitude: 0
+ property real zoomLevel: 18
+ property real heading: 0
+ property bool alwaysNorth: true
+ property bool interactive: true
+ property bool showWaypoints: false
+ property string mapName: 'defaultMap'
+ property alias mapItem: map
+ property alias waypoints: polyLine
+ property alias mapMenu: mapTypeMenu
+ property alias readOnly: map.readOnly
- color: Qt.rgba(0,0,0,0)
- clip: true
+ Component.onCompleted: {
+ map.zoomLevel = 18
+ map.markers = []
+ mapTypeMenu.update();
+ }
+
+ //-- Menu to select supported map types
+ Menu {
+ id: mapTypeMenu
+ title: "Map Type..."
+ ExclusiveGroup { id: currMapType }
+ function setCurrentMap(mapID) {
+ for (var i = 0; i < map.supportedMapTypes.length; i++) {
+ if (mapID === map.supportedMapTypes[i].name) {
+ map.activeMapType = map.supportedMapTypes[i]
+ MavManager.saveSetting(root.mapName + "/currentMapType", mapID);
+ return;
+ }
+ }
+ }
+ function addMap(mapID, checked) {
+ var mItem = mapTypeMenu.addItem(mapID);
+ mItem.checkable = true
+ mItem.checked = checked
+ mItem.exclusiveGroup = currMapType
+ var menuSlot = function() {setCurrentMap(mapID);};
+ mItem.triggered.connect(menuSlot);
+ }
+ function update() {
+ clear()
+ var mapID = ''
+ if (map.supportedMapTypes.length > 0)
+ mapID = map.activeMapType.name;
+ mapID = MavManager.loadSetting(root.mapName + "/currentMapType", mapID);
+ for (var i = 0; i < map.supportedMapTypes.length; i++) {
+ var name = map.supportedMapTypes[i].name;
+ addMap(name, mapID === name);
+ }
+ if(mapID != '')
+ setCurrentMap(mapID);
+ }
+ }
function adjustSize() {
if(root.visible) {
@@ -83,34 +132,75 @@ Rectangle {
return dist
}
+ function updateWaypoints() {
+ polyLine.path = []
+ // Are we initialized?
+ if (typeof map.markers != 'undefined' && typeof root.longitude != 'undefined') {
+ map.deleteMarkers()
+ if(root.showWaypoints) {
+ for(var i = 0; i < MavManager.waypoints.length; i++) {
+ var coord = QtPositioning.coordinate(MavManager.waypoints[i].latitude, MavManager.waypoints[i].longitude, MavManager.waypoints[i].altitude);
+ polyLine.addCoordinate(coord);
+ map.addMarker(coord, MavManager.waypoints[i].id);
+ }
+ if (typeof MavManager.waypoints != 'undefined' && MavManager.waypoints.length > 0) {
+ root.longitude = MavManager.waypoints[0].longitude
+ root.latitude = MavManager.waypoints[0].latitude
+ }
+ map.changed = false
+ }
+ }
+ }
+
Plugin {
id: mapPlugin
name: "QGroundControl"
}
+ Connections {
+ target: MavManager
+ onWaypointsChanged: {
+ root.updateWaypoints();
+ }
+ }
+
+ onShowWaypointsChanged: {
+ root.updateWaypoints();
+ }
+
Map {
id: map
- property real lon: (longitude >= -180 && longitude <= 180) ? longitude : 0
- property real lat: (latitude >= -90 && latitude <= 90) ? latitude : 0
+
+ property real lon: (longitude >= -180 && longitude <= 180) ? longitude : 0
+ property real lat: (latitude >= -90 && latitude <= 90) ? latitude : 0
+ property int currentMarker
+ property int pressX : -1
+ property int pressY : -1
+ property bool changed: false
+ property bool readOnly: false
property variant scaleLengths: [5, 10, 25, 50, 100, 150, 250, 500, 1000, 2000, 5000, 10000, 20000, 50000, 100000, 200000, 500000, 1000000, 2000000]
+ property variant markers
+
plugin: mapPlugin
width: 1
height: 1
zoomLevel: root.zoomLevel
anchors.centerIn: parent
center: QtPositioning.coordinate(lat, lon)
+ gesture.flickDeceleration: 3000
+ gesture.enabled: root.interactive
+
/*
// There is a bug currently in Qt where it fails to render a map taller than 6 tiles high. Until
// that's fixed, we can't rotate the map as it would require a larger map, which can easely grow
// larger than 6 tiles high.
+ // https://bugreports.qt.io/browse/QTBUG-45508
transform: Rotation {
origin.x: map.width / 2
origin.y: map.height / 2
angle: map.visible ? (alwaysNorth ? 0 : -heading) : 0
}
*/
- gesture.flickDeceleration: 3000
- gesture.enabled: root.interactive
onWidthChanged: {
scaleTimer.restart()
@@ -124,8 +214,42 @@ Rectangle {
scaleTimer.restart()
}
- Component.onCompleted: {
- map.zoomLevel = 18
+ function updateMarker(coord, wpid)
+ {
+ if(wpid < polyLine.path.length) {
+ var tmpPath = polyLine.path;
+ tmpPath[wpid] = coord;
+ polyLine.path = tmpPath;
+ map.changed = true;
+ }
+ }
+
+ function addMarker(coord, wpid)
+ {
+ var marker = Qt.createQmlObject ('QGCWaypoint {}', map)
+ map.addMapItem(marker)
+ marker.z = map.z + 1
+ marker.coordinate = coord
+ marker.waypointID = wpid
+ // Update list of markers
+ var count = map.markers.length
+ var myArray = []
+ for (var i = 0; i < count; i++){
+ myArray.push(markers[i])
+ }
+ myArray.push(marker)
+ markers = myArray
+ }
+
+ function deleteMarkers()
+ {
+ if (typeof map.markers != 'undefined') {
+ var count = map.markers.length
+ for (var i = 0; i < count; i++) {
+ map.markers[i].destroy()
+ }
+ }
+ map.markers = []
}
function calculateScale() {
@@ -153,6 +277,15 @@ Rectangle {
scaleImage.width = (scaleImage.sourceSize.width * f) - 2 * scaleImageLeft.sourceSize.width
scaleText.text = text
}
+
+ MapPolyline {
+ id: polyLine
+ visible: path.length > 1 && root.showWaypoints
+ line.width: 3
+ line.color: map.changed ? "#f97a2e" : "#e35cd8"
+ smooth: true
+ antialiasing: true
+ }
}
QGCSlider {
@@ -161,7 +294,7 @@ Rectangle {
maximum: map.maximumZoomLevel;
opacity: 1
visible: parent.visible
- z: map.z + 20
+ z: 1000
anchors {
bottom: parent.bottom;
bottomMargin: ScreenTools.pixelSizeFactor * (15)
diff --git a/src/ui/qmlcommon/QGCWaypoint.qml b/src/ui/qmlcommon/QGCWaypoint.qml
new file mode 100644
index 0000000000000000000000000000000000000000..29d59ae38647fe1c62eac7b4fc43ee8d24013488
--- /dev/null
+++ b/src/ui/qmlcommon/QGCWaypoint.qml
@@ -0,0 +1,81 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2015 QGROUNDCONTROL PROJECT
+
+This file is part of the QGROUNDCONTROL 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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief QGC Waypoint Marker
+ * @author Gus Grubba
+ */
+
+import QtQuick 2.4
+import QtLocation 5.3
+
+MapQuickItem {
+ id: marker
+ property int waypointID: 0
+ anchorPoint.x: markerIcon.width / 2
+ anchorPoint.y: markerIcon.height / 2
+ sourceItem: Rectangle {
+ id: markerIcon
+ width: 30
+ height: 30
+ color: markerMouseArea.containsMouse ? (markerMouseArea.pressed ? Qt.rgba(0.69,0.2,0.68,0.25) : Qt.rgba(0.69,0.2,0.68,0.75)) : Qt.rgba(0,0,0,0.5)
+ radius: 8
+ border.color: Qt.rgba(0,0,0,0.75)
+ Text {
+ id: number
+ anchors.centerIn: parent
+ font.pointSize: 11
+ font.weight: Font.DemiBold
+ color: "white"
+ text: marker.waypointID
+ }
+ MouseArea {
+ id: markerMouseArea
+ enabled: !map.readOnly
+ anchors.fill: parent
+ hoverEnabled: true
+ drag.target: marker
+ preventStealing: true
+ property int pressX : -1
+ property int pressY : -1
+ property int jitterThreshold : 4
+ onPressed : {
+ pressX = mouse.x;
+ pressY = mouse.y;
+ map.currentMarker = -1;
+ for (var i = 0; i < map.markers.length; i++) {
+ if (marker === map.markers[i]) {
+ map.currentMarker = i;
+ break;
+ }
+ }
+ }
+ onPositionChanged: {
+ if (Math.abs(pressX - mouse.x ) < jitterThreshold && Math.abs(pressY - mouse.y) < jitterThreshold) {
+ map.updateMarker(marker.coordinate, marker.waypointID)
+ }
+ }
+ }
+ }
+}
diff --git a/src/ui/qmlcommon/qmldir b/src/ui/qmlcommon/qmldir
index 6bd91f6d99d9a4061f043766c4580c8743325b02..389840c256c8784b1d9629c29591ea053c6e35b9 100644
--- a/src/ui/qmlcommon/qmldir
+++ b/src/ui/qmlcommon/qmldir
@@ -14,3 +14,4 @@ QGCMapToolButton 1.0 QGCMapToolButton.qml
QGCAttitudeInstrument 1.0 QGCAttitudeInstrument.qml
QGCCompassInstrument 1.0 QGCCompassInstrument.qml
QGCArtificialHorizon 1.0 QGCArtificialHorizon.qml
+QGCWaypoint 1.0 QGCWaypoint.qml
diff --git a/src/ui/toolbar/MainToolBar.cc b/src/ui/toolbar/MainToolBar.cc
index e41ea702a3e11daa3a894c06dfe7ccf696d81808..9fcc54fe8ee2f8c1f873ee44c3b1ab159b629b05 100644
--- a/src/ui/toolbar/MainToolBar.cc
+++ b/src/ui/toolbar/MainToolBar.cc
@@ -41,21 +41,13 @@ MainToolBar::MainToolBar(QWidget* parent)
, _mav(NULL)
, _toolBar(NULL)
, _currentView(ViewNone)
- , _batteryVoltage(0.0)
- , _batteryPercent(0.0)
, _connectionCount(0)
- , _systemArmed(false)
- , _currentHeartbeatTimeout(0)
- , _waypointDistance(0.0)
- , _currentWaypoint(0)
, _currentMessageCount(0)
, _messageCount(0)
, _currentErrorCount(0)
, _currentWarningCount(0)
, _currentNormalCount(0)
, _currentMessageType(MessageNone)
- , _satelliteCount(-1)
- , _satelliteLock(0)
, _showGPS(true)
, _showMav(true)
, _showMessages(true)
@@ -88,13 +80,10 @@ MainToolBar::MainToolBar(QWidget* parent)
setContextPropertyObject("mainToolBar", this);
setSource(QUrl::fromUserInput("qrc:/qml/MainToolBar.qml"));
setVisible(true);
- // Configure the toolbar for the current default UAS (which should be none as we just booted)
- _setActiveUAS(UASManager::instance()->getActiveUAS());
emit configListChanged();
- emit heartbeatTimeoutChanged(_currentHeartbeatTimeout);
emit connectionCountChanged(_connectionCount);
+ _setActiveUAS(UASManager::instance()->getActiveUAS());
// Link signals
- connect(UASManager::instance(), &UASManager::activeUASSet, this, &MainToolBar::_setActiveUAS);
connect(LinkManager::instance(), &LinkManager::linkConfigurationChanged, this, &MainToolBar::_updateConfigurations);
connect(LinkManager::instance(), &LinkManager::linkConnected, this, &MainToolBar::_linkConnected);
connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &MainToolBar::_linkDisconnected);
@@ -103,6 +92,8 @@ MainToolBar::MainToolBar(QWidget* parent)
connect(MAVLinkProtocol::instance(),
SIGNAL(radioStatusChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned)), this,
SLOT(_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned)));
+ connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*)));
+ connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_forgetUAS(UASInterface*)));
}
MainToolBar::~MainToolBar()
@@ -254,15 +245,6 @@ void MainToolBar::onEnterMessageArea(int x, int y)
}
}
-QString MainToolBar::getMavIconColor()
-{
- // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
- if(_mav)
- return _mav->getColor().name();
- else
- return QString("black");
-}
-
void MainToolBar::_leaveMessageView()
{
// Mouse has left the message window area (and it has closed itself)
@@ -295,74 +277,53 @@ void MainToolBar::setCurrentView(int currentView)
}
}
+void MainToolBar::_forgetUAS(UASInterface* uas)
+{
+ if (_mav != NULL && _mav == uas) {
+ disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &MainToolBar::_handleTextMessage);
+ disconnect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged);
+ disconnect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue);
+ _mav = NULL;
+ }
+}
void MainToolBar::_setActiveUAS(UASInterface* active)
{
// Do nothing if system is the same
if (_mav == active) {
return;
}
- // If switching the UAS, disconnect the existing one.
- if (_mav)
- {
- disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &MainToolBar::_handleTextMessage);
- disconnect(_mav, &UASInterface::heartbeatTimeout, this, &MainToolBar::_heartbeatTimeout);
- disconnect(_mav, &UASInterface::batteryChanged, this, &MainToolBar::_updateBatteryRemaining);
- disconnect(_mav, &UASInterface::modeChanged, this, &MainToolBar::_updateMode);
- disconnect(_mav, &UASInterface::nameChanged, this, &MainToolBar::_updateName);
- disconnect(_mav, &UASInterface::systemTypeSet, this, &MainToolBar::_setSystemType);
- disconnect(_mav, &UASInterface::localizationChanged, this, &MainToolBar::_setSatLoc);
- disconnect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged);
- disconnect(_mav, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*,QString,QString)));
- disconnect(_mav, SIGNAL(armingChanged(bool)), this, SLOT(_updateArmingState(bool)));
-
- if (_mav->getWaypointManager()) {
- disconnect(_mav->getWaypointManager(), &UASWaypointManager::currentWaypointChanged, this, &MainToolBar::_updateCurrentWaypoint);
- disconnect(_mav->getWaypointManager(), &UASWaypointManager::waypointDistanceChanged, this, &MainToolBar::_updateWaypointDistance);
- }
-
- UAS* pUas = dynamic_cast(_mav);
- if(pUas) {
- disconnect(pUas, &UAS::satelliteCountChanged, this, &MainToolBar::_setSatelliteCount);
- }
-
- disconnect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue);
+ // Disconnect the previous one (if any)
+ if(_mav) {
+ _forgetUAS(_mav);
}
// Connect new system
_mav = active;
if (_mav)
{
- _setSystemType(_mav, _mav->getSystemType());
- _updateArmingState(_mav->isArmed());
connect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &MainToolBar::_handleTextMessage);
- connect(_mav, &UASInterface::heartbeatTimeout, this, &MainToolBar::_heartbeatTimeout);
- connect(_mav, &UASInterface::batteryChanged, this, &MainToolBar::_updateBatteryRemaining);
- connect(_mav, &UASInterface::modeChanged, this, &MainToolBar::_updateMode);
- connect(_mav, &UASInterface::nameChanged, this, &MainToolBar::_updateName);
- connect(_mav, &UASInterface::systemTypeSet, this, &MainToolBar::_setSystemType);
- connect(_mav, &UASInterface::localizationChanged, this, &MainToolBar::_setSatLoc);
- connect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged);
- connect(_mav, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*, QString,QString)));
- connect(_mav, SIGNAL(armingChanged(bool)), this, SLOT(_updateArmingState(bool)));
-
- if (_mav->getWaypointManager()) {
- connect(_mav->getWaypointManager(), &UASWaypointManager::currentWaypointChanged, this, &MainToolBar::_updateCurrentWaypoint);
- connect(_mav->getWaypointManager(), &UASWaypointManager::waypointDistanceChanged, this, &MainToolBar::_updateWaypointDistance);
- }
-
- UAS* pUas = dynamic_cast(_mav);
- if(pUas) {
- _setSatelliteCount(pUas->getSatelliteCount(), QString(""));
- connect(pUas, &UAS::satelliteCountChanged, this, &MainToolBar::_setSatelliteCount);
- }
-
+ connect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged);
connect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue);
-
- // Reset connection lost (if any)
- _currentHeartbeatTimeout = 0;
- emit heartbeatTimeoutChanged(_currentHeartbeatTimeout);
}
- // Let toolbar know about it
- emit mavPresentChanged(_mav != NULL);
+}
+
+void MainToolBar::_updateConfigurations()
+{
+ QStringList tmpList;
+ QList configs = LinkManager::instance()->getLinkConfigurationList();
+ foreach(LinkConfiguration* conf, configs) {
+ if(conf) {
+ if(conf->isPreferred()) {
+ tmpList.insert(0,conf->name());
+ } else {
+ tmpList << conf->name();
+ }
+ }
+ }
+ // Any changes?
+ if(tmpList != _linkConfigurations) {
+ _linkConfigurations = tmpList;
+ emit configListChanged();
+ }
}
void MainToolBar::_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned rssi, unsigned remrssi, unsigned, unsigned, unsigned)
@@ -393,53 +354,6 @@ void MainToolBar::_remoteControlRSSIChanged(uint8_t rssi)
}
}
-void MainToolBar::_updateArmingState(bool armed)
-{
- if(_systemArmed != armed) {
- _systemArmed = armed;
- emit systemArmedChanged(armed);
- }
-}
-
-void MainToolBar::_updateBatteryRemaining(UASInterface*, double voltage, double, double percent, int)
-{
-
- if(percent < 0.0) {
- percent = 0.0;
- }
- if(voltage < 0.0) {
- voltage = 0.0;
- }
- if (_batteryVoltage != voltage) {
- _batteryVoltage = voltage;
- emit batteryVoltageChanged(voltage);
- }
- if (_batteryPercent != percent) {
- _batteryPercent = percent;
- emit batteryPercentChanged(voltage);
- }
-}
-
-void MainToolBar::_updateConfigurations()
-{
- QStringList tmpList;
- QList configs = LinkManager::instance()->getLinkConfigurationList();
- foreach(LinkConfiguration* conf, configs) {
- if(conf) {
- if(conf->isPreferred()) {
- tmpList.insert(0,conf->name());
- } else {
- tmpList << conf->name();
- }
- }
- }
- // Any changes?
- if(tmpList != _linkConfigurations) {
- _linkConfigurations = tmpList;
- emit configListChanged();
- }
-}
-
void MainToolBar::_linkConnected(LinkInterface*)
{
_updateConnection();
@@ -487,120 +401,6 @@ void MainToolBar::_updateConnection(LinkInterface *disconnectedLink)
}
}
-void MainToolBar::_updateState(UASInterface*, QString name, QString)
-{
- if (_currentState != name) {
- _currentState = name;
- emit currentStateChanged(_currentState);
- }
-}
-
-void MainToolBar::_updateMode(int, QString name, QString)
-{
- if (name.size()) {
- QString shortMode = name;
- shortMode = shortMode.replace("D|", "");
- shortMode = shortMode.replace("A|", "");
- if (_currentMode != shortMode) {
- _currentMode = shortMode;
- emit currentModeChanged();
- }
- }
-}
-
-void MainToolBar::_updateName(const QString& name)
-{
- if (_systemName != name) {
- _systemName = name;
- // TODO: emit signal and use it
- }
-}
-
-/**
- * The current system type is represented through the system icon.
- *
- * @param uas Source system, has to be the same as this->uas
- * @param systemType type ID, following the MAVLink system type conventions
- * @see http://pixhawk.ethz.ch/software/mavlink
- */
-void MainToolBar::_setSystemType(UASInterface*, unsigned int systemType)
-{
- _systemPixmap = "qrc:/res/mavs/";
- switch (systemType) {
- case MAV_TYPE_GENERIC:
- _systemPixmap += "Generic";
- break;
- case MAV_TYPE_FIXED_WING:
- _systemPixmap += "FixedWing";
- break;
- case MAV_TYPE_QUADROTOR:
- _systemPixmap += "QuadRotor";
- break;
- case MAV_TYPE_COAXIAL:
- _systemPixmap += "Coaxial";
- break;
- case MAV_TYPE_HELICOPTER:
- _systemPixmap += "Helicopter";
- break;
- case MAV_TYPE_ANTENNA_TRACKER:
- _systemPixmap += "AntennaTracker";
- break;
- case MAV_TYPE_GCS:
- _systemPixmap += "Groundstation";
- break;
- case MAV_TYPE_AIRSHIP:
- _systemPixmap += "Airship";
- break;
- case MAV_TYPE_FREE_BALLOON:
- _systemPixmap += "FreeBalloon";
- break;
- case MAV_TYPE_ROCKET:
- _systemPixmap += "Rocket";
- break;
- case MAV_TYPE_GROUND_ROVER:
- _systemPixmap += "GroundRover";
- break;
- case MAV_TYPE_SURFACE_BOAT:
- _systemPixmap += "SurfaceBoat";
- break;
- case MAV_TYPE_SUBMARINE:
- _systemPixmap += "Submarine";
- break;
- case MAV_TYPE_HEXAROTOR:
- _systemPixmap += "HexaRotor";
- break;
- case MAV_TYPE_OCTOROTOR:
- _systemPixmap += "OctoRotor";
- break;
- case MAV_TYPE_TRICOPTER:
- _systemPixmap += "TriCopter";
- break;
- case MAV_TYPE_FLAPPING_WING:
- _systemPixmap += "FlappingWing";
- break;
- case MAV_TYPE_KITE:
- _systemPixmap += "Kite";
- break;
- default:
- _systemPixmap += "Unknown";
- break;
- }
- emit systemPixmapChanged(_systemPixmap);
-}
-
-void MainToolBar::_heartbeatTimeout(bool timeout, unsigned int ms)
-{
- unsigned int elapsed = ms;
- if (!timeout)
- {
- elapsed = 0;
- }
- if(elapsed != _currentHeartbeatTimeout) {
- _currentHeartbeatTimeout = elapsed;
- emit heartbeatTimeoutChanged(_currentHeartbeatTimeout);
- }
-}
-
void MainToolBar::_handleTextMessage(int newCount)
{
// Reset?
@@ -662,42 +462,6 @@ void MainToolBar::_handleTextMessage(int newCount)
}
}
-void MainToolBar::_updateWaypointDistance(double distance)
-{
- if (_waypointDistance != distance) {
- _waypointDistance = distance;
- // TODO: emit signal and use it
- }
-}
-
-void MainToolBar::_updateCurrentWaypoint(quint16 id)
-{
- if (_currentWaypoint != id) {
- _currentWaypoint = id;
- // TODO: emit signal and use it
- }
-}
-
-void MainToolBar::_setSatelliteCount(double val, QString)
-{
- // I'm assuming that a negative value or over 99 means there is no GPS
- if(val < 0.0) val = -1.0;
- if(val > 99.0) val = -1.0;
- if(_satelliteCount != (int)val) {
- _satelliteCount = (int)val;
- emit satelliteCountChanged(_satelliteCount);
- }
-}
-
-void MainToolBar::_setSatLoc(UASInterface*, int fix)
-{
- // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock
- if(_satelliteLock != fix) {
- _satelliteLock = fix;
- emit satelliteLockChanged(_satelliteLock);
- }
-}
-
void MainToolBar::_setProgressBarValue(float value)
{
_progressBarValue = value;
diff --git a/src/ui/toolbar/MainToolBar.h b/src/ui/toolbar/MainToolBar.h
index 0a45a6cdd695607983a5418ad25db3b8c00f3c60..a9c26c8b9807cd1fe87787439dac19ffdaf4c251 100644
--- a/src/ui/toolbar/MainToolBar.h
+++ b/src/ui/toolbar/MainToolBar.h
@@ -37,7 +37,7 @@ This file is part of the QGROUNDCONTROL project
#define TOOL_BAR_SHOW_GPS "ShowGPS"
#define TOOL_BAR_SHOW_MAV "ShowMav"
#define TOOL_BAR_SHOW_MESSAGES "ShowMessages"
-#define TOOL_BAR_SHOW_RSSI "ShowRSSI"
+#define TOOL_BAR_SHOW_RSSI "ShowRSSI"
class UASInterface;
class UASMessage;
@@ -52,6 +52,13 @@ public:
MainToolBar(QWidget* parent = NULL);
~MainToolBar();
+ typedef enum {
+ MessageNone,
+ MessageNormal,
+ MessageWarning,
+ MessageError
+ } MessageType_t;
+
typedef enum {
ViewNone = -1,
ViewAnalyze, // MainWindow::VIEW_ENGINEER
@@ -60,13 +67,6 @@ public:
ViewSetup , // MainWindow::VIEW_SETUP
} ViewType_t;
- typedef enum {
- MessageNone,
- MessageNormal,
- MessageWarning,
- MessageError
- } MessageType_t;
-
Q_INVOKABLE void onSetupView();
Q_INVOKABLE void onPlanView();
Q_INVOKABLE void onFlyView();
@@ -75,25 +75,14 @@ public:
Q_INVOKABLE void onConnect(QString conf);
Q_INVOKABLE void onDisconnect(QString conf);
Q_INVOKABLE void onEnterMessageArea(int x, int y);
- Q_INVOKABLE QString getMavIconColor();
- Q_PROPERTY(int connectionCount MEMBER _connectionCount NOTIFY connectionCountChanged)
- Q_PROPERTY(double batteryVoltage MEMBER _batteryVoltage NOTIFY batteryVoltageChanged)
- Q_PROPERTY(double batteryPercent MEMBER _batteryPercent NOTIFY batteryPercentChanged)
Q_PROPERTY(ViewType_t currentView MEMBER _currentView NOTIFY currentViewChanged)
Q_PROPERTY(QStringList configList MEMBER _linkConfigurations NOTIFY configListChanged)
- Q_PROPERTY(bool systemArmed MEMBER _systemArmed NOTIFY systemArmedChanged)
- Q_PROPERTY(unsigned int heartbeatTimeout MEMBER _currentHeartbeatTimeout NOTIFY heartbeatTimeoutChanged)
- Q_PROPERTY(QString currentMode MEMBER _currentMode NOTIFY currentModeChanged)
Q_PROPERTY(MessageType_t messageType MEMBER _currentMessageType NOTIFY messageTypeChanged)
Q_PROPERTY(int newMessageCount MEMBER _currentMessageCount NOTIFY newMessageCountChanged)
Q_PROPERTY(int messageCount MEMBER _messageCount NOTIFY messageCountChanged)
- Q_PROPERTY(QString systemPixmap MEMBER _systemPixmap NOTIFY systemPixmapChanged)
- Q_PROPERTY(int satelliteCount READ satelliteCount NOTIFY satelliteCountChanged)
+ Q_PROPERTY(int connectionCount READ connectionCount NOTIFY connectionCountChanged)
Q_PROPERTY(QStringList connectedList MEMBER _connectedList NOTIFY connectedListChanged)
- Q_PROPERTY(bool mavPresent READ mavPresent NOTIFY mavPresentChanged)
- Q_PROPERTY(QString currentState MEMBER _currentState NOTIFY currentStateChanged)
- Q_PROPERTY(int satelliteLock MEMBER _satelliteLock NOTIFY satelliteLockChanged)
Q_PROPERTY(bool showGPS MEMBER _showGPS NOTIFY showGPSChanged)
Q_PROPERTY(bool showMav MEMBER _showMav NOTIFY showMavChanged)
Q_PROPERTY(bool showMessages MEMBER _showMessages NOTIFY showMessagesChanged)
@@ -103,48 +92,22 @@ public:
Q_PROPERTY(int remoteRSSI READ remoteRSSI NOTIFY remoteRSSIChanged)
Q_PROPERTY(int telemetryRRSSI READ telemetryRRSSI NOTIFY telemetryRRSSIChanged)
Q_PROPERTY(int telemetryLRSSI READ telemetryLRSSI NOTIFY telemetryLRSSIChanged)
- Q_PROPERTY(bool isAndroid READ isAndroid CONSTANT)
- Q_PROPERTY(bool isiOS READ isiOS CONSTANT)
- Q_PROPERTY(bool isMobile READ isMobile CONSTANT)
- bool mavPresent () { return _mav != NULL; }
- int satelliteCount () { return _satelliteCount; }
+ void setCurrentView (int currentView);
+ void viewStateChanged (const QString& key, bool value);
int remoteRSSI () { return _remoteRSSI; }
int telemetryRRSSI () { return _telemetryRRSSI; }
int telemetryLRSSI () { return _telemetryLRSSI; }
-
-#if defined (__android__)
- bool isAndroid () { return true; }
- bool isiOS () { return false; }
- bool isMobile () { return true; }
-#else
- bool isAndroid () { return false; }
- bool isiOS () { return false; }
- bool isMobile () { return false; }
-#endif
-
- void setCurrentView (int currentView);
- void viewStateChanged (const QString& key, bool value);
+ int connectionCount () { return _connectionCount; }
signals:
void connectionCountChanged (int count);
- void batteryVoltageChanged (double value);
- void batteryPercentChanged (double value);
void currentViewChanged ();
void configListChanged ();
- void systemArmedChanged (bool systemArmed);
- void heartbeatTimeoutChanged (unsigned int hbTimeout);
- void currentModeChanged ();
void messageTypeChanged (MessageType_t type);
void newMessageCountChanged (int count);
void messageCountChanged (int count);
- void currentConfigChanged (QString config);
- void systemPixmapChanged (QPixmap pix);
- void satelliteCountChanged (int count);
void connectedListChanged (QStringList connectedList);
- void mavPresentChanged (bool present);
- void currentStateChanged (QString state);
- void satelliteLockChanged (int lock);
void showGPSChanged (bool value);
void showMavChanged (bool value);
void showMessagesChanged (bool value);
@@ -156,27 +119,17 @@ signals:
void telemetryLRSSIChanged (int value);
private slots:
- void _setActiveUAS (UASInterface* active);
- void _updateBatteryRemaining (UASInterface*, double voltage, double, double percent, int);
- void _updateArmingState (bool armed);
+ void _forgetUAS (UASInterface* uas);
+ void _setActiveUAS (UASInterface* uas);
void _updateConfigurations ();
void _linkConnected (LinkInterface* link);
void _linkDisconnected (LinkInterface* link);
- void _updateState (UASInterface* system, QString name, QString description);
- void _updateMode (int system, QString name, QString description);
- void _updateName (const QString& name);
- void _setSystemType (UASInterface* uas, unsigned int systemType);
- void _heartbeatTimeout (bool timeout, unsigned int ms);
void _handleTextMessage (int newCount);
- void _updateCurrentWaypoint (quint16 id);
- void _updateWaypointDistance (double distance);
- void _setSatelliteCount (double val, QString name);
void _leaveMessageView ();
- void _setSatLoc (UASInterface* uas, int fix);
void _setProgressBarValue (float value);
+ void _updatePixelSize ();
void _remoteControlRSSIChanged (uint8_t rssi);
void _telemetryChanged (LinkInterface* link, unsigned rxerrors, unsigned fixed, unsigned rssi, unsigned remrssi, unsigned txbuf, unsigned noise, unsigned remnoise);
- void _updatePixelSize ();
private:
void _updateConnection (LinkInterface *disconnectedLink = NULL);
@@ -186,27 +139,15 @@ private:
UASInterface* _mav;
QQuickItem* _toolBar;
ViewType_t _currentView;
- double _batteryVoltage;
- double _batteryPercent;
QStringList _linkConfigurations;
int _connectionCount;
- bool _systemArmed;
- QString _currentState;
- QString _currentMode;
- QString _systemName;
- QString _systemPixmap;
- unsigned int _currentHeartbeatTimeout;
- double _waypointDistance;
- quint16 _currentWaypoint;
int _currentMessageCount;
int _messageCount;
int _currentErrorCount;
int _currentWarningCount;
int _currentNormalCount;
MessageType_t _currentMessageType;
- int _satelliteCount;
QStringList _connectedList;
- int _satelliteLock;
bool _showGPS;
bool _showMav;
bool _showMessages;
diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml
index c785c54fa1c3e2ab39baf6edd7b1525c1a3f14d3..e36203af5a1bd95fa59e66fcc0240b8be37cb585 100644
--- a/src/ui/toolbar/MainToolBar.qml
+++ b/src/ui/toolbar/MainToolBar.qml
@@ -35,6 +35,7 @@ import QGroundControl.Controls 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Palette 1.0
import QGroundControl.MainToolBar 1.0
+import QGroundControl.MavManager 1.0
import QGroundControl.ScreenTools 1.0
Rectangle {
@@ -42,7 +43,7 @@ Rectangle {
property var qgcPal: QGCPalette { id: palette; colorGroupEnabled: true }
- property int cellSpacerSize: mainToolBar.isMobile ? getProportionalDimmension(6) : getProportionalDimmension(4)
+ property int cellSpacerSize: ScreenTools.isMobile ? getProportionalDimmension(6) : getProportionalDimmension(4)
property int cellHeight: getProportionalDimmension(30)
property var colorBlue: "#1a6eaa"
@@ -83,24 +84,24 @@ Rectangle {
}
function getBatteryIcon() {
- if(mainToolBar.batteryPercent < 20.0)
+ if(MavManager.batteryPercent < 20.0)
return "qrc:/res/Battery_0";
- else if(mainToolBar.batteryPercent < 40.0)
+ else if(MavManager.batteryPercent < 40.0)
return "qrc:/res/Battery_20";
- else if(mainToolBar.batteryPercent < 60.0)
+ else if(MavManager.batteryPercent < 60.0)
return "qrc:/res/Battery_40";
- else if(mainToolBar.batteryPercent < 80.0)
+ else if(MavManager.batteryPercent < 80.0)
return "qrc:/res/Battery_60";
- else if(mainToolBar.batteryPercent < 90.0)
+ else if(MavManager.batteryPercent < 90.0)
return "qrc:/res/Battery_80";
else
return "qrc:/res/Battery_100";
}
function getBatteryColor() {
- if (mainToolBar.batteryPercent > 40.0)
+ if (MavManager.batteryPercent > 40.0)
return colorGreen;
- if(mainToolBar.batteryPercent > 0.01)
+ if(MavManager.batteryPercent > 0.01)
return colorRed;
// This means there is no battery level data
return colorBlue;
@@ -108,13 +109,13 @@ Rectangle {
function getSatelliteColor() {
// No GPS data
- if (mainToolBar.satelliteCount < 0)
+ if (MavManager.satelliteCount < 0)
return qgcPal.button
// No Lock
- if(mainToolBar.satelliteLock < 2)
+ if(MavManager.satelliteLock < 2)
return colorRed;
// 2D Lock
- if(mainToolBar.satelliteLock === 2)
+ if(MavManager.satelliteLock === 2)
return colorBlue;
// Lock is 3D or more
return colorGreen;
@@ -129,7 +130,7 @@ Rectangle {
}
function showMavStatus() {
- return (mainToolBar.mavPresent && mainToolBar.heartbeatTimeout === 0 && mainToolBar.connectionCount > 0);
+ return (MavManager.mavPresent && MavManager.heartbeatTimeout === 0 && mainToolBar.connectionCount > 0);
}
//-------------------------------------------------------------------------
@@ -193,7 +194,7 @@ Rectangle {
height: cellHeight
spacing: -getProportionalDimmension(12)
anchors.verticalCenter: parent.verticalCenter
- visible: !mainToolBar.isMobile
+ visible: !ScreenTools.isMobile
Connections {
target: ScreenTools
onRepaintRequestedChanged: {
@@ -275,7 +276,7 @@ Rectangle {
//-- "Hamburger" menu for Mobile Devices
Item {
id: actionButton
- visible: mainToolBar.isMobile
+ visible: ScreenTools.isMobile
height: cellHeight
width: cellHeight
Image {
@@ -391,7 +392,7 @@ Rectangle {
border.color: "#00000000"
border.width: 0
Image {
- source: mainToolBar.systemPixmap
+ source: MavManager.systemPixmap
height: cellHeight * 0.75
fillMode: Image.PreserveAspectFit
anchors.verticalCenter: parent.verticalCenter
@@ -422,8 +423,8 @@ Rectangle {
QGCLabel {
id: satelitteText
- text: mainToolBar.satelliteCount >= 0 ? mainToolBar.satelliteCount : 'NA'
- font.pointSize: mainToolBar.satelliteCount >= 0 ? ScreenTools.fontPointFactor * (14) : ScreenTools.fontPointFactor * (10)
+ text: MavManager.satelliteCount >= 0 ? MavManager.satelliteCount : 'NA'
+ font.pointSize: MavManager.satelliteCount >= 0 ? ScreenTools.fontPointFactor * (14) : ScreenTools.fontPointFactor * (10)
font.weight: Font.DemiBold
anchors.verticalCenter: parent.verticalCenter
anchors.right: parent.right
@@ -547,7 +548,7 @@ Rectangle {
QGCLabel {
id: batteryText
- text: mainToolBar.batteryVoltage.toFixed(1) + 'V';
+ text: MavManager.batteryVoltage.toFixed(1) + 'V';
font.pointSize: ScreenTools.fontPointFactor * (11);
font.weight: Font.DemiBold
anchors.verticalCenter: parent.verticalCenter
@@ -575,11 +576,11 @@ Rectangle {
QGCLabel {
id: armedStatusText
- text: (mainToolBar.systemArmed) ? qsTr("ARMED") : qsTr("DISARMED")
+ text: (MavManager.systemArmed) ? qsTr("ARMED") : qsTr("DISARMED")
font.pointSize: ScreenTools.fontPointFactor * (12);
font.weight: Font.DemiBold
anchors.centerIn: parent
- color: (mainToolBar.systemArmed) ? colorOrangeText : colorGreenText
+ color: (MavManager.systemArmed) ? colorOrangeText : colorGreenText
}
}
@@ -594,11 +595,11 @@ Rectangle {
QGCLabel {
id: stateStatusText
- text: mainToolBar.currentState
+ text: MavManager.currentState
font.pointSize: ScreenTools.fontPointFactor * (12);
font.weight: Font.DemiBold
anchors.centerIn: parent
- color: (mainToolBar.currentState === "STANDBY") ? colorGreenText : colorRedText
+ color: (MavManager.currentState === "STANDBY") ? colorGreenText : colorRedText
}
}
@@ -615,7 +616,7 @@ Rectangle {
QGCLabel {
id: modeStatusText
- text: mainToolBar.currentMode
+ text: MavManager.currentMode
font.pointSize: ScreenTools.fontPointFactor * (12);
font.weight: Font.DemiBold
anchors.horizontalCenter: parent.horizontalCenter
@@ -628,7 +629,7 @@ Rectangle {
id: connectionStatus
width: getProportionalDimmension(160)
height: cellHeight
- visible: (mainToolBar.connectionCount > 0 && mainToolBar.mavPresent && mainToolBar.heartbeatTimeout != 0)
+ visible: (mainToolBar.connectionCount > 0 && MavManager.mavPresent && MavManager.heartbeatTimeout != 0)
anchors.verticalCenter: parent.verticalCenter
color: "#00000000"
border.color: "#00000000"