diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b6d383f3b7aa395bcdd8105daa90d8e13ca329d2..74e6e2d72896f5959d4f1c150655cd37030e7697 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -426,6 +426,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/SectionTest.h \ src/MissionManager/SimpleMissionItemTest.h \ src/MissionManager/SpeedSectionTest.h \ + src/MissionManager/StructureScanComplexItemTest.h \ src/MissionManager/SurveyMissionItemTest.h \ src/MissionManager/VisualMissionItemTest.h \ src/qgcunittest/FileDialogTest.h \ @@ -462,6 +463,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/SectionTest.cc \ src/MissionManager/SimpleMissionItemTest.cc \ src/MissionManager/SpeedSectionTest.cc \ + src/MissionManager/StructureScanComplexItemTest.cc \ src/MissionManager/SurveyMissionItemTest.cc \ src/MissionManager/VisualMissionItemTest.cc \ src/qgcunittest/FileDialogTest.cc \ diff --git a/src/MissionManager/StructureScan.SettingsGroup.json b/src/MissionManager/StructureScan.SettingsGroup.json index 8439fa69dccd3d144ed7cf526707fc641b4c8c8a..9c3beebabe537b31799f6365c8cee58cd2697ffc 100644 --- a/src/MissionManager/StructureScan.SettingsGroup.json +++ b/src/MissionManager/StructureScan.SettingsGroup.json @@ -1,4 +1,24 @@ [ +{ + "name": "GimbalPitch", + "shortDescription": "Gimbal pitch rotation.", + "type": "double", + "units": "gimbal-degrees", + "min": -90, + "max": 0, + "decimalPlaces": 0, + "defaultValue": 0 +}, +{ + "name": "GimbalYaw", + "shortDescription": "Gimbal yaw rotation.", + "type": "double", + "units": "deg", + "min": -180.0, + "max": 180.0, + "decimalPlaces": 0, + "defaultValue": 90 +}, { "name": "Altitude", "shortDescription": "Altitude for the bottom layer of the structure scan.", diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index c03379bf76642c95b6caeb6e325dbe738a893c3c..b5e9c8b595c5e978d602e5585466dd2e9b87f2da 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -21,9 +21,12 @@ QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") -const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::_altitudeFactName = "Altitude"; const char* StructureScanComplexItem::_layersFactName = "Layers"; +const char* StructureScanComplexItem::_gimbalPitchFactName = "GimbalPitch"; +const char* StructureScanComplexItem::_gimbalYawFactName = "GimbalYaw"; + +const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; const char* StructureScanComplexItem::_jsonYawVehicleToStructureKey = "yawVehicleToStructure"; @@ -44,6 +47,8 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa , _yawVehicleToStructure (false) , _altitudeFact (0, _altitudeFactName, FactMetaData::valueTypeDouble) , _layersFact (0, _layersFactName, FactMetaData::valueTypeUint32) + , _gimbalPitchFact (0, _gimbalPitchFactName, FactMetaData::valueTypeDouble) + , _gimbalYawFact (0, _gimbalYawFactName, FactMetaData::valueTypeDouble) { _editorQml = "qrc:/qml/StructureScanEditor.qml"; @@ -53,18 +58,25 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa _altitudeFact.setMetaData (_metaDataMap[_altitudeFactName]); _layersFact.setMetaData (_metaDataMap[_layersFactName]); + _gimbalPitchFact.setMetaData(_metaDataMap[_gimbalPitchFactName]); + _gimbalYawFact.setMetaData (_metaDataMap[_gimbalYawFactName]); _altitudeFact.setRawValue (_altitudeFact.rawDefaultValue()); _layersFact.setRawValue (_layersFact.rawDefaultValue()); + _gimbalPitchFact.setRawValue(_gimbalPitchFact.rawDefaultValue()); + _gimbalYawFact.setRawValue (_gimbalYawFact.rawDefaultValue()); _altitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); - connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_gimbalYawFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::_setDirty); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); + connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::_setDirty); + connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged); + connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); + connect(this, &StructureScanComplexItem::yawVehicleToStructureChanged, this, &StructureScanComplexItem::_setDirty); connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes); @@ -74,7 +86,8 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); - connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); + connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); + connect(&_cameraCalc, &CameraCalc::cameraSpecTypeChanged, this, &StructureScanComplexItem::_cameraSpecTypeChanged); connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcCameraShots); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); @@ -115,7 +128,7 @@ int StructureScanComplexItem::lastSequenceNumber(void) const { return _sequenceNumber + ((_flightPolygon.count() + 1) * _layersFact.rawValue().toInt()) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex - 1; // Gimbal yaw command + 1; // Gimbal control command } void StructureScanComplexItem::setDirty(bool dirty) @@ -135,6 +148,8 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + saveObject[_gimbalPitchFactName] = _gimbalPitchFact.rawValue().toDouble(); + saveObject[_gimbalYawFactName] = _gimbalYawFact.rawValue().toDouble(); saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble(); saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; saveObject[_layersFactName] = _layersFact.rawValue().toDouble(); @@ -165,6 +180,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, + { _gimbalPitchFactName, QJsonValue::Double, true }, + { _gimbalYawFactName, QJsonValue::Double, true }, { _altitudeFactName, QJsonValue::Double, true }, { _jsonAltitudeRelativeKey, QJsonValue::Bool, false }, { _layersFactName, QJsonValue::Double, true }, @@ -192,6 +209,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen setSequenceNumber(sequenceNumber); + _gimbalPitchFact.setRawValue(complexObject[_gimbalPitchFactName].toDouble()); + _gimbalYawFact.setRawValue (complexObject[_gimbalYawFactName].toDouble()); _altitudeFact.setRawValue (complexObject[_altitudeFactName].toDouble()); _layersFact.setRawValue (complexObject[_layersFactName].toDouble()); _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); @@ -258,9 +277,9 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_MOUNT_CONTROL, MAV_FRAME_MISSION, - 0, // Gimbal pitch + _gimbalPitchFact.rawValue().toDouble(), 0, // Gimbal roll - 90, // Gimbal yaw + _gimbalYawFact.rawValue().toDouble(), 0, 0, 0, // param 4-6 not used MAV_MOUNT_MODE_MAVLINK_TARGETING, true, // autoContinue @@ -433,3 +452,27 @@ void StructureScanComplexItem::_recalcCameraShots(void) int cameraShots = distance / _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); _setCameraShots(cameraShots * _layersFact.rawValue().toInt()); } + +void StructureScanComplexItem::_cameraSpecTypeChanged(CameraCalc::CameraSpecType cameraSpecType) +{ + Q_UNUSED(cameraSpecType); + + _gimbalPitchFact.setCookedValue(0); + _gimbalYawFact.setCookedValue(90); +} + +void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative) +{ + if (altitudeRelative != _altitudeRelative) { + _altitudeRelative = altitudeRelative; + emit altitudeRelativeChanged(altitudeRelative); + } +} + +void StructureScanComplexItem::setYawVehicleToStructure(bool yawVehicleToStructure) +{ + if (yawVehicleToStructure != _yawVehicleToStructure) { + _yawVehicleToStructure = yawVehicleToStructure; + emit yawVehicleToStructureChanged(yawVehicleToStructure); + } +} diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index 1b3374d98e793b5beef7d5d0e3bfbc842cdc9afb..1843edbbee17fd19dc397209f748805f5901df8e 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -27,25 +27,34 @@ class StructureScanComplexItem : public ComplexMissionItem public: StructureScanComplexItem(Vehicle* vehicle, QObject* parent = NULL); - Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* altitude READ altitude CONSTANT) - Q_PROPERTY(Fact* layers READ layers CONSTANT) - Q_PROPERTY(bool altitudeRelative MEMBER _altitudeRelative NOTIFY altitudeRelativeChanged) - Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) - Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) - Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) - Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT) - Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT) - Q_PROPERTY(bool yawVehicleToStructure MEMBER _yawVehicleToStructure NOTIFY yawVehicleToStructureChanged) ///< true: vehicle yaws to point to structure, false: gimbal yaws to point to structure + Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) + Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) + Q_PROPERTY(Fact* gimbalYaw READ gimbalYaw CONSTANT) + Q_PROPERTY(Fact* altitude READ altitude CONSTANT) + Q_PROPERTY(Fact* layers READ layers CONSTANT) + Q_PROPERTY(bool altitudeRelative READ altitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) + Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) + Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) + Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) + Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT) + Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT) + Q_PROPERTY(bool yawVehicleToStructure READ yawVehicleToStructure WRITE setYawVehicleToStructure NOTIFY yawVehicleToStructureChanged) ///< true: vehicle yaws to point to structure, false: gimbal yaws to point to structure CameraCalc* cameraCalc (void) { return &_cameraCalc; } Fact* altitude (void) { return &_altitudeFact; } Fact* layers (void) { return &_layersFact; } - int cameraShots (void) const; - double timeBetweenShots(void); - QGCMapPolygon* structurePolygon(void) { return &_structurePolygon; } - QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } + bool altitudeRelative (void) const { return _altitudeRelative; } + int cameraShots (void) const; + Fact* gimbalPitch (void) { return &_gimbalPitchFact; } + Fact* gimbalYaw (void) { return &_gimbalYawFact; } + double timeBetweenShots (void); + QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; } + QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } + bool yawVehicleToStructure (void) const { return _yawVehicleToStructure; } + + void setAltitudeRelative (bool altitudeRelative); + void setYawVehicleToStructure (bool yawVehicleToStructure); Q_INVOKABLE void rotateEntryPoint(void); @@ -96,13 +105,14 @@ signals: private slots: void _setDirty(void); - void _polygonDirtyChanged(bool dirty); - void _polygonCountChanged(int count); - void _flightPathChanged(void); - void _clearInternal(void); - void _updateCoordinateAltitudes(void); - void _rebuildFlightPolygon(void); - void _recalcCameraShots(void); + void _polygonDirtyChanged (bool dirty); + void _polygonCountChanged (int count); + void _flightPathChanged (void); + void _clearInternal (void); + void _updateCoordinateAltitudes (void); + void _rebuildFlightPolygon (void); + void _recalcCameraShots (void); + void _cameraSpecTypeChanged (CameraCalc::CameraSpecType cameraSpecType); private: void _setExitCoordinate(const QGeoCoordinate& coordinate); @@ -130,13 +140,19 @@ private: Fact _altitudeFact; Fact _layersFact; + Fact _gimbalPitchFact; + Fact _gimbalYawFact; static const char* _altitudeFactName; static const char* _layersFactName; + static const char* _gimbalPitchFactName; + static const char* _gimbalYawFactName; static const char* _jsonCameraCalcKey; static const char* _jsonAltitudeRelativeKey; static const char* _jsonYawVehicleToStructureKey; + + friend class StructureScanComplexItemTest; }; #endif diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..f894e5f6ee2f92f7bf143d6e0cc7187062108469 --- /dev/null +++ b/src/MissionManager/StructureScanComplexItemTest.cc @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "StructureScanComplexItemTest.h" +#include "QGCApplication.h" + +StructureScanComplexItemTest::StructureScanComplexItemTest(void) + : _offlineVehicle(NULL) +{ + _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << + QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); +} + +void StructureScanComplexItemTest::init(void) +{ + UnitTest::init(); + + _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + + _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); + _structureScanItem = new StructureScanComplexItem(_offlineVehicle, this); + _structureScanItem->setDirty(false); + + _multiSpy = new MultiSignalSpy(); + Q_CHECK_PTR(_multiSpy); + QCOMPARE(_multiSpy->init(_structureScanItem, _rgSignals, _cSignals), true); +} + +void StructureScanComplexItemTest::cleanup(void) +{ + delete _structureScanItem; + delete _offlineVehicle; +} + +void StructureScanComplexItemTest::_testDirty(void) +{ + QVERIFY(!_structureScanItem->dirty()); + _structureScanItem->setDirty(false); + QVERIFY(!_structureScanItem->dirty()); + QVERIFY(_multiSpy->checkNoSignals()); + + _structureScanItem->setDirty(true); + QVERIFY(_structureScanItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _multiSpy->clearAllSignals(); + + _structureScanItem->setDirty(false); + QVERIFY(!_structureScanItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(!_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _multiSpy->clearAllSignals(); + + // These facts should set dirty when changed + QList rgFacts; + rgFacts << _structureScanItem->gimbalPitch() << _structureScanItem->gimbalYaw() << _structureScanItem->altitude() << _structureScanItem->layers(); + foreach(Fact* fact, rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_structureScanItem->dirty()); + if (fact->typeIsBool()) { + fact->setRawValue(!fact->rawValue().toBool()); + } else { + fact->setRawValue(fact->rawValue().toDouble() + 1); + } + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _structureScanItem->setDirty(false); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); + + QVERIFY(!_structureScanItem->dirty()); + _structureScanItem->setAltitudeRelative(!_structureScanItem->altitudeRelative()); + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _structureScanItem->setDirty(false); + _multiSpy->clearAllSignals(); + + QVERIFY(!_structureScanItem->dirty()); + _structureScanItem->setYawVehicleToStructure(!_structureScanItem->yawVehicleToStructure()); + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _structureScanItem->setDirty(false); + _multiSpy->clearAllSignals(); +} + +void StructureScanComplexItemTest::_initItem(void) +{ + QGCMapPolygon* mapPolygon = _structureScanItem->structurePolygon(); + + for (int i=0; i<_polyPoints.count(); i++) { + QGeoCoordinate& vertex = _polyPoints[i]; + mapPolygon->appendVertex(vertex); + } + + _structureScanItem->cameraCalc()->setCameraSpecType(CameraCalc::CameraSpecNone); + _structureScanItem->gimbalPitch()->setCookedValue(45); + _structureScanItem->gimbalYaw()->setCookedValue(45); + _structureScanItem->layers()->setCookedValue(2); + _structureScanItem->setDirty(false); + + _validateItem(_structureScanItem); +} + +void StructureScanComplexItemTest::_validateItem(StructureScanComplexItem* item) +{ + QGCMapPolygon* mapPolygon = item->structurePolygon(); + + for (int i=0; i<_polyPoints.count(); i++) { + QGeoCoordinate& expectedVertex = _polyPoints[i]; + QGeoCoordinate actualVertex = mapPolygon->vertexCoordinate(i); + QCOMPARE(expectedVertex, actualVertex); + } + + QCOMPARE((int)item->cameraCalc()->cameraSpecType(), (int)CameraCalc::CameraSpecNone); + QCOMPARE(item->gimbalPitch()->cookedValue().toDouble(), 45.0); + QCOMPARE(item->gimbalYaw()->cookedValue().toDouble(), 45.0); + QCOMPARE(item->layers()->cookedValue().toInt(), 2); + + int seqNum = item->sequenceNumber(); + QCOMPARE(item->lastSequenceNumber(), seqNum + (5 /* 5 waypoints per layer */ * item->layers()->cookedValue().toInt()) + 1 /* gimbal command */); +} + +void StructureScanComplexItemTest::_testSaveLoad(void) +{ + _initItem(); + + QJsonArray items; + _structureScanItem->save(items); + + QString errorString; + StructureScanComplexItem* newItem = new StructureScanComplexItem(_offlineVehicle, this); + QVERIFY(newItem->load(items[0].toObject(), 10, errorString)); + QVERIFY(errorString.isEmpty()); + _validateItem(newItem); + newItem->deleteLater(); +} + +void StructureScanComplexItemTest::_testGimbalAngleUpdate(void) +{ + // This sets the item to CameraCalc::CameraSpecNone and non-standard gimbal angles + _initItem(); + + // Switching to a camera specific setup should set gimbal angles to defaults surface scan + _structureScanItem->cameraCalc()->setCameraSpecType(CameraCalc::CameraSpecCustom); + QCOMPARE(_structureScanItem->gimbalPitch()->cookedValue().toDouble(), 0.0); + QCOMPARE(_structureScanItem->gimbalYaw()->cookedValue().toDouble(), 90.0); +} diff --git a/src/MissionManager/StructureScanComplexItemTest.h b/src/MissionManager/StructureScanComplexItemTest.h new file mode 100644 index 0000000000000000000000000000000000000000..535842422b8ed07449f87c8433e1bef6e2aa9bd0 --- /dev/null +++ b/src/MissionManager/StructureScanComplexItemTest.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" +#include "MultiSignalSpy.h" +#include "StructureScanComplexItem.h" + +class StructureScanComplexItemTest : public UnitTest +{ + Q_OBJECT + +public: + StructureScanComplexItemTest(void); + +protected: + void init(void) final; + void cleanup(void) final; + +private slots: + void _testDirty(void); + void _testSaveLoad(void); + void _testGimbalAngleUpdate(void); + +private: + void _initItem(void); + void _validateItem(StructureScanComplexItem* item); + + enum { + dirtyChangedIndex, + maxSignalIndex + }; + + enum { + dirtyChangedMask = 1 << dirtyChangedIndex + }; + + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; + + Vehicle* _offlineVehicle; + MultiSignalSpy* _multiSpy; + StructureScanComplexItem* _structureScanItem; + QList _polyPoints; +}; diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 0aeeae7e330d9e8381e3dcc487c2c6793d2a5d80..52fb2c9860f41a73232e84e3fd4505280c03695f 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -29,7 +29,6 @@ Rectangle { property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle - function polygonCaptureStarted() { missionItem.clearPolygon() } @@ -62,14 +61,6 @@ Rectangle { anchors.right: parent.right spacing: _margin - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - text: qsTr("WORK IN PROGRESS. CAREFUL!") - wrapMode: Text.WordWrap - color: qgcPal.warningText - } - QGCLabel { anchors.left: parent.left anchors.right: parent.right @@ -95,6 +86,33 @@ Rectangle { sideDistanceLabel: qsTr("Trigger Distance") } + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: ScreenTools.defaultFontPixelWidth / 2 + rowSpacing: 0 + columns: 3 + enabled: missionItem.cameraCalc.cameraSpecType === CameraCalc.CameraSpecNone + + Item { width: 1; height: 1 } + QGCLabel { text: qsTr("Pitch") } + QGCLabel { text: qsTr("Yaw") } + + QGCLabel { + text: qsTr("Gimbal") + Layout.fillWidth: true + } + FactTextField { + fact: missionItem.gimbalPitch + implicitWidth: ScreenTools.defaultFontPixelWidth * 9 + } + + FactTextField { + fact: missionItem.gimbalYaw + implicitWidth: ScreenTools.defaultFontPixelWidth * 9 + } + } + SectionHeader { id: scanHeader text: qsTr("Scan") @@ -155,6 +173,11 @@ Rectangle { } } + Item { + height: ScreenTools.defaultFontPixelHeight / 2 + width: 1 + } + QGCButton { text: qsTr("Rotate entry point") onClicked: missionItem.rotateEntryPoint() diff --git a/src/PlanView/StructureScanMapVisual.qml b/src/PlanView/StructureScanMapVisual.qml index 21f97ce10e81f111048f975f2f9c11a959a5ebc2..333c53b992738b7678c945162f12b564f6c2935e 100644 --- a/src/PlanView/StructureScanMapVisual.qml +++ b/src/PlanView/StructureScanMapVisual.qml @@ -61,9 +61,10 @@ Item { var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */) var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) - // Initial polygon has max width and height of 3000 meters - var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2 - var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2 + // Adjust polygon to max size + var maxSize = 100 + var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), maxSize) / 2 + var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), maxSize) / 2 topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0) topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0) bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180) diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 0fb23b5ecbee2968ab57a1c4eca63c4b6dff32a1..fe67513cc739dcc3e4222173b87b48043dda3be0 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -39,6 +39,7 @@ #include "MissionSettingsTest.h" #include "QGCMapPolygonTest.h" #include "AudioOutputTest.h" +#include "StructureScanComplexItemTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -65,6 +66,7 @@ UT_REGISTER_TEST(PlanMasterControllerTest) UT_REGISTER_TEST(MissionSettingsTest) UT_REGISTER_TEST(QGCMapPolygonTest) UT_REGISTER_TEST(AudioOutputTest) +UT_REGISTER_TEST(StructureScanComplexItemTest) // List of unit test which are currently disabled. // If disabling a new test, include reason in comment.