From 2b206c002481be5205158a90dc57ad421a198510 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 5 Oct 2015 09:14:00 -0700 Subject: [PATCH] New toolbar dropdown support - select active vehicle - arm/disarm - select flight mode --- src/AutoPilotPlugins/AutoPilotPlugin.cc | 6 - src/AutoPilotPlugins/AutoPilotPlugin.h | 4 - src/AutoPilotPlugins/PX4/SensorsComponent.cc | 1 - src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 2 +- src/Joystick/Joystick.cc | 47 ++--- src/Joystick/Joystick.h | 7 +- src/QmlControls/QmlObjectListModel.cc | 3 + src/Vehicle/MultiVehicleManager.h | 2 +- src/Vehicle/Vehicle.cc | 168 ++++++++++++----- src/Vehicle/Vehicle.h | 42 +++-- src/VehicleSetup/JoystickConfig.qml | 8 +- src/VehicleSetup/SetupView.qml | 6 +- src/comm/MockLink.cc | 1 - src/qgcunittest/MavlinkLogTest.cc | 5 +- src/ui/MainWindow.cc | 12 +- src/ui/MainWindow.h | 2 - src/ui/WaypointEditableView.cc | 1 + src/ui/toolbar/MainToolBar.qml | 178 ++++++++++++------- src/ui/uas/UASInfoWidget.h | 3 - 19 files changed, 322 insertions(+), 176 deletions(-) diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index c8dfa0cce..837798dc3 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -39,7 +39,6 @@ AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent) : Q_ASSERT(vehicle); connect(_vehicle->uas(), &UASInterface::disconnected, this, &AutoPilotPlugin::_uasDisconnected); - connect(_vehicle->uas(), &UASInterface::armingChanged, this, &AutoPilotPlugin::armedChanged); connect(this, &AutoPilotPlugin::pluginReadyChanged, this, &AutoPilotPlugin::_pluginReadyChanged); } @@ -176,8 +175,3 @@ QString AutoPilotPlugin::readParametersFromStream(QTextStream &stream) { return _getParameterLoader()->readParametersFromStream(stream); } - -bool AutoPilotPlugin::armed(void) -{ - return _vehicle->uas()->isArmed(); -} diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 47ac288a2..c96ccf4e5 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -63,8 +63,6 @@ public: /// false: One or more vehicle components require setup Q_PROPERTY(bool setupComplete READ setupComplete NOTIFY setupCompleteChanged) - Q_PROPERTY(bool armed READ armed NOTIFY armedChanged) - /// Reset all parameters to their default values Q_INVOKABLE void resetAllParametersToDefaults(void); @@ -117,7 +115,6 @@ public: // Property accessors bool pluginReady(void) { return _pluginReady; } bool setupComplete(void); - bool armed(void); Vehicle* vehicle(void) { return _vehicle; } @@ -125,7 +122,6 @@ signals: void pluginReadyChanged(bool pluginReady); void setupCompleteChanged(bool setupComplete); void parameterListProgress(float value); - void armedChanged(bool armed); protected: /// All access to AutoPilotPugin objects is through getInstanceForAutoPilotPlugin diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.cc b/src/AutoPilotPlugins/PX4/SensorsComponent.cc index e6b64d464..c6311fcf5 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc @@ -115,7 +115,6 @@ QUrl SensorsComponent::summaryQmlSource(void) const { QString summaryQml; - qDebug() << _uas->getSystemType(); if (_uas->getSystemType() == MAV_TYPE_FIXED_WING || _uas->getSystemType() == MAV_TYPE_VTOL_DUOROTOR || _uas->getSystemType() == MAV_TYPE_VTOL_QUADROTOR || diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 3acb4f403..6ade5c344 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -191,5 +191,5 @@ void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) { - return capabilities == MavCmdPreflightStorageCapability; + return capabilities && (MavCmdPreflightStorageCapability | SetFlightModeCapability); } diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index e5aa66495..c5df9fe70 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -42,7 +42,7 @@ QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") const char* Joystick::_settingsGroup = "Joysticks"; const char* Joystick::_calibratedSettingsKey = "Calibrated"; -const char* Joystick::_buttonActionSettingsKey = "ButtonAction%1"; +const char* Joystick::_buttonActionSettingsKey = "ButtonActionName%1"; const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { @@ -144,10 +144,8 @@ void Joystick::_loadSettings(void) } for (int button=0; button<_cButtons; button++) { - _rgButtonActions[button] = settings.value(QString(_buttonActionSettingsKey).arg(button), -1).toInt(&convertOk); - badSettings |= !convertOk; - - qCDebug(JoystickLog) << "_loadSettings button:action:badsettings" << button << _rgButtonActions[button] << badSettings; + _rgButtonActions[button] = settings.value(QString(_buttonActionSettingsKey).arg(button), QString()).toString(); + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _rgButtonActions[button]; } if (badSettings) { @@ -322,10 +320,9 @@ void Joystick::run(void) if (buttonIndex >= reservedButtonCount) { // Button is above firmware reserved set - int buttonAction =_rgButtonActions[buttonIndex]; - if (buttonAction != -1) { - qCDebug(JoystickLog) << "buttonActionTriggered" << buttonAction; - emit buttonActionTriggered(buttonAction); + QString buttonAction =_rgButtonActions[buttonIndex]; + if (!buttonAction.isEmpty()) { + _buttonAction(buttonAction); } } } @@ -369,7 +366,8 @@ void Joystick::startPolling(Vehicle* vehicle) UAS* uas = _activeVehicle->uas(); connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); - connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); + // FIXME: **** + //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); _exitThread = false; start(); @@ -382,7 +380,8 @@ void Joystick::stopPolling(void) UAS* uas = _activeVehicle->uas(); disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); - disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); + // FIXME: **** + //disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); _exitThread = true; } @@ -435,27 +434,27 @@ int Joystick::getFunctionAxis(AxisFunction_t function) QStringList Joystick::actions(void) { QStringList list; - - foreach(QAction* action, MultiVehicleManager::instance()->activeVehicle()->uas()->getActions()) { - list += action->text(); - } + + list << "Arm" << "Disarm"; return list; } -void Joystick::setButtonAction(int button, int action) +void Joystick::setButtonAction(int button, const QString& action) { if (button < 0 || button > _cButtons) { qCWarning(JoystickLog) << "Invalid button index" << button; return; } + qDebug() << "setButtonAction" << action; + _rgButtonActions[button] = action; _saveSettings(); emit buttonActionsChanged(buttonActions()); } -int Joystick::getButtonAction(int button) +QString Joystick::getButtonAction(int button) { if (button < 0 || button > _cButtons) { qCWarning(JoystickLog) << "Invalid button index" << button; @@ -512,9 +511,6 @@ void Joystick::stopCalibrationMode(CalibrationMode_t mode) if (mode == CalibrationModeOff) { qWarning() << "Incorrect mode: CalibrationModeOff"; return; - } else if (mode != _calibrationMode) { - qWarning() << "Incorrect mode sequence request:active" << mode << _calibrationMode; - return; } if (mode == CalibrationModeCalibrating) { @@ -527,4 +523,15 @@ void Joystick::stopCalibrationMode(CalibrationMode_t mode) } } +void Joystick::_buttonAction(const QString& action) +{ + if (action == "Arm") { + _activeVehicle->setArmed(true); + } else if (action == "Disarm") { + _activeVehicle->setArmed(false); + } else { + qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; + } +} + #endif // __mobile__ diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 7d699b1c4..669d46e95 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -73,8 +73,8 @@ public: Q_PROPERTY(QStringList actions READ actions CONSTANT) Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged) - Q_INVOKABLE void setButtonAction(int button, int action); - Q_INVOKABLE int getButtonAction(int button); + Q_INVOKABLE void setButtonAction(int button, const QString& action); + Q_INVOKABLE QString getButtonAction(int button); Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) @@ -135,6 +135,7 @@ private: void _saveSettings(void); void _loadSettings(void); float _adjustRange(int value, Calibration_t calibration); + void _buttonAction(const QString& action); // Override from QThread virtual void run(void); @@ -158,7 +159,7 @@ private: static const int _cButtons = 12; bool _rgButtonValues[_cButtons]; - int _rgButtonActions[_cButtons]; + QString _rgButtonActions[_cButtons]; quint16 _lastButtonBits; ThrottleMode_t _throttleMode; diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index dde0f688f..58017c847 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -27,6 +27,7 @@ #include "QmlObjectListModel.h" #include +#include const int QmlObjectListModel::ObjectRole = Qt::UserRole; const int QmlObjectListModel::TextRole = Qt::UserRole + 1; @@ -156,6 +157,8 @@ void QmlObjectListModel::insert(int i, QObject* object) qWarning() << "Invalid index index:count" << i << _objectList.count(); } + QQmlEngine::setObjectOwnership(object, QQmlEngine::CppOwnership); + _objectList.insert(i, object); insertRows(i, 1); } diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index bf740b28f..0b969bab0 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -58,7 +58,7 @@ public: /// @return true: continue further processing of this message, false: disregard this message bool notifyHeartbeatInfo(LinkInterface* link, int vehicleId, mavlink_heartbeat_t& heartbeat); - Vehicle* getVehicleById(int vehicleId); + Q_INVOKABLE Vehicle* getVehicleById(int vehicleId); void setHomePositionForAllVehicles(double lat, double lon, double alt); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6bcc7a9b2..35d07524d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -78,7 +78,6 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _batteryVoltage(0.0) , _batteryPercent(0.0) , _batteryConsumed(-1.0) - , _systemArmed(false) , _currentHeartbeatTimeout(0) , _waypointDistance(0.0) , _currentWaypoint(0) @@ -86,13 +85,18 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _satelliteLock(0) , _wpm(NULL) , _updateCount(0) + , _armed(false) + , _base_mode(0) + , _custom_mode(0) { _addLink(link); - connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); + _mavlink = MAVLinkProtocol::instance(); + + connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection); - _uas = new UAS(MAVLinkProtocol::instance(), this); + _uas = new UAS(_mavlink, this); setLatitude(_uas->getLatitude()); setLongitude(_uas->getLongitude()); @@ -125,12 +129,10 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) 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); @@ -148,7 +150,6 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) connect(pUas, &UAS::satelliteCountChanged, this, &Vehicle::_setSatelliteCount); } _setSystemType(_mav, _mav->getSystemType()); - _updateArmingState(_mav->isArmed()); _waypointViewOnlyListChanged(); @@ -170,12 +171,10 @@ Vehicle::~Vehicle() 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); @@ -204,18 +203,13 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes // Give the plugin a change to adjust the message contents _firmwarePlugin->adjustMavlinkMessage(&message); - if (message.msgid == MAVLINK_MSG_ID_HOME_POSITION) { - mavlink_home_position_t homePos; - - mavlink_msg_home_position_decode(&message, &homePos); - - _homePosition.setLatitude(homePos.latitude / 10000000.0); - _homePosition.setLongitude(homePos.longitude / 10000000.0); - _homePosition.setAltitude(homePos.altitude / 1000.0); - _homePositionAvailable = true; - - emit homePositionAvailableChanged(true); - emit homePositionChanged(_homePosition); + switch (message.msgid) { + case MAVLINK_MSG_ID_HOME_POSITION: + _handleHomePosition(message); + break; + case MAVLINK_MSG_ID_HEARTBEAT: + _handleHeartbeat(message); + break; } emit mavlinkMessageReceived(message); @@ -223,6 +217,41 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } +void Vehicle::_handleHomePosition(mavlink_message_t& message) +{ + mavlink_home_position_t homePos; + + mavlink_msg_home_position_decode(&message, &homePos); + + _homePosition.setLatitude(homePos.latitude / 10000000.0); + _homePosition.setLongitude(homePos.longitude / 10000000.0); + _homePosition.setAltitude(homePos.altitude / 1000.0); + _homePositionAvailable = true; + + emit homePositionAvailableChanged(true); + emit homePositionChanged(_homePosition); +} + +void Vehicle::_handleHeartbeat(mavlink_message_t& message) +{ + mavlink_heartbeat_t heartbeat; + + mavlink_msg_heartbeat_decode(&message, &heartbeat); + + bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; + + if (_armed != newArmed) { + _armed = newArmed; + emit armedChanged(_armed); + } + + if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) { + _base_mode = heartbeat.base_mode; + _custom_mode = heartbeat.custom_mode; + emit flightModeChanged(flightMode()); + } +} + bool Vehicle::_containsLink(LinkInterface* link) { foreach (SharedLinkInterface sharedLink, _links) { @@ -273,7 +302,7 @@ void Vehicle::_sendMessage(mavlink_message_t message) Q_ASSERT(link); if (link->isConnected()) { - MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); + MAVLinkProtocol* mavlink = _mavlink; // Give the plugin a chance to adjust _firmwarePlugin->adjustMavlinkMessage(&message); @@ -542,14 +571,6 @@ QString Vehicle::getMavIconColor() 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) { @@ -586,19 +607,6 @@ void Vehicle::_updateState(UASInterface*, QString name, QString) } } -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) { @@ -974,3 +982,79 @@ QGeoCoordinate Vehicle::homePosition(void) return _homePosition; } + +void Vehicle::setArmed(bool armed) +{ + // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks. + + mavlink_message_t msg; + mavlink_command_long_t cmd; + + cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM; + cmd.confirmation = 0; + cmd.param1 = armed ? 1.0f : 0.0f; + cmd.param2 = 0.0f; + cmd.param3 = 0.0f; + cmd.param4 = 0.0f; + cmd.param5 = 0.0f; + cmd.param6 = 0.0f; + cmd.param7 = 0.0f; + cmd.target_system = id(); + cmd.target_component = 0; + + mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); + + sendMessage(msg); +} + +bool Vehicle::flightModeSetAvailable(void) +{ + return _firmwarePlugin->isCapable(FirmwarePlugin::SetFlightModeCapability); +} + +QStringList Vehicle::flightModes(void) +{ + return _firmwarePlugin->flightModes(); +} + +QString Vehicle::flightMode(void) +{ + return _firmwarePlugin->flightMode(_base_mode, _custom_mode); +} + +void Vehicle::setFlightMode(const QString& flightMode) +{ + uint8_t base_mode; + uint32_t custom_mode; + + if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) { + // setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing + // states. + uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE; + newBaseMode |= base_mode; + + mavlink_message_t msg; + mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, custom_mode); + sendMessage(msg); + } else { + qCWarning(VehicleLog) << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; + } +} + +bool Vehicle::hilMode(void) +{ + return _base_mode & MAV_MODE_FLAG_HIL_ENABLED; +} + +void Vehicle::setHilMode(bool hilMode) +{ + mavlink_message_t msg; + + uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL; + if (hilMode) { + newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, _custom_mode); + sendMessage(msg); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index cffb42170..03f022b49 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -35,6 +35,7 @@ #include "QGCMAVLink.h" #include "MissionItem.h" #include "QmlObjectListModel.h" +#include "MAVLinkProtocol.h" class UAS; class UASInterface; @@ -62,6 +63,14 @@ public: Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) + Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) + + Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) + Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) + Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) + + Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) + Q_INVOKABLE QString getMavIconColor(); //-- System Messages @@ -87,8 +96,6 @@ public: 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) @@ -160,6 +167,17 @@ public: bool homePositionAvailable(void); QGeoCoordinate homePosition(void); + bool armed(void) { return _armed; } + void setArmed(bool armed); + + bool flightModeSetAvailable(void); + QStringList flightModes(void); + QString flightMode(void); + void setFlightMode(const QString& flightMode); + + bool hilMode(void); + void setHilMode(bool hilMode); + typedef enum { MessageNone, MessageNormal, @@ -206,8 +224,6 @@ public: 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; } @@ -229,6 +245,9 @@ signals: void mavlinkMessageReceived(const mavlink_message_t& message); void homePositionAvailableChanged(bool homePositionAvailable); void homePositionChanged(const QGeoCoordinate& homePosition); + void armedChanged(bool armed); + void flightModeChanged(const QString& flightMode); + void hilModeChanged(bool hilMode); /// Used internally to move sendMessage call to main thread void _sendMessageOnThread(mavlink_message_t message); @@ -251,9 +270,7 @@ signals: void batteryVoltageChanged (); void batteryPercentChanged (); void batteryConsumedChanged (); - void systemArmedChanged (); void heartbeatTimeoutChanged(); - void currentModeChanged (); void currentConfigChanged (); void systemPixmapChanged (); void satelliteCountChanged (); @@ -282,9 +299,7 @@ private slots: 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); @@ -301,7 +316,9 @@ private: void _loadSettings(void); void _saveSettings(void); void _startJoystick(bool start); - + void _handleHomePosition(mavlink_message_t& message); + void _handleHeartbeat(mavlink_message_t& message); + bool _isAirplane (); void _addChange (int id); float _oneDecimal (float value); @@ -313,6 +330,7 @@ private: MAV_AUTOPILOT _firmwareType; FirmwarePlugin* _firmwarePlugin; AutoPilotPlugin* _autopilotPlugin; + MAVLinkProtocol* _mavlink; /// List of all links associated with this vehicle. We keep SharedLinkInterface objects /// which are QSharedPointer's in order to maintain reference counts across threads. @@ -357,9 +375,7 @@ private: double _batteryVoltage; double _batteryPercent; double _batteryConsumed; - bool _systemArmed; QString _currentState; - QString _currentMode; QString _systemName; QString _systemPixmap; unsigned int _currentHeartbeatTimeout; @@ -373,6 +389,10 @@ private: MissionManager* _missionManager; QmlObjectListModel _missionItems; + bool _armed; ///< true: vehicle is armed + uint8_t _base_mode; ///< base_mode from HEARTBEAT + uint32_t _custom_mode; ///< custom_mode from HEARTBEAT + static const char* _settingsGroup; static const char* _joystickModeSettingsKey; static const char* _joystickEnabledSettingsKey; diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index 1fba08978..7b9cdba0f 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -482,9 +482,9 @@ QGCView { QGCCheckBox { anchors.verticalCenter: parent.verticalCenter - checked: _activeJoystick.buttonActions[modelData] != -1 + checked: _activeJoystick.buttonActions[modelData] != "" - onClicked: _activeJoystick.setButtonAction(modelData, checked ? buttonActionCombo.currentIndex : -1) + onClicked: _activeJoystick.setButtonAction(modelData, checked ? buttonActionCombo.textAt(buttonActionCombo.currentIndex) : "") } Rectangle { @@ -509,9 +509,9 @@ QGCView { id: buttonActionCombo width: ScreenTools.defaultFontPixelWidth * 20 model: _activeJoystick.actions - currentIndex: _activeJoystick.buttonActions[modelData] - onActivated: _activeJoystick.setButtonAction(modelData, index) + onActivated: _activeJoystick.setButtonAction(modelData, textAt(index)) + Component.onCompleted: currentIndex = find(_activeJoystick.buttonActions[modelData]) } } } // Repeater diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index 61402000b..881cd4bd1 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -63,7 +63,7 @@ Rectangle { function showFirmwarePanel() { if (!ScreenTools.isMobile) { - if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.autopilot.armed) { + if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.armed) { messagePanelText = armedVehicleText panelLoader.sourceComponent = messagePanelComponent } else { @@ -74,7 +74,7 @@ Rectangle { function showJoystickPanel() { - if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.autopilot.armed) { + if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.armed) { messagePanelText = armedVehicleText panelLoader.sourceComponent = messagePanelComponent } else { @@ -89,7 +89,7 @@ Rectangle { function showVehicleComponentPanel(vehicleComponent) { - if (multiVehicleManager.activeVehicle.autopilot.armed) { + if (multiVehicleManager.activeVehicle.armed) { messagePanelText = armedVehicleText panelLoader.sourceComponent = messagePanelComponent } else { diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index a847ad764..e4cd1e30c 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -338,7 +338,6 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) break; default: - qDebug() << "MockLink: Unhandled mavlink message, id:" << msg.msgid; break; } } diff --git a/src/qgcunittest/MavlinkLogTest.cc b/src/qgcunittest/MavlinkLogTest.cc index 36a49486c..68fb543a2 100644 --- a/src/qgcunittest/MavlinkLogTest.cc +++ b/src/qgcunittest/MavlinkLogTest.cc @@ -150,13 +150,10 @@ void MavlinkLogTest::_connectLogWorker(bool arm) QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*))); QCOMPARE(spyVehicle.wait(5000), true); - UAS* uas = MultiVehicleManager::instance()->activeUas(); - QVERIFY(uas); - QDir logSaveDir; if (arm) { - uas->armSystem(); + MultiVehicleManager::instance()->activeVehicle()->setArmed(true); QTest::qWait(1500); // Wait long enough for heartbeat to come through // On Disconnect: We should get a getSaveFileName dialog. diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 7ab7bd7a0..53b3581e2 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -86,8 +86,6 @@ This file is part of the QGROUNDCONTROL project /// The key under which the Main Window settings are saved const char* MAIN_SETTINGS_GROUP = "QGC_MAINWINDOW"; -const char* MainWindow::_uasControlDockWidgetName = "UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"; -const char* MainWindow::_uasListDockWidgetName = "UNMANNED_SYSTEM_LIST_DOCKWIDGET"; const char* MainWindow::_waypointsDockWidgetName = "WAYPOINT_LIST_DOCKWIDGET"; const char* MainWindow::_mavlinkDockWidgetName = "MAVLINK_INSPECTOR_DOCKWIDGET"; const char* MainWindow::_customCommandWidgetName = "CUSTOM_COMMAND_DOCKWIDGET"; @@ -388,8 +386,6 @@ void MainWindow::_buildCommonWidgets(void) }; static const struct DockWidgetInfo rgDockWidgetInfo[] = { - { _uasControlDockWidgetName, "Control", Qt::LeftDockWidgetArea }, - { _uasListDockWidgetName, "Unmanned Systems", Qt::RightDockWidgetArea }, { _waypointsDockWidgetName, "Mission Plan", Qt::BottomDockWidgetArea }, { _mavlinkDockWidgetName, "MAVLink Inspector", Qt::RightDockWidgetArea }, { _customCommandWidgetName, "Custom Command", Qt::RightDockWidgetArea }, @@ -487,11 +483,7 @@ void MainWindow::_createInnerDockWidget(const QString& widgetName) QWidget* widget = NULL; - if (widgetName == _uasControlDockWidgetName) { - widget = new UASControlWidget(this); - } else if (widgetName == _uasListDockWidgetName) { - widget = new UASListWidget(this); - } else if (widgetName == _waypointsDockWidgetName) { + if (widgetName == _waypointsDockWidgetName) { widget = new QGCWaypointListMulti(this); } else if (widgetName == _mavlinkDockWidgetName) { widget = new QGCMAVLinkInspector(MAVLinkProtocol::instance(),this); @@ -880,7 +872,7 @@ void MainWindow::_loadCurrentViewState(void) case VIEW_SIMULATION: _buildSimView(); centerView = _simView; - defaultWidgets = "UNMANNED_SYSTEM_CONTROL_DOCKWIDGET,WAYPOINT_LIST_DOCKWIDGET,PARAMETER_INTERFACE_DOCKWIDGET,PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET"; + defaultWidgets = "WAYPOINT_LIST_DOCKWIDGET,PARAMETER_INTERFACE_DOCKWIDGET,PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET"; break; default: diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index ed76bbc4a..22e56b10f 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -41,11 +41,9 @@ This file is part of the QGROUNDCONTROL project #include "LinkManager.h" #include "LinkInterface.h" #include "UASInterface.h" -#include "UASControlWidget.h" #include "UASInfoWidget.h" #include "WaypointList.h" #include "CameraView.h" -#include "UASListWidget.h" #if (defined QGC_MOUSE_ENABLED_WIN) | (defined QGC_MOUSE_ENABLED_LINUX) #include "Mouse6dofInput.h" #endif // QGC_MOUSE_ENABLED_WIN diff --git a/src/ui/WaypointEditableView.cc b/src/ui/WaypointEditableView.cc index 88dd17c8d..b2ba45fed 100644 --- a/src/ui/WaypointEditableView.cc +++ b/src/ui/WaypointEditableView.cc @@ -13,6 +13,7 @@ #include #include +#include #include #include diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index f75fd16fd..ef1b6da79 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/src/ui/toolbar/MainToolBar.qml @@ -279,24 +279,59 @@ Rectangle { } - Rectangle { - id: mavIcon - width: cellHeight - height: cellHeight - visible: mainToolBar.showMav + QGCButton { + width: ScreenTools.defaultFontPixelWidth * 12 + height: cellHeight + visible: mainToolBar.showMav anchors.verticalCenter: parent.verticalCenter - color: colorBlue - border.color: "#00000000" - border.width: 0 - Image { - source: activeVehicle.systemPixmap - height: cellHeight * 0.75 - fillMode: Image.PreserveAspectFit - anchors.verticalCenter: parent.verticalCenter - anchors.horizontalCenter: parent.horizontalCenter + text: "Vehicle " + activeVehicle.id + + menu: vehicleMenu + + Menu { + id: vehicleMenu + } + + Component { + id: vehicleMenuItemComponent + + MenuItem { + checkable: true + checked: vehicle.active + onTriggered: multiVehicleManager.activeVehicle = vehicle + + property int vehicleId: Number(text.split(" ")[1]) + property var vehicle: multiVehicleManager.getVehicleById(vehicleId) + } + } + + property var vehicleMenuItems: [] + + function updateVehicleMenu() { + // Remove old menu items + for (var i=0; i