diff --git a/QGCApplication.pro b/QGCApplication.pro index cdbfaceaa78e0348108146b77b5c14ef6a2e0817..4402e8a260bfc765ceb0e5dd6f4bae60dccb3e3d 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -256,7 +256,6 @@ HEADERS += \ src/QGCQuickWidget.h \ src/QGCSingleton.h \ src/QGCTemporaryFile.h \ - src/QmlControls/MavManager.h \ src/QmlControls/ParameterEditorController.h \ src/QmlControls/ScreenToolsController.h \ src/SerialPortIds.h \ @@ -389,7 +388,6 @@ SOURCES += \ src/QGCQuickWidget.cc \ src/QGCSingleton.cc \ src/QGCTemporaryFile.cc \ - src/QmlControls/MavManager.cc \ src/QmlControls/ParameterEditorController.cc \ src/QmlControls/ScreenToolsController.cc \ src/uas/FileManager.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 5d90fb8f6e114469fb94adda7de53547454baf43..ff8a881336e80fbb9bdb2151a96dfebd2adaf00a 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -132,7 +132,6 @@ src/FlightMap/Widgets/QGCCompassHUD.qml src/FlightMap/Widgets/QGCCurrentAltitude.qml src/FlightMap/Widgets/QGCCurrentSpeed.qml - src/FlightMap/Widgets/QGCHudMessage.qml src/FlightMap/Widgets/QGCMapToolButton.qml src/FlightMap/Widgets/QGCPitchIndicator.qml src/FlightMap/Widgets/QGCSlider.qml diff --git a/src/FlightMap/FlightMap.qml b/src/FlightMap/FlightMap.qml index ed7d3a0383d9c21ed5af13a45a893e0ebf97a79b..39f56970c0d21eac701893b36a902fe7d8510b8c 100644 --- a/src/FlightMap/FlightMap.qml +++ b/src/FlightMap/FlightMap.qml @@ -35,7 +35,6 @@ import QtPositioning 5.3 import QGroundControl.Controls 1.0 import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 -import QGroundControl.MavManager 1.0 import QGroundControl.MultiVehicleManager 1.0 import QGroundControl.Vehicle 1.0 @@ -50,16 +49,13 @@ Item { property bool alwaysNorth: true property bool interactive: true property bool showVehicles: 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 Component.onCompleted: { map.zoomLevel = 18 - map.markers = [] mapTypeMenu.update(); if (showVehicles) { addExistingVehicles() @@ -76,7 +72,7 @@ Item { 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); + multiVehicleManager.saveSetting(root.mapName + "/currentMapType", mapID); return; } } @@ -94,7 +90,7 @@ Item { var mapID = '' if (map.supportedMapTypes.length > 0) mapID = map.activeMapType.name; - mapID = MavManager.loadSetting(root.mapName + "/currentMapType", mapID); + mapID = multiVehicleManager.loadSetting(root.mapName + "/currentMapType", mapID); for (var i = 0; i < map.supportedMapTypes.length; i++) { var name = map.supportedMapTypes[i].name; addMap(name, mapID === name); @@ -139,26 +135,6 @@ Item { 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 - } - } - } - property var vehicles: [] ///< List of known vehicles property var vehicleMapItems: [] ///< List of know vehicle map items @@ -202,13 +178,6 @@ Item { name: "QGroundControl" } - Connections { - target: MavManager - onWaypointsChanged: { - root.updateWaypoints(); - } - } - Connections { target: multiVehicleManager @@ -216,10 +185,6 @@ Item { onVehicleRemoved: removeVehicle(vehicle) } - onShowWaypointsChanged: { - root.updateWaypoints(); - } - Map { id: map @@ -231,7 +196,6 @@ Item { 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 @@ -276,44 +240,6 @@ Item { } } - 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() { var coord1, coord2, dist, text, f f = 0 @@ -339,15 +265,6 @@ Item { 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 { diff --git a/src/FlightMap/Widgets/QGCHudMessage.qml b/src/FlightMap/Widgets/QGCHudMessage.qml deleted file mode 100644 index 5b3d396dd35b9119e5cef2ec048156061716d231..0000000000000000000000000000000000000000 --- a/src/FlightMap/Widgets/QGCHudMessage.qml +++ /dev/null @@ -1,93 +0,0 @@ -/*===================================================================== - -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 HUD Message - * @author Gus Grubba - */ - -import QtQuick 2.4 -import QtQuick.Controls 1.3 - -import QGroundControl.Controls 1.0 -import QGroundControl.ScreenTools 1.0 -import QGroundControl.MavManager 1.0 - -Item { - id: root - visible: MavManager.latestError !== '' - Rectangle { - anchors.fill: parent - color: Qt.rgba(0,0,0,0.75) - border.color: Qt.rgba(1,1,1,0.75) - radius: 4 - QGCLabel { - anchors.horizontalCenter: parent.horizontalCenter - anchors.verticalCenter: parent.verticalCenter - antialiasing: true - font.weight: Font.DemiBold - text: MavManager.latestError - color: "#f84444" - } - OpacityAnimator { - id: vanish - target: root; - from: 1; - to: 0; - duration: 2000 - running: false - } - } - Timer { - id: vanishTimer - interval: 30000 - running: false - repeat: false - onTriggered: { - vanish.start(); - } - } - MouseArea { - anchors.fill: parent - z: 1000 - acceptedButtons: Qt.LeftButton - onClicked: { - if (mouse.button == Qt.LeftButton) - { - vanishTimer.stop(); - vanish.stop(); - root.opacity = 0; - } - } - } - Connections { - target: MavManager - onLatestErrorChanged: { - vanishTimer.stop(); - vanish.stop(); - vanishTimer.start(); - root.opacity = 1; - } - } -} diff --git a/src/FlightMap/qmldir b/src/FlightMap/qmldir index 7b559fec948dacf21a6a317ca2fab3d3d566b644..0f00c982ac409efa27a4c16c1e1627881578c990 100644 --- a/src/FlightMap/qmldir +++ b/src/FlightMap/qmldir @@ -13,7 +13,6 @@ QGCCompassHUD 1.0 QGCCompassHUD.qml QGCCompassWidget 1.0 QGCCompassWidget.qml QGCCurrentAltitude 1.0 QGCCurrentAltitude.qml QGCCurrentSpeed 1.0 QGCCurrentSpeed.qml -QGCHudMessage 1.0 QGCHudMessage.qml QGCMapToolButton 1.0 QGCMapToolButton.qml QGCPitchIndicator 1.0 QGCPitchIndicator.qml QGCSlider 1.0 QGCSlider.qml diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index eb88cc13bed54e9518bd854e8dc6368cc06eb9aa..b263313b16c4e404e1f2b3a5fb3aa0c65ea398f6 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -83,7 +83,6 @@ G_END_DECLS #endif #include "AutoPilotPlugin.h" #include "VehicleComponent.h" -#include "MavManager.h" #include "FirmwarePluginManager.h" #include "MultiVehicleManager.h" #include "Generic/GenericFirmwarePlugin.h" @@ -122,19 +121,6 @@ static QObject* screenToolsControllerSingletonFactory(QQmlEngine*, QJSEngine*) return screenToolsController; } -/** - * @brief MavManager creation callback - * - * This is called by the QtQuick engine for creating the singleton -**/ - -static QObject* mavManagerSingletonFactory(QQmlEngine*, QJSEngine*) -{ - MavManager* mavManager = new MavManager; - qgcApp()->setMavManager(mavManager); - return mavManager; -} - #if defined(QGC_GST_STREAMING) #ifdef Q_OS_MAC #ifndef __ios__ @@ -163,7 +149,6 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) : QApplication(argc, argv) , _runningUnitTests(unitTesting) , _styleIsDark(true) - , _pMavManager(NULL) , _fakeMobile(false) { Q_ASSERT(_app == NULL); @@ -356,7 +341,6 @@ void QGCApplication::_initCommon(void) //-- Create QML Singleton Interfaces qmlRegisterSingletonType("QGroundControl.ScreenToolsController", 1, 0, "ScreenToolsController", screenToolsControllerSingletonFactory); - qmlRegisterSingletonType("QGroundControl.MavManager", 1, 0, "MavManager", mavManagerSingletonFactory); //-- Register Waypoint Interface qmlRegisterInterface("Waypoint"); @@ -788,17 +772,6 @@ void QGCApplication::_missingParamsDisplay(void) "You should quit QGroundControl immediately and update your firmware.").arg(params)); } -void QGCApplication::setMavManager(MavManager* pMgr) -{ - if(!_pMavManager) - _pMavManager = pMgr; -} - -MavManager* QGCApplication::getMavManager() -{ - return _pMavManager; -} - void QGCApplication::showToolBarMessage(const QString& message) { MainWindow* mainWindow = MainWindow::instance(); diff --git a/src/QGCApplication.h b/src/QGCApplication.h index ae66e7d87cb32cd777373f91bcd30adc0c2178a7..9c18443f659ea4b7f84918886bd4e3065d257aa8 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -44,7 +44,6 @@ // Work around circular header includes class QGCSingleton; class MainWindow; -class MavManager; /** * @brief The main application and management class. @@ -101,12 +100,6 @@ public: /// multiple times. void reportMissingParameter(int componentId, const QString& name); - /// When the singleton is created, it sets a pointer for subsequent use - void setMavManager(MavManager* pMgr); - - /// MavManager accessor - MavManager* getMavManager(); - /// Show a non-modal message to the user void showToolBarMessage(const QString& message); @@ -179,7 +172,6 @@ private: static const int _missingParamsDelayedDisplayTimerTimeout = 1000; ///< Timeout to wait for next missing fact to come in before display QTimer _missingParamsDelayedDisplayTimer; ///< Timer use to delay missing fact display QStringList _missingParams; ///< List of missing facts to be displayed - MavManager* _pMavManager; bool _fakeMobile; ///< true: Fake ui into displaying mobile interface diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index c3b741cf90b94cfb2aeb8a3b03dbdaa944e2c415..7bb1da915f2e8226f06b6715aa132aaea5c7e0df 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -240,3 +240,15 @@ QVariantList MultiVehicleManager::vehiclesAsVariants(void) return list; } + +void MultiVehicleManager::saveSetting(const QString &name, const QString& value) +{ + QSettings settings; + settings.setValue(name, value); +} + +QString MultiVehicleManager::loadSetting(const QString &name, const QString& defaultValue) +{ + QSettings settings; + return settings.value(name, defaultValue).toString(); +} diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 67ac1918f872772c183f089a615eafc986641a6b..05c2da55d22a0781e0cb96c4581631585795b437 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -39,6 +39,9 @@ class MultiVehicleManager : public QGCSingleton DECLARE_QGC_SINGLETON(MultiVehicleManager, MultiVehicleManager) public: + Q_INVOKABLE void saveSetting (const QString &key, const QString& value); + Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue); + Q_PROPERTY(bool activeVehicleAvailable READ activeVehicleAvailable NOTIFY activeVehicleAvailableChanged) Q_PROPERTY(bool parameterReadyVehicleAvailable READ parameterReadyVehicleAvailable NOTIFY parameterReadyVehicleAvailableChanged) Q_PROPERTY(Vehicle* activeVehicle READ activeVehicle WRITE setActiveVehicle NOTIFY activeVehicleChanged) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ebe69d2c5be202716d34b84bc463937a9d48325f..74612fc6b8f15778a9c811dbb26318a0c10f8cd0 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -21,22 +21,58 @@ ======================================================================*/ -/// @file -/// @author Don Gagne - #include "Vehicle.h" #include "MAVLinkProtocol.h" #include "FirmwarePluginManager.h" #include "LinkManager.h" #include "FirmwarePlugin.h" #include "AutoPilotPluginManager.h" +#include "UASMessageHandler.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") +#define UPDATE_TIMER 50 +#define DEFAULT_LAT 38.965767f +#define DEFAULT_LON -120.083923f + Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) : _id(vehicleId) , _firmwareType(firmwareType) , _uas(NULL) + , _mav(NULL) + , _currentMessageCount(0) + , _messageCount(0) + , _currentErrorCount(0) + , _currentWarningCount(0) + , _currentNormalCount(0) + , _currentMessageType(MessageNone) + , _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) + , _batteryConsumed(-1.0) + , _systemArmed(false) + , _currentHeartbeatTimeout(0) + , _waypointDistance(0.0) + , _currentWaypoint(0) + , _satelliteCount(-1) + , _satelliteLock(0) + , _wpm(NULL) + , _updateCount(0) { _addLink(link); @@ -47,14 +83,91 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) setLatitude(_uas->getLatitude()); setLongitude(_uas->getLongitude()); - _setYaw(_uas->getYaw()); connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude); connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude); - connect(_uas, &UAS::yawChanged, this, &Vehicle::_setYaw); _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType); _autopilotPlugin = AutoPilotPluginManager::instance()->newAutopilotPluginForVehicle(this); + + // Refresh timer + connect(_refreshTimer, SIGNAL(timeout()), this, SLOT(_checkUpdate())); + _refreshTimer->setInterval(UPDATE_TIMER); + _refreshTimer->start(UPDATE_TIMER); + emit heartbeatTimeoutChanged(); + + _mav = uas(); + // Reset satellite count (no GPS) + _satelliteCount = -1; + emit satelliteCountChanged(); + // Reset connection lost (if any) + _currentHeartbeatTimeout = 0; + emit heartbeatTimeoutChanged(); + // Listen for system messages + connect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); + // 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, &Vehicle::_updateNavigationControllerData); + connect(_mav, &UASInterface::heartbeatTimeout, this, &Vehicle::_heartbeatTimeout); + connect(_mav, &UASInterface::batteryChanged, this, &Vehicle::_updateBatteryRemaining); + connect(_mav, &UASInterface::batteryConsumedChanged, this, &Vehicle::_updateBatteryConsumedChanged); + connect(_mav, &UASInterface::modeChanged, this, &Vehicle::_updateMode); + connect(_mav, &UASInterface::nameChanged, this, &Vehicle::_updateName); + connect(_mav, &UASInterface::systemTypeSet, this, &Vehicle::_setSystemType); + connect(_mav, &UASInterface::localizationChanged, this, &Vehicle::_setSatLoc); + _wpm = _mav->getWaypointManager(); + if (_wpm) { + connect(_wpm, &UASWaypointManager::currentWaypointChanged, this, &Vehicle::_updateCurrentWaypoint); + connect(_wpm, &UASWaypointManager::waypointDistanceChanged, this, &Vehicle::_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, &Vehicle::_setSatelliteCount); + } + _setSystemType(_mav, _mav->getSystemType()); + _updateArmingState(_mav->isArmed()); +} + +Vehicle::~Vehicle() +{ + // Stop listening for system messages + disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); + // 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, &Vehicle::_updateNavigationControllerData); + disconnect(_mav, &UASInterface::heartbeatTimeout, this, &Vehicle::_heartbeatTimeout); + disconnect(_mav, &UASInterface::batteryChanged, this, &Vehicle::_updateBatteryRemaining); + disconnect(_mav, &UASInterface::batteryConsumedChanged, this, &Vehicle::_updateBatteryConsumedChanged); + disconnect(_mav, &UASInterface::modeChanged, this, &Vehicle::_updateMode); + disconnect(_mav, &UASInterface::nameChanged, this, &Vehicle::_updateName); + disconnect(_mav, &UASInterface::systemTypeSet, this, &Vehicle::_setSystemType); + disconnect(_mav, &UASInterface::localizationChanged, this, &Vehicle::_setSatLoc); + if (_wpm) { + disconnect(_wpm, &UASWaypointManager::currentWaypointChanged, this, &Vehicle::_updateCurrentWaypoint); + disconnect(_wpm, &UASWaypointManager::waypointDistanceChanged, this, &Vehicle::_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, &Vehicle::_setSatelliteCount); + } } void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) @@ -159,8 +272,540 @@ void Vehicle::setLongitude(double longitude){ emit coordinateChanged(_geoCoordinate); } -void Vehicle::_setYaw(double yaw) +void Vehicle::_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 Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp) +{ + _updateAttitude(uas, roll, pitch, yaw, timestamp); +} + +void Vehicle::_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 Vehicle::_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 Vehicle::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) { + _navigationAltitudeError = altitudeError; + _navigationSpeedError = speedError; + _navigationCrosstrackError = xtrackError; +} + +void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) { + if (_mav == uas) { + _navigationTargetBearing = targetBearing; + } +} + +/* + * Internal + */ + +bool Vehicle::_isAirplane() { + if (_mav) + return _mav->isAirplane(); + return false; +} + +void Vehicle::_addChange(int id) +{ + if(!_changes.contains(id)) { + _changes.append(id); + } +} + +float Vehicle::_oneDecimal(float value) +{ + int i = (value * 10); + return (float)i / 10.0; +} + +void Vehicle::_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 Vehicle::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 Vehicle::_updateArmingState(bool armed) +{ + if(_systemArmed != armed) { + _systemArmed = armed; + emit systemArmedChanged(); + } +} + +void Vehicle::_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 Vehicle::_updateBatteryConsumedChanged(UASInterface*, double current_consumed) +{ + if(_batteryConsumed != current_consumed) { + _batteryConsumed = current_consumed; + emit batteryConsumedChanged(); + } +} + + +void Vehicle::_updateState(UASInterface*, QString name, QString) +{ + if (_currentState != name) { + _currentState = name; + emit currentStateChanged(); + } +} + +void Vehicle::_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 Vehicle::_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 Vehicle::_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 Vehicle::_heartbeatTimeout(bool timeout, unsigned int ms) +{ + unsigned int elapsed = ms; + if (!timeout) + { + elapsed = 0; + } + if(elapsed != _currentHeartbeatTimeout) { + _currentHeartbeatTimeout = elapsed; + emit heartbeatTimeoutChanged(); + } +} + +void Vehicle::_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 Vehicle::_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 Vehicle::_updateWaypointDistance(double distance) { - _heading = yaw * (180.0 / M_PI); - emit headingChanged(_heading); -} \ No newline at end of file + if (_waypointDistance != distance) { + _waypointDistance = distance; + emit waypointDistanceChanged(); + } +} + +void Vehicle::_updateCurrentWaypoint(quint16 id) +{ + if (_currentWaypoint != id) { + _currentWaypoint = id; + emit currentWaypointChanged(); + } +} + +void Vehicle::_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 Vehicle::_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(); + } + */ + } +} + +void Vehicle::_handleTextMessage(int newCount) +{ + // Reset? + if(!newCount) { + _currentMessageCount = 0; + _currentNormalCount = 0; + _currentWarningCount = 0; + _currentErrorCount = 0; + _messageCount = 0; + _currentMessageType = MessageNone; + emit newMessageCountChanged(); + emit messageTypeChanged(); + emit messageCountChanged(); + return; + } + + UASMessageHandler* pMh = UASMessageHandler::instance(); + Q_ASSERT(pMh); + MessageType_t type = newCount ? _currentMessageType : MessageNone; + int errorCount = _currentErrorCount; + int warnCount = _currentWarningCount; + int normalCount = _currentNormalCount; + //-- Add current message counts + errorCount += pMh->getErrorCount(); + warnCount += pMh->getWarningCount(); + normalCount += pMh->getNormalCount(); + //-- See if we have a higher level + if(errorCount != _currentErrorCount) { + _currentErrorCount = errorCount; + type = MessageError; + } + if(warnCount != _currentWarningCount) { + _currentWarningCount = warnCount; + if(_currentMessageType != MessageError) { + type = MessageWarning; + } + } + if(normalCount != _currentNormalCount) { + _currentNormalCount = normalCount; + if(_currentMessageType != MessageError && _currentMessageType != MessageWarning) { + type = MessageNormal; + } + } + int count = _currentErrorCount + _currentWarningCount + _currentNormalCount; + if(count != _currentMessageCount) { + _currentMessageCount = count; + // Display current total new messages count + emit newMessageCountChanged(); + } + if(type != _currentMessageType) { + _currentMessageType = type; + // Update message level + emit messageTypeChanged(); + } + // Update message count (all messages) + if(newCount != _messageCount) { + _messageCount = newCount; + emit messageCountChanged(); + } + QString errMsg = pMh->getLatestError(); + if(errMsg != _latestError) { + _latestError = errMsg; + emit latestErrorChanged(); + } +} + +void Vehicle::resetMessages() +{ + // Reset Counts + int count = _currentMessageCount; + MessageType_t type = _currentMessageType; + _currentErrorCount = 0; + _currentWarningCount = 0; + _currentNormalCount = 0; + _currentMessageCount = 0; + _currentMessageType = MessageNone; + if(count != _currentMessageCount) { + emit newMessageCountChanged(); + } + if(type != _currentMessageType) { + emit messageTypeChanged(); + } +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 0573924d8bdd5ae471755396819568ef36ae1c45..b38ac95c88d7e70245777af8ca4bcdf4d7552278 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -45,13 +45,50 @@ class Vehicle : public QObject public: Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType); + ~Vehicle(); Q_PROPERTY(int id READ id CONSTANT) Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged) - Q_PROPERTY(double heading MEMBER _heading NOTIFY headingChanged) + Q_INVOKABLE QString getMavIconColor(); + + //-- System Messages + Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeNormal READ messageTypeNormal NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeWarning READ messageTypeWarning NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeError READ messageTypeError NOTIFY messageTypeChanged) + Q_PROPERTY(int newMessageCount READ newMessageCount NOTIFY newMessageCountChanged) + Q_PROPERTY(int messageCount READ messageCount NOTIFY messageCountChanged) + Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) + //-- UAV Stats + 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(double batteryVoltage READ batteryVoltage NOTIFY batteryVoltageChanged) + Q_PROPERTY(double batteryPercent READ batteryPercent NOTIFY batteryPercentChanged) + Q_PROPERTY(double batteryConsumed READ batteryConsumed NOTIFY batteryConsumedChanged) + 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) + //-- Waypoint management + Q_PROPERTY(QQmlListProperty waypoints READ waypoints NOTIFY waypointsChanged) // Property accesors int id(void) { return _id; } @@ -68,6 +105,64 @@ public: QList links(void); + typedef enum { + MessageNone, + MessageNormal, + MessageWarning, + MessageError + } MessageType_t; + + enum { + ROLL_CHANGED, + PITCH_CHANGED, + HEADING_CHANGED, + GROUNDSPEED_CHANGED, + AIRSPEED_CHANGED, + CLIMBRATE_CHANGED, + ALTITUDERELATIVE_CHANGED, + ALTITUDEWGS84_CHANGED, + ALTITUDEAMSL_CHANGED + }; + + // Called when the message drop-down is invoked to clear current count + void resetMessages(); + + + bool messageTypeNone () { return _currentMessageType == MessageNone; } + bool messageTypeNormal () { return _currentMessageType == MessageNormal; } + bool messageTypeWarning () { return _currentMessageType == MessageWarning; } + bool messageTypeError () { return _currentMessageType == MessageError; } + int newMessageCount () { return _currentMessageCount; } + int messageCount () { return _messageCount; } + QString latestError () { return _latestError; } + 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; } + double batteryConsumed () { return _batteryConsumed; } + 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); } + public slots: void setLatitude(double latitude); void setLongitude(double longitude); @@ -75,21 +170,81 @@ public slots: signals: void allLinksDisconnected(void); void coordinateChanged(QGeoCoordinate coordinate); - void headingChanged(double heading); /// Used internally to move sendMessage call to main thread void _sendMessageOnThread(mavlink_message_t message); + void messageTypeChanged (); + void newMessageCountChanged (); + void messageCountChanged (); + void latestErrorChanged (); + void rollChanged (); + void pitchChanged (); + void headingChanged (); + void groundSpeedChanged (); + void airSpeedChanged (); + void climbRateChanged (); + void altitudeRelativeChanged(); + void altitudeWGS84Changed (); + void altitudeAMSLChanged (); + void latitudeChanged (); + void longitudeChanged (); + void batteryVoltageChanged (); + void batteryPercentChanged (); + void batteryConsumedChanged (); + 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: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkDisconnected(LinkInterface* link); void _sendMessage(mavlink_message_t message); - void _setYaw(double yaw); + void _handleTextMessage (int newCount); + /** @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 _checkUpdate (); + void _updateBatteryRemaining (UASInterface*, double voltage, double, double percent, int); + void _updateBatteryConsumedChanged (UASInterface*, double current_consumed); + 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 _containsLink(LinkInterface* link); void _addLink(LinkInterface* link); + bool _isAirplane (); + void _addChange (int id); + float _oneDecimal (float value); +private: int _id; ///< Mavlink system id MAV_AUTOPILOT _firmwareType; @@ -105,7 +260,46 @@ private: QGeoCoordinate _geoCoordinate; - double _heading; + UASInterface* _mav; + int _currentMessageCount; + int _messageCount; + int _currentErrorCount; + int _currentWarningCount; + int _currentNormalCount; + MessageType_t _currentMessageType; + QString _latestError; + 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; + double _batteryConsumed; + 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 diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 09122de81dcdd30a39e5ae9173cfa66085ed931e..e766ebca28fe3edb1646e654d5c84c76cb6bb74d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -298,7 +298,6 @@ void UAS::readSettings() QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); this->name = settings.value("NAME", this->name).toString(); - qDebug() << "Name" << name; this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); if (settings.contains("BATTERY_SPECS")) { diff --git a/src/ui/flightdisplay/FlightDisplay.qml b/src/ui/flightdisplay/FlightDisplay.qml index 2b9e06d0e83c0978016c98e347b71ec4d4d404db..360ab9c1dc504e10664067808d397c91331c262d 100644 --- a/src/ui/flightdisplay/FlightDisplay.qml +++ b/src/ui/flightdisplay/FlightDisplay.qml @@ -33,7 +33,6 @@ import QtQuick.Controls.Styles 1.2 import QtQuick.Dialogs 1.2 import QGroundControl.FlightMap 1.0 -import QGroundControl.MavManager 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controls 1.0 import QGroundControl.Palette 1.0 @@ -43,8 +42,27 @@ Item { property var __qgcPal: QGCPalette { colorGroupEnabled: enabled } - property real roll: isNaN(MavManager.roll) ? 0 : MavManager.roll - property real pitch: isNaN(MavManager.pitch) ? 0 : MavManager.pitch + property var activeVehicle: multiVehicleManager.activeVehicle + + readonly property real defaultLatitude: 37.803784 + readonly property real defaultLongitude: -122.462276 + readonly property real defaultRoll: 0 + readonly property real defaultPitch: 0 + readonly property real defaultHeading: 0 + readonly property real defaultAltitudeWGS84: 0 + readonly property real defaultGroundSpeed: 0 + readonly property real defaultAirSpeed: 0 + readonly property real defaultClimbRate: 0 + + property real roll: activeVehicle ? (isNaN(activeVehicle.roll) ? defaultRoll : activeVehicle.roll) : defaultRoll + property real pitch: activeVehicle ? (isNaN(activeVehicle.pitch) ? defaultPitch : activeVehicle.pitch) : defaultPitch + property real latitude: activeVehicle ? ((activeVehicle.latitude === 0) ? defaultLatitude : activeVehicle.latitude) : defaultLatitude + property real longitude: activeVehicle ? ((activeVehicle.longitude === 0) ? defaultlongitude : activeVehicle.longitude) : defaultLongitude + property real heading: activeVehicle ? (isNaN(activeVehicle.heading) ? defaultHeading : activeVehicle.heading) : defaultHeading + property real altitudeWGS84: activeVehicle ? activeVehicle.altitudeWGS84 : defaultAltitudeWGS84 + property real groundSpeed: activeVehicle ? activeVehicle.groundSpeed : defaultGroundSpeed + property real airSpeed: activeVehicle ? activeVehicle.airSpeed : defaultAirSpeed + property real climbRate: activeVehicle ? activeVehicle.climbRate : defaultClimbRate property bool showPitchIndicator: true @@ -73,7 +91,6 @@ Item { Component.onCompleted: { mapBackground.visible = getBool(flightDisplay.loadSetting("showMapBackground", "0")); - mapBackground.showWaypoints = getBool(flightDisplay.loadSetting("mapShowWaypoints", "0")); mapBackground.alwaysNorth = getBool(flightDisplay.loadSetting("mapAlwaysPointsNorth", "0")); videoBackground.visible = getBool(flightDisplay.loadSetting("showVideoBackground", "0")); showPitchIndicator = getBool(flightDisplay.loadSetting("showPitchIndicator", "1")); @@ -126,18 +143,6 @@ Item { } } - MenuItem { - text: "Map Show Waypoints" - checkable: true - checked: mapBackground.showWaypoints - enabled: mapBackground.visible - onTriggered: - { - mapBackground.showWaypoints = !mapBackground.showWaypoints; - flightDisplay.saveSetting("mapShowWaypoints", setBool(mapBackground.showWaypoints)); - } - } - /* //-- Off until Qt 5.5.x, which fixes bug in 5.4.x MenuItem { @@ -340,23 +345,12 @@ Item { id: mapBackground anchors.fill: parent 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 + latitude: mapBackground.visible ? root.latitude : root.defaultLatitude + longitude: mapBackground.visible ? root.longitude : root.defaultLongitude readOnly: true - //interactive: !MavManager.mavPresent z: 10 } - QGCHudMessage { - id: hudMessage - y: ScreenTools.defaultFontPizelSize * (0.42) - width: (parent.width - 520 > 200) ? parent.width - 520 : 200 - height: ScreenTools.defaultFontPizelSize * (2.5) - anchors.horizontalCenter: parent.horizontalCenter - z: mapBackground.z + 1 - } - // Floating (Top Left) Compass Widget QGCCompassWidget { @@ -364,7 +358,7 @@ Item { y: ScreenTools.defaultFontPixelSize * (0.42) x: ScreenTools.defaultFontPixelSize * (7.1) size: ScreenTools.defaultFontPixelSize * (13.3) - heading: isNaN(MavManager.heading) ? 0 : MavManager.heading + heading: root.heading z: mapBackground.z + 2 onResetRequested: { y = ScreenTools.defaultFontPixelSize * (0.42) @@ -383,7 +377,7 @@ Item { x: root.width * 0.5 - ScreenTools.defaultFontPixelSize * (5) width: ScreenTools.defaultFontPixelSize * (10) height: ScreenTools.defaultFontPixelSize * (10) - heading: isNaN(MavManager.heading) ? 0 : MavManager.heading + heading: root.heading z: 70 } @@ -436,7 +430,7 @@ Item { anchors.right: parent.right width: ScreenTools.defaultFontPixelSize * (5) height: parent.height * 0.65 > ScreenTools.defaultFontPixelSize * (23.4) ? ScreenTools.defaultFontPixelSize * (23.4) : parent.height * 0.65 - altitude: MavManager.altitudeWGS84 + altitude: root.altitudeWGS84 z: 30 } @@ -445,7 +439,7 @@ Item { anchors.left: parent.left width: ScreenTools.defaultFontPixelSize * (5) height: parent.height * 0.65 > ScreenTools.defaultFontPixelSize * (23.4) ? ScreenTools.defaultFontPixelSize * (23.4) : parent.height * 0.65 - speed: MavManager.groundSpeed + speed: root.groundSpeed z: 40 } @@ -453,8 +447,8 @@ Item { id: currentSpeed anchors.left: parent.left width: ScreenTools.defaultFontPixelSize * (6.25) - airspeed: MavManager.airSpeed - groundspeed: MavManager.groundSpeed + airspeed: root.airSpeed + groundspeed: root.groundSpeed showAirSpeed: true showGroundSpeed: true visible: (currentSpeed.showGroundSpeed || currentSpeed.showAirSpeed) @@ -465,8 +459,8 @@ Item { id: currentAltitude anchors.right: parent.right width: ScreenTools.defaultFontPixelSize * (6.25) - altitude: MavManager.altitudeWGS84 - vertZ: MavManager.climbRate + altitude: root.altitudeWGS84 + vertZ: root.climbRate showAltitude: true showClimbRate: true visible: (currentAltitude.showAltitude || currentAltitude.showClimbRate) diff --git a/src/ui/toolbar/MainToolBar.cc b/src/ui/toolbar/MainToolBar.cc index 9199d639b400e120dd5c940554ba32688497c855..18164abca684acf11b5f4b29fe58668bd2000e2a 100644 --- a/src/ui/toolbar/MainToolBar.cc +++ b/src/ui/toolbar/MainToolBar.cc @@ -37,7 +37,6 @@ This file is part of the QGROUNDCONTROL project #include "UASMessageHandler.h" #include "FlightDisplay.h" #include "QGCApplication.h" -#include "MavManager.h" #include "MultiVehicleManager.h" MainToolBar::MainToolBar(QWidget* parent) @@ -220,8 +219,8 @@ void MainToolBar::onEnterMessageArea(int x, int y) // If not already there and messages are actually present if(!_rollDownMessages && UASMessageHandler::instance()->messages().count()) { - if(qgcApp()->getMavManager()) - qgcApp()->getMavManager()->resetMessages(); + if (MultiVehicleManager::instance()->activeVehicle()) + MultiVehicleManager::instance()->activeVehicle()->resetMessages(); // Show messages int dialogWidth = 400; x = x - (dialogWidth >> 1); diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index 9bca0b274eb4b0320ff182f9271b20e00016ac39..f75fd16fddbc1373fd73467496683186c8a172d3 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/src/ui/toolbar/MainToolBar.qml @@ -31,18 +31,20 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Controls.Styles 1.2 -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 +import QGroundControl.Controls 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.MainToolBar 1.0 +import QGroundControl.MultiVehicleManager 1.0 +import QGroundControl.ScreenTools 1.0 Rectangle { id: toolBarHolder property var qgcPal: QGCPalette { id: palette; colorGroupEnabled: true } + property var activeVehicle: multiVehicleManager.activeVehicle + readonly property real toolBarHeight: ScreenTools.defaultFontPixelHeight * 3 property int cellSpacerSize: ScreenTools.isMobile ? getProportionalDimmension(6) : getProportionalDimmension(4) readonly property int cellHeight: toolBarHeight * 0.75 @@ -82,44 +84,44 @@ Rectangle { } function getMessageColor() { - if(MavManager.messageType === MavManager.MessageNone) + if (activeVehicle.messageTypeNone) return qgcPal.button; - if(MavManager.messageType === MavManager.MessageNormal) + if (activeVehicle.messageTypeNorma) return colorBlue; - if(MavManager.messageType === MavManager.MessageWarning) + if (activeVehicle.messageTypeWarning) return colorOrange; - if(MavManager.messageType === MavManager.MessageError) + if (activeVehicle.messageTypeError) return colorRed; // Cannot be so make make it obnoxious to show error return "purple"; } function getMessageIcon() { - if(MavManager.messageType === MavManager.MessageNormal || MavManager.messageType === MavManager.MessageNone) + if (activeVehicle.messageTypeNormal || activeVehicle.messageTypeNone) return "qrc:/res/Megaphone"; else return "qrc:/res/Yield"; } function getBatteryIcon() { - if(MavManager.batteryPercent < 20.0) + if(activeVehicle.batteryPercent < 20.0) return "qrc:/res/Battery_0"; - else if(MavManager.batteryPercent < 40.0) + else if(activeVehicle.batteryPercent < 40.0) return "qrc:/res/Battery_20"; - else if(MavManager.batteryPercent < 60.0) + else if(activeVehicle.batteryPercent < 60.0) return "qrc:/res/Battery_40"; - else if(MavManager.batteryPercent < 80.0) + else if(activeVehicle.batteryPercent < 80.0) return "qrc:/res/Battery_60"; - else if(MavManager.batteryPercent < 90.0) + else if(activeVehicle.batteryPercent < 90.0) return "qrc:/res/Battery_80"; else return "qrc:/res/Battery_100"; } function getBatteryColor() { - if (MavManager.batteryPercent > 40.0) + if (activeVehicle.batteryPercent > 40.0) return colorGreen; - if(MavManager.batteryPercent > 0.01) + if(activeVehicle.batteryPercent > 0.01) return colorRed; // This means there is no battery level data return colorBlue; @@ -127,13 +129,13 @@ Rectangle { function getSatelliteColor() { // No GPS data - if (MavManager.satelliteCount < 0) + if (activeVehicle.satelliteCount < 0) return qgcPal.button // No Lock - if(MavManager.satelliteLock < 2) + if(activeVehicle.satelliteLock < 2) return colorRed; // 2D Lock - if(MavManager.satelliteLock === 2) + if(activeVehicle.satelliteLock === 2) return colorBlue; // Lock is 3D or more return colorGreen; @@ -148,7 +150,7 @@ Rectangle { } function showMavStatus() { - return (MavManager.mavPresent && MavManager.heartbeatTimeout === 0 && mainToolBar.connectionCount > 0); + return (multiVehicleManager.activeVehicleAvailable && activeVehicle.heartbeatTimeout === 0 && mainToolBar.connectionCount > 0); } //------------------------------------------------------------------------- @@ -197,136 +199,18 @@ Rectangle { } } // Menu - Row { - id: toolRow - x: horizontalMargins - y: (toolBarHeight - cellHeight) / 2 - height: cellHeight - spacing: getProportionalDimmension(4) - - //--------------------------------------------------------------------- - //-- Main menu for Non Mobile Devices (Chevron Buttons) - Row { - id: row11 - height: cellHeight - spacing: -getProportionalDimmension(12) - anchors.top: parent.top - visible: !ScreenTools.isMobile - - Connections { - target: ScreenTools - onRepaintRequested: { - setupButton.repaintChevron = true; - planButton.repaintChevron = true; - flyButton.repaintChevron = true; - analyzeButton.repaintChevron = true; - } - } - - ExclusiveGroup { id: mainActionGroup } - - QGCToolBarButton { - id: setupButton - width: getProportionalDimmension(90) - height: cellHeight - exclusiveGroup: mainActionGroup - text: qsTr("Setup") - checked: (mainToolBar.currentView === MainToolBar.ViewSetup) - onClicked: { - mainToolBar.onSetupView(); - } - z: 1000 - } - - QGCToolBarButton { - id: planButton - width: getProportionalDimmension(90) - height: cellHeight - exclusiveGroup: mainActionGroup - text: qsTr("Plan") - checked: (mainToolBar.currentView === MainToolBar.ViewPlan) - onClicked: { - mainToolBar.onPlanView(); - } - z: 900 - } - - QGCToolBarButton { - id: flyButton - width: getProportionalDimmension(90) - height: cellHeight - exclusiveGroup: mainActionGroup - text: qsTr("Fly") - checked: (mainToolBar.currentView === MainToolBar.ViewFly) - onClicked: { - mainToolBar.onFlyView(); - } - z: 800 - } - - QGCToolBarButton { - id: analyzeButton - width: getProportionalDimmension(90) - height: cellHeight - exclusiveGroup: mainActionGroup - text: qsTr("Analyze") - checked: (mainToolBar.currentView === MainToolBar.ViewAnalyze) - onClicked: { - mainToolBar.onAnalyzeView(); - } - z: 700 - } - } // Row + Component { + id: activeVehicleComponent - //--------------------------------------------------------------------- - //-- Indicators Row { - id: row12 - height: cellHeight - spacing: cellSpacerSize - anchors.verticalCenter: parent.verticalCenter - - //-- "Hamburger" menu for Mobile Devices - Item { - id: actionButton - visible: ScreenTools.isMobile - height: cellHeight - width: cellHeight - Image { - id: buttomImg - anchors.fill: parent - source: "/qmlimages/buttonMore.svg" - mipmap: true - smooth: true - antialiasing: true - fillMode: Image.PreserveAspectFit - } - MouseArea { - anchors.fill: parent - acceptedButtons: Qt.LeftButton - onClicked: { - if (mouse.button == Qt.LeftButton) - { - maintMenu.popup(); - } - } - } - } - - //-- Separator if Hamburger menu is visible - Rectangle { - visible: actionButton.visible - height: cellHeight - width: cellHeight - color: "#00000000" - anchors.verticalCenter: parent.verticalCenter - } + height: cellHeight + spacing: cellSpacerSize Rectangle { id: messages - width: (MavManager.messageCount > 99) ? getProportionalDimmension(65) : getProportionalDimmension(60) + width: (activeVehicle.messageCount > 99) ? getProportionalDimmension(65) : getProportionalDimmension(60) height: cellHeight - visible: (mainToolBar.connectionCount > 0) && (mainToolBar.showMessages) + visible: mainToolBar.showMessages anchors.verticalCenter: parent.verticalCenter color: getMessageColor() border.color: "#00000000" @@ -350,7 +234,7 @@ Rectangle { width: messages.width - messageIcon.width QGCLabel { id: messageText - text: (MavManager.messageCount > 0) ? MavManager.messageCount : '' + text: (activeVehicle.messageCount > 0) ? activeVehicle.messageCount : '' font.pixelSize: ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.verticalCenter: parent.verticalCenter @@ -363,7 +247,7 @@ Rectangle { Image { id: dropDown source: "/qmlimages/arrow-down.png" - visible: (messages.showTriangle) && (MavManager.messageCount > 0) + visible: (messages.showTriangle) && (activeVehicle.messageCount > 0) anchors.bottom: parent.bottom anchors.right: parent.right anchors.bottomMargin: getProportionalDimmension(3) @@ -399,13 +283,13 @@ Rectangle { id: mavIcon width: cellHeight height: cellHeight - visible: showMavStatus() && (mainToolBar.showMav) + visible: mainToolBar.showMav anchors.verticalCenter: parent.verticalCenter color: colorBlue border.color: "#00000000" border.width: 0 Image { - source: MavManager.systemPixmap + source: activeVehicle.systemPixmap height: cellHeight * 0.75 fillMode: Image.PreserveAspectFit anchors.verticalCenter: parent.verticalCenter @@ -417,7 +301,7 @@ Rectangle { id: satelitte width: getProportionalDimmension(55) height: cellHeight - visible: showMavStatus() && (mainToolBar.showGPS) + visible: mainToolBar.showGPS anchors.verticalCenter: parent.verticalCenter color: getSatelliteColor(); border.color: "#00000000" @@ -436,8 +320,8 @@ Rectangle { QGCLabel { id: satelitteText - text: MavManager.satelliteCount >= 0 ? MavManager.satelliteCount : 'NA' - font.pixelSize: MavManager.satelliteCount >= 0 ? ScreenTools.defaultFontPixelSize : ScreenTools.smallFontPixelSize + text: activeVehicle.satelliteCount >= 0 ? activeVehicle.satelliteCount : 'NA' + font.pixelSize: activeVehicle.satelliteCount >= 0 ? ScreenTools.defaultFontPixelSize : ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.verticalCenter: parent.verticalCenter anchors.right: parent.right @@ -451,7 +335,7 @@ Rectangle { id: rssiRC width: getProportionalDimmension(55) height: cellHeight - visible: showMavStatus() && mainToolBar.showRSSI && mainToolBar.remoteRSSI <= 100 + visible: mainToolBar.showRSSI && mainToolBar.remoteRSSI <= 100 anchors.verticalCenter: parent.verticalCenter color: getRSSIColor(mainToolBar.remoteRSSI); border.color: "#00000000" @@ -482,7 +366,7 @@ Rectangle { id: rssiTelemetry width: getProportionalDimmension(80) height: cellHeight - visible: showMavStatus() && (mainToolBar.showRSSI) && ((mainToolBar.telemetryRRSSI > 0) && (mainToolBar.telemetryLRSSI > 0)) + visible: mainToolBar.showRSSI && (mainToolBar.telemetryRRSSI > 0) && (mainToolBar.telemetryLRSSI > 0) anchors.verticalCenter: parent.verticalCenter color: getRSSIColor(Math.min(mainToolBar.telemetryRRSSI,mainToolBar.telemetryLRSSI)); border.color: "#00000000" @@ -540,9 +424,9 @@ Rectangle { Rectangle { id: batteryStatus - width: MavManager.batteryConsumed < 0.0 ? getProportionalDimmension(60) : getProportionalDimmension(80) + width: activeVehicle.batteryConsumed < 0.0 ? getProportionalDimmension(60) : getProportionalDimmension(80) height: cellHeight - visible: showMavStatus() && (mainToolBar.showBattery) + visible: mainToolBar.showBattery anchors.verticalCenter: parent.verticalCenter color: getBatteryColor(); border.color: "#00000000" @@ -559,8 +443,8 @@ Rectangle { } QGCLabel { - visible: batteryStatus.visible && MavManager.batteryConsumed < 0.0 - text: MavManager.batteryVoltage.toFixed(1) + 'V'; + visible: batteryStatus.visible && activeVehicle.batteryConsumed < 0.0 + text: activeVehicle.batteryVoltage.toFixed(1) + 'V'; font.pixelSize: ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.right: parent.right @@ -574,9 +458,9 @@ Rectangle { anchors.verticalCenter: parent.verticalCenter anchors.right: parent.right anchors.rightMargin: getProportionalDimmension(6) - visible: batteryStatus.visible && MavManager.batteryConsumed >= 0.0 + visible: batteryStatus.visible && activeVehicle.batteryConsumed >= 0.0 QGCLabel { - text: MavManager.batteryVoltage.toFixed(1) + 'V'; + text: activeVehicle.batteryVoltage.toFixed(1) + 'V'; width: getProportionalDimmension(30) horizontalAlignment: Text.AlignRight font.pixelSize: ScreenTools.smallFontPixelSize @@ -584,7 +468,7 @@ Rectangle { color: colorWhite } QGCLabel { - text: MavManager.batteryConsumed.toFixed(0) + 'mAh'; + text: activeVehicle.batteryConsumed.toFixed(0) + 'mAh'; width: getProportionalDimmension(30) horizontalAlignment: Text.AlignRight font.pixelSize: ScreenTools.smallFontPixelSize @@ -595,7 +479,6 @@ Rectangle { } Column { - visible: showMavStatus() height: cellHeight * 0.85 width: getProportionalDimmension(80) anchors.verticalCenter: parent.verticalCenter @@ -611,11 +494,11 @@ Rectangle { QGCLabel { id: armedStatusText - text: (MavManager.systemArmed) ? qsTr("ARMED") : qsTr("DISARMED") + text: (activeVehicle.systemArmed) ? qsTr("ARMED") : qsTr("DISARMED") font.pixelSize: ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.centerIn: parent - color: (MavManager.systemArmed) ? colorOrangeText : colorGreenText + color: (activeVehicle.systemArmed) ? colorOrangeText : colorGreenText } } @@ -630,11 +513,11 @@ Rectangle { QGCLabel { id: stateStatusText - text: MavManager.currentState + text: activeVehicle.currentState font.pixelSize: ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.centerIn: parent - color: (MavManager.currentState === "STANDBY") ? colorGreenText : colorRedText + color: (activeVehicle.currentState === "STANDBY") ? colorGreenText : colorRedText } } @@ -644,14 +527,13 @@ Rectangle { id: modeStatus width: getProportionalDimmension(90) height: cellHeight - visible: showMavStatus() color: "#00000000" border.color: "#00000000" border.width: 0 QGCLabel { id: modeStatusText - text: MavManager.currentMode + text: activeVehicle.currentMode font.pixelSize: ScreenTools.smallFontPixelSize font.weight: Font.DemiBold anchors.horizontalCenter: parent.horizontalCenter @@ -659,12 +541,148 @@ Rectangle { color: colorWhiteText } } + } // Row + } // Component - activeVehicleComponent + + Row { + id: toolRow + x: horizontalMargins + y: (toolBarHeight - cellHeight) / 2 + height: cellHeight + spacing: getProportionalDimmension(4) + + //--------------------------------------------------------------------- + //-- Main menu for Non Mobile Devices (Chevron Buttons) + Row { + id: row11 + height: cellHeight + spacing: -getProportionalDimmension(12) + anchors.top: parent.top + visible: !ScreenTools.isMobile + + Connections { + target: ScreenTools + onRepaintRequested: { + setupButton.repaintChevron = true; + planButton.repaintChevron = true; + flyButton.repaintChevron = true; + analyzeButton.repaintChevron = true; + } + } + + ExclusiveGroup { id: mainActionGroup } + + QGCToolBarButton { + id: setupButton + width: getProportionalDimmension(90) + height: cellHeight + exclusiveGroup: mainActionGroup + text: qsTr("Setup") + checked: (mainToolBar.currentView === MainToolBar.ViewSetup) + onClicked: { + mainToolBar.onSetupView(); + } + z: 1000 + } + + QGCToolBarButton { + id: planButton + width: getProportionalDimmension(90) + height: cellHeight + exclusiveGroup: mainActionGroup + text: qsTr("Plan") + checked: (mainToolBar.currentView === MainToolBar.ViewPlan) + onClicked: { + mainToolBar.onPlanView(); + } + z: 900 + } + + QGCToolBarButton { + id: flyButton + width: getProportionalDimmension(90) + height: cellHeight + exclusiveGroup: mainActionGroup + text: qsTr("Fly") + checked: (mainToolBar.currentView === MainToolBar.ViewFly) + onClicked: { + mainToolBar.onFlyView(); + } + z: 800 + } + + QGCToolBarButton { + id: analyzeButton + width: getProportionalDimmension(90) + height: cellHeight + exclusiveGroup: mainActionGroup + text: qsTr("Analyze") + checked: (mainToolBar.currentView === MainToolBar.ViewAnalyze) + onClicked: { + mainToolBar.onAnalyzeView(); + } + z: 700 + } + } // Row + + //--------------------------------------------------------------------- + //-- Indicators + Row { + id: row12 + height: cellHeight + spacing: cellSpacerSize + anchors.verticalCenter: parent.verticalCenter + + //-- "Hamburger" menu for Mobile Devices + Item { + id: actionButton + visible: ScreenTools.isMobile + height: cellHeight + width: cellHeight + Image { + id: buttomImg + anchors.fill: parent + source: "/qmlimages/buttonMore.svg" + mipmap: true + smooth: true + antialiasing: true + fillMode: Image.PreserveAspectFit + } + MouseArea { + anchors.fill: parent + acceptedButtons: Qt.LeftButton + onClicked: { + if (mouse.button == Qt.LeftButton) + { + maintMenu.popup(); + } + } + } + } + + //-- Separator if Hamburger menu is visible + Rectangle { + visible: actionButton.visible + height: cellHeight + width: cellHeight + color: "#00000000" + anchors.verticalCenter: parent.verticalCenter + } + + Loader { + id: activeVehicleLoader + visible: showMavStatus() + sourceComponent: multiVehicleManager.activeVehicleAvailable ? activeVehicleComponent : undefined + + property real cellHeight: toolBarHolder.cellHeight + property real cellSpacerSize: toolBarHolder.cellSpacerSize + } Rectangle { id: connectionStatus width: getProportionalDimmension(160) height: cellHeight - visible: (mainToolBar.connectionCount > 0 && MavManager.mavPresent && MavManager.heartbeatTimeout != 0) + visible: (mainToolBar.connectionCount > 0 && multiVehicleManager.activeVehicleAvailable && activeVehicle.heartbeatTimeout != 0) anchors.verticalCenter: parent.verticalCenter color: "#00000000" border.color: "#00000000"