From 59ddfdff6a9f8cfdd2fd58ebf4c5a1ff442165be Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 2 Nov 2017 14:41:07 -0700 Subject: [PATCH] Support for vehicle yaw to structure --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 3 +- .../CameraCalc.FactMetaData.json | 2 +- .../StructureScanComplexItem.cc | 88 ++++++++++++++----- src/MissionManager/StructureScanComplexItem.h | 5 ++ src/PlanView/StructureScanEditor.qml | 25 +++++- 5 files changed, 99 insertions(+), 24 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 6908eeca0..135969f9b 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -277,7 +277,8 @@ QList PX4FirmwarePlugin::supportedMissionCommands(void) << MAV_CMD_DO_MOUNT_CONTROL << MAV_CMD_SET_CAMERA_MODE << MAV_CMD_IMAGE_START_CAPTURE << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE - << MAV_CMD_NAV_DELAY; + << MAV_CMD_NAV_DELAY + << MAV_CMD_CONDITION_YAW; return list; } diff --git a/src/MissionManager/CameraCalc.FactMetaData.json b/src/MissionManager/CameraCalc.FactMetaData.json index 2778a493a..7df18d099 100644 --- a/src/MissionManager/CameraCalc.FactMetaData.json +++ b/src/MissionManager/CameraCalc.FactMetaData.json @@ -12,7 +12,7 @@ "min": 0.1, "units": "m", "decimalPlaces": 2, - "defaultValue": 100.0 + "defaultValue": 10.0 }, { "name": "ImageDensity", diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 70528ad12..947af8533 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -21,11 +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::_jsonCameraCalcKey = "CameraCalc"; -const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; +const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; +const char* StructureScanComplexItem::_altitudeFactName = "Altitude"; +const char* StructureScanComplexItem::_layersFactName = "Layers"; +const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; +const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; +const char* StructureScanComplexItem::_jsonYawVehicleToStructureKey = "yawVehicleToStructure"; QMap StructureScanComplexItem::_metaDataMap; @@ -40,6 +41,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa , _cameraShots (0) , _cameraMinTriggerInterval (0) , _cameraCalc (vehicle) + , _yawVehicleToStructure (true) , _altitudeFact (0, _altitudeFactName, FactMetaData::valueTypeDouble) , _layersFact (0, _layersFactName, FactMetaData::valueTypeUint32) { @@ -73,6 +75,10 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); + + connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcCameraShots); + connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); + connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); } void StructureScanComplexItem::_setScanDistance(double scanDistance) @@ -129,9 +135,10 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble(); - saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; - saveObject[_layersFactName] = _layersFact.rawValue().toDouble(); + saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble(); + saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; + saveObject[_layersFactName] = _layersFact.rawValue().toDouble(); + saveObject[_jsonYawVehicleToStructureKey] = _yawVehicleToStructure; QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); @@ -162,6 +169,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { _jsonAltitudeRelativeKey, QJsonValue::Bool, false }, { _layersFactName, QJsonValue::Double, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, + { _jsonYawVehicleToStructureKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; @@ -187,6 +195,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen _altitudeFact.setRawValue (complexObject[_altitudeFactName].toDouble()); _layersFact.setRawValue (complexObject[_layersFactName].toDouble()); _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); + _yawVehicleToStructure = complexObject[_jsonYawVehicleToStructureKey].toBool(true); if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) { return false; @@ -231,19 +240,35 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO { int seqNum = _sequenceNumber; double baseAltitude = _altitudeFact.rawValue().toDouble(); + MissionItem* item; - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_MOUNT_CONTROL, - MAV_FRAME_MISSION, - 0, // Gimbal pitch - 0, // Gimbal roll - 90, // Gimbal yaw - 0, 0, 0, // param 4-6 not used - MAV_MOUNT_MODE_MAVLINK_TARGETING, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + if (_yawVehicleToStructure) { + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_CONDITION_YAW, + MAV_FRAME_MISSION, + 90.0, // Target angle + 0, // Use default turn rate + 1, // Clockwise turn + 0, // Absolute angle specified + 0, 0, 0, // param 5-7 not used + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } else { + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_MOUNT_CONTROL, + MAV_FRAME_MISSION, + 0, // Gimbal pitch + 0, // Gimbal roll + 90, // Gimbal yaw + 0, 0, 0, // param 4-6 not used + MAV_MOUNT_MODE_MAVLINK_TARGETING, + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { bool addTriggerStart = true; @@ -386,3 +411,26 @@ void StructureScanComplexItem::_rebuildFlightPolygon(void) _flightPolygon = _structurePolygon; _flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble()); } + +void StructureScanComplexItem::_recalcCameraShots(void) +{ + if (_flightPolygon.count() < 3) { + _setCameraShots(0); + return; + } + + // Determine the distance for each polygon traverse + double distance = 0; + for (int i=0; i<_flightPolygon.count(); i++) { + QGeoCoordinate coord1 = _flightPolygon.vertexCoordinate(i); + QGeoCoordinate coord2 = _flightPolygon.vertexCoordinate(i + 1 == _flightPolygon.count() ? 0 : i + 1); + distance += coord1.distanceTo(coord2); + } + if (distance == 0.0) { + _setCameraShots(0); + return; + } + + int cameraShots = distance / _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); + _setCameraShots(cameraShots * _layersFact.rawValue().toInt()); +} diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index f06d5090c..1b3374d98 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -36,6 +36,7 @@ public: 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 CameraCalc* cameraCalc (void) { return &_cameraCalc; } Fact* altitude (void) { return &_altitudeFact; } @@ -91,6 +92,7 @@ signals: void timeBetweenShotsChanged (void); void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval); void altitudeRelativeChanged (bool altitudeRelative); + void yawVehicleToStructureChanged (bool yawVehicleToStructure); private slots: void _setDirty(void); @@ -100,6 +102,7 @@ private slots: void _clearInternal(void); void _updateCoordinateAltitudes(void); void _rebuildFlightPolygon(void); + void _recalcCameraShots(void); private: void _setExitCoordinate(const QGeoCoordinate& coordinate); @@ -121,6 +124,7 @@ private: double _cameraMinTriggerInterval; double _cruiseSpeed; CameraCalc _cameraCalc; + bool _yawVehicleToStructure; static QMap _metaDataMap; @@ -132,6 +136,7 @@ private: static const char* _jsonCameraCalcKey; static const char* _jsonAltitudeRelativeKey; + static const char* _jsonYawVehicleToStructureKey; }; #endif diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 6a27f88fb..6c8c0e5eb 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -49,6 +49,11 @@ Rectangle { QGCPalette { id: qgcPal; colorGroupEnabled: true } + ExclusiveGroup { + id: yawRadiosGroup + onCurrentChanged: missionItem.yawVehicleToStructure = yawVehicleRadio.checked + } + Column { id: editorColumn anchors.margins: _margin @@ -129,8 +134,24 @@ Rectangle { } QGCLabel { text: qsTr("Point camera to structure using:") } - QGCRadioButton { text: qsTr("Vehicle yaw"); enabled: false } - QGCRadioButton { text: qsTr("Gimbal yaw"); checked: true; enabled: false } + + RowLayout { + spacing: _margin + + QGCRadioButton { + id: yawVehicleRadio + text: qsTr("Vehicle yaw") + exclusiveGroup: yawRadiosGroup + checked: !!missionItem.yawVehicleToStructure + } + + QGCRadioButton + { + text: qsTr("Gimbal yaw") + exclusiveGroup: yawRadiosGroup + checked: !missionItem.yawVehicleToStructure + } + } QGCButton { text: qsTr("Rotate entry point") -- 2.22.0