diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index a69b25831813093008c547a51e3f297f7fe245f3..09441f6e8a3eeccee84ea691b7d41650e4b7a6cb 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -131,7 +131,22 @@ QVariant Fact::value(void) const QString Fact::valueString(void) const { - return _value.toString(); + QString valueString; + + switch (type()) { + case FactMetaData::valueTypeFloat: + qDebug() << name() << value() << decimalPlaces(); + valueString = QString("%1").arg(value().toFloat(), 0, 'g', decimalPlaces()); + break; + case FactMetaData::valueTypeDouble: + valueString = QString("%1").arg(value().toDouble(), 0, 'g', decimalPlaces()); + break; + default: + valueString = value().toString(); + break; + } + + return valueString; } QVariant Fact::defaultValue(void) const @@ -222,6 +237,16 @@ bool Fact::maxIsDefaultForType(void) const } } +int Fact::decimalPlaces(void) const +{ + if (_metaData) { + return _metaData->decimalPlaces(); + } else { + qWarning() << "Meta data pointer missing"; + return FactMetaData::defaultDecimalPlaces; + } +} + QString Fact::group(void) const { if (_metaData) { diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index da8a85efebd290f8e8ffc5f1ea16ce1085354358..c946b198362dad272bd2d15903a657eddfdae593 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -46,22 +46,23 @@ public: const Fact& operator=(const Fact& other); - Q_PROPERTY(int componentId READ componentId CONSTANT) - Q_PROPERTY(QString name READ name CONSTANT) - Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true) - Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) - Q_PROPERTY(QString units READ units CONSTANT) - Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT) - Q_PROPERTY(bool defaultValueAvailable READ defaultValueAvailable CONSTANT) - Q_PROPERTY(bool valueEqualsDefault READ valueEqualsDefault NOTIFY valueChanged) - Q_PROPERTY(FactMetaData::ValueType_t type READ type CONSTANT) - Q_PROPERTY(QString shortDescription READ shortDescription CONSTANT) - Q_PROPERTY(QString longDescription READ longDescription CONSTANT) - Q_PROPERTY(QVariant min READ min CONSTANT) - Q_PROPERTY(bool minIsDefaultForType READ minIsDefaultForType CONSTANT) - Q_PROPERTY(QVariant max READ max CONSTANT) - Q_PROPERTY(bool maxIsDefaultForType READ maxIsDefaultForType CONSTANT) - Q_PROPERTY(QString group READ group CONSTANT) + Q_PROPERTY(int componentId READ componentId CONSTANT) + Q_PROPERTY(QString group READ group CONSTANT) + Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true) + Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) + Q_PROPERTY(QString units READ units CONSTANT) + Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT) + Q_PROPERTY(bool defaultValueAvailable READ defaultValueAvailable CONSTANT) + Q_PROPERTY(bool valueEqualsDefault READ valueEqualsDefault NOTIFY valueChanged) + Q_PROPERTY(FactMetaData::ValueType_t type READ type CONSTANT) + Q_PROPERTY(QString shortDescription READ shortDescription CONSTANT) + Q_PROPERTY(QString longDescription READ longDescription CONSTANT) + Q_PROPERTY(QVariant min READ min CONSTANT) + Q_PROPERTY(bool minIsDefaultForType READ minIsDefaultForType CONSTANT) + Q_PROPERTY(QVariant max READ max CONSTANT) + Q_PROPERTY(bool maxIsDefaultForType READ maxIsDefaultForType CONSTANT) + Q_PROPERTY(int decimalPlaces READ decimalPlaces CONSTANT) /// Convert and validate value /// @param convertOnly true: validate type conversion only, false: validate against meta data as well @@ -86,6 +87,7 @@ public: QVariant max(void) const; bool maxIsDefaultForType(void) const; QString group(void) const; + int decimalPlaces(void) const; /// Sets and sends new value to vehicle even if value is the same void forceSetValue(const QVariant& value); diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index 7360e65a7912a0417476a8d8dbc59c8bf2d7704a..942d4e35af262a976c8c94ff2d0f4d6557b80e18 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -32,30 +32,32 @@ #include -FactMetaData::FactMetaData(QObject* parent) : - QObject(parent), - _group("*Default Group"), - _type(valueTypeInt32), - _defaultValue(0), - _defaultValueAvailable(false), - _min(_minForType()), - _max(_maxForType()), - _minIsDefaultForType(true), - _maxIsDefaultForType(true) +FactMetaData::FactMetaData(QObject* parent) + : QObject(parent) + , _group("*Default Group") + , _type(valueTypeInt32) + , _defaultValue(0) + , _defaultValueAvailable(false) + , _min(_minForType()) + , _max(_maxForType()) + , _minIsDefaultForType(true) + , _maxIsDefaultForType(true) + , _decimalPlaces(defaultDecimalPlaces) { } -FactMetaData::FactMetaData(ValueType_t type, QObject* parent) : - QObject(parent), - _group("*Default Group"), - _type(type), - _defaultValue(0), - _defaultValueAvailable(false), - _min(_minForType()), - _max(_maxForType()), - _minIsDefaultForType(true), - _maxIsDefaultForType(true) +FactMetaData::FactMetaData(ValueType_t type, QObject* parent) + : QObject(parent) + , _group("*Default Group") + , _type(type) + , _defaultValue(0) + , _defaultValueAvailable(false) + , _min(_minForType()) + , _max(_maxForType()) + , _minIsDefaultForType(true) + , _maxIsDefaultForType(true) + , _decimalPlaces(defaultDecimalPlaces) { } @@ -76,6 +78,7 @@ const FactMetaData& FactMetaData::operator=(const FactMetaData& other) _max = other._max; _minIsDefaultForType = other._minIsDefaultForType; _maxIsDefaultForType = other._maxIsDefaultForType; + _decimalPlaces = other._decimalPlaces; return *this; } diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 49c72d8784332c2a3e4ad27ac79f2af15a7cbe86..03c78d2f87642f7a213de8d455e6bc61198d6d1e 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -59,18 +59,19 @@ public: const FactMetaData& operator=(const FactMetaData& other); // Property accessors - QString name(void) const { return _name; } - QString group(void) const { return _group; } - ValueType_t type(void) const { return _type; } + QString name(void) const { return _name; } + QString group(void) const { return _group; } + ValueType_t type(void) const { return _type; } QVariant defaultValue(void) const; - bool defaultValueAvailable(void) const { return _defaultValueAvailable; } - QString shortDescription(void) const { return _shortDescription; } - QString longDescription(void) const { return _longDescription;} - QString units(void) const { return _units; } - QVariant min(void) const { return _min; } - QVariant max(void) const { return _max; } - bool minIsDefaultForType(void) const { return _minIsDefaultForType; } - bool maxIsDefaultForType(void) const { return _maxIsDefaultForType; } + bool defaultValueAvailable(void) const { return _defaultValueAvailable; } + QString shortDescription(void) const { return _shortDescription; } + QString longDescription(void) const { return _longDescription;} + QString units(void) const { return _units; } + QVariant min(void) const { return _min; } + QVariant max(void) const { return _max; } + bool minIsDefaultForType(void) const { return _minIsDefaultForType; } + bool maxIsDefaultForType(void) const { return _maxIsDefaultForType; } + int decimalPlaces(void) const { return _decimalPlaces; } // Property setters void setName(const QString& name) { _name = name; } @@ -81,6 +82,7 @@ public: void setUnits(const QString& units) { _units = units; } void setMin(const QVariant& max); void setMax(const QVariant& max); + void setDecimalPlaces(int decimalPlaces) { _decimalPlaces = decimalPlaces; } /// Converts the specified value, validating against meta data /// @param value Value to convert, can be string @@ -90,6 +92,8 @@ public: /// @returns false: Convert failed, errorString set bool convertAndValidate(const QVariant& value, bool convertOnly, QVariant& typedValue, QString& errorString); + static const int defaultDecimalPlaces = 3; + private: QVariant _minForType(void) const; QVariant _maxForType(void) const; @@ -106,6 +110,7 @@ private: QVariant _max; bool _minIsDefaultForType; bool _maxIsDefaultForType; + int _decimalPlaces; }; #endif diff --git a/src/FirmwarePlugin/PX4/PX4ParameterLoader.cc b/src/FirmwarePlugin/PX4/PX4ParameterLoader.cc index aea72c11f4e4ddd93139dd04a864c7fc6c662b59..c7d989dd263e2fa1ac238860336992dd9c3caeb8 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterLoader.cc +++ b/src/FirmwarePlugin/PX4/PX4ParameterLoader.cc @@ -299,6 +299,19 @@ void PX4ParameterLoader::loadParameterFactMetaData(void) qCDebug(PX4ParameterLoaderLog) << "Unit:" << text; metaData->setUnits(text); + } else if (elementName == "decimal") { + Q_ASSERT(metaData); + QString text = xml.readElementText(); + qCDebug(PX4ParameterLoaderLog) << "Decimal:" << text; + + bool convertOk; + QVariant varDecimals = QVariant(text).toUInt(&convertOk); + if (convertOk) { + metaData->setDecimalPlaces(varDecimals.toInt()); + } else { + qCWarning(PX4ParameterLoaderLog) << "Invalid decimals value, name:" << metaData->name() << " type:" << metaData->type() << " decimals:" << text << " error: invalid number"; + } + } else { qDebug() << "Unknown element in XML: " << elementName; } diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 83f00d560089e0e560767450b31c027a87f11617..a7e92f5b428091a442be0480676709cbfa2d6860 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -322,8 +322,6 @@ QGCView { // Move to the new position editorMap.latitude = object.coordinate.latitude editorMap.longitude = object.coordinate.longitude - } else { - itemDragger.clearItem() } } } diff --git a/src/MissionItem.cc b/src/MissionItem.cc index 0d0943eaa7722a1f4d397f8671897e0a1b3e782c..4e3f9d147f2ea994e310c035fdb56604db0257e2 100644 --- a/src/MissionItem.cc +++ b/src/MissionItem.cc @@ -20,15 +20,6 @@ This file is part of the QGROUNDCONTROL project ======================================================================*/ -/** - * @file - * @brief MissionItem class - * - * @author Benjamin Knecht - * @author Petri Tanskanen - * - */ - #include #include @@ -115,10 +106,12 @@ MissionItem::MissionItem(QObject* parent, FactMetaData* latitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _latitudeFact); latitudeMetaData->setUnits("deg"); - + latitudeMetaData->setDecimalPlaces(7); + FactMetaData* longitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _longitudeFact); longitudeMetaData->setUnits("deg"); - + longitudeMetaData->setDecimalPlaces(7); + FactMetaData* altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _altitudeFact); altitudeMetaData->setUnits("meters"); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index bad302403de79d96753a11d9180de2442f971baf..01f9be44d28268ca1be793e7571a5e09cfcabb64 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -66,21 +66,23 @@ void MissionController::start(bool editMode) connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged); - _setupMissionItems(true /* fromVehicle */, true /* force */); + _setupMissionItems(true /* loadFromVehicle */, true /* forceLoad */); + _setupActiveVehicle(multiVehicleMgr->activeVehicle(), true /* forceLoadFromVehicle */); } void MissionController::_newMissionItemsAvailableFromVehicle(void) { qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle"; - _setupMissionItems(true /* fromVehicle */, false /* force */); + _setupMissionItems(true /* loadFromVehicle */, false /* forceLoad */); } -/// @param fromVehicle true: load items from vehicle -/// @param force true: disregard any flags which may prevent load -void MissionController::_setupMissionItems(bool fromVehicle, bool force) +/// @param loadFromVehicle true: load items from vehicle +/// @param forceLoad true: disregard any flags which may prevent load +void MissionController::_setupMissionItems(bool loadFromVehicle, bool forceLoad) { - qCDebug(MissionControllerLog) << "_setupMissionItems fromVehicle:force:_editMode:_firstItemsFromVehicle" << fromVehicle << force << _editMode << _firstItemsFromVehicle; + qCDebug(MissionControllerLog) << "_setupMissionItems loadFromVehicle:forceLoad:_editMode:_firstItemsFromVehicle" + << loadFromVehicle << forceLoad << _editMode << _firstItemsFromVehicle; MissionManager* missionManager = NULL; if (_activeVehicle) { @@ -89,8 +91,8 @@ void MissionController::_setupMissionItems(bool fromVehicle, bool force) qCDebug(MissionControllerLog) << "running offline"; } - if (!force) { - if (_editMode && fromVehicle) { + if (!forceLoad) { + if (_editMode && loadFromVehicle) { if (_firstItemsFromVehicle) { if (missionManager) { if (missionManager->inProgress()) { @@ -126,7 +128,7 @@ void MissionController::_setupMissionItems(bool fromVehicle, bool force) _missionItems->deleteLater(); } - if (!missionManager || !fromVehicle || missionManager->inProgress()) { + if (!missionManager || !loadFromVehicle || missionManager->inProgress()) { _canEdit = true; _missionItems = new QmlObjectListModel(this); qCDebug(MissionControllerLog) << "creating empty set"; @@ -440,10 +442,22 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle) _activeVehicle = NULL; // When the active vehicle goes away we toss the editor items - _setupMissionItems(false /* fromVehicle */, true /* force */); + _setupMissionItems(false /* loadFromVehicle */, true /* forceLoad */); _activeVehicleHomePositionAvailableChanged(false); } + _setupActiveVehicle(activeVehicle, false /* forceLoadFromVehicle */); +} + +void MissionController::_setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle) +{ + qCDebug(MissionControllerLog) << "_setupActiveVehicle activeVehicle:forceLoadFromVehicle" + << activeVehicle << forceLoadFromVehicle; + + if (_activeVehicle) { + qCWarning(MissionControllerLog) << "_activeVehicle != NULL"; + } + _activeVehicle = activeVehicle; if (_activeVehicle) { @@ -455,7 +469,7 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle) connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); _firstItemsFromVehicle = true; - _setupMissionItems(true /* fromVehicle */, false /* force */); + _setupMissionItems(true /* fromVehicle */, forceLoadFromVehicle); _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); @@ -464,20 +478,15 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle) void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) { - MissionItem* homeItem = qobject_cast(_missionItems->get(0)); - - if (homePositionAvailable) { - homeItem->setCoordinate(_liveHomePosition); - } - homeItem->setHomePositionValid(homePositionAvailable); - _liveHomePositionAvailable = homePositionAvailable; + qobject_cast(_missionItems->get(0))->setHomePositionValid(homePositionAvailable); emit liveHomePositionAvailableChanged(_liveHomePositionAvailable); } void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) { _liveHomePosition = homePosition; + qobject_cast(_missionItems->get(0))->setCoordinate(_liveHomePosition); emit liveHomePositionChanged(_liveHomePosition); } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 50078c912f6d3e201435e5a117bc4d94fe846220..2913268ded8b3f6135c506bfbf104d7303f3b30f 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -94,7 +94,8 @@ private: void _initMissionItem(MissionItem* item); void _deinitMissionItem(MissionItem* item); void _autoSyncSend(void); - void _setupMissionItems(bool fromVehicle, bool force); + void _setupMissionItems(bool loadFromVehicle, bool forceLoad); + void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); private: bool _editMode; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 1991995eaf436af9d14e187aa32c02ea457359a5..f97a7ed5052b7f2c557bb9968b0019ad5fcc6378 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -202,17 +202,32 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes void Vehicle::_handleHomePosition(mavlink_message_t& message) { + bool emitHomePositionChanged = false; + bool emitHomePositionAvailableChanged = false; + 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); + + QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0, + homePos.longitude / 10000000.0, + homePos.altitude / 1000.0); + if (newHomePosition != _homePosition) { + emitHomePositionChanged = true; + _homePosition = newHomePosition; + } + + if (!_homePositionAvailable) { + emitHomePositionAvailableChanged = true; + } _homePositionAvailable = true; - - emit homePositionChanged(_homePosition); - emit homePositionAvailableChanged(true); + + if (emitHomePositionChanged) { + emit homePositionChanged(_homePosition); + } + if (emitHomePositionAvailableChanged) { + emit homePositionAvailableChanged(true); + } } void Vehicle::_handleHeartbeat(mavlink_message_t& message)