diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 68869da6575d4ca61b92e9081b7c81587f157ed6..cd003f27415dcb7abd94867fd5c44cda2fc3bdf5 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 68869da6575d4ca61b92e9081b7c81587f157ed6 +Subproject commit cd003f27415dcb7abd94867fd5c44cda2fc3bdf5 diff --git a/src/AnalyzeView/MAVLinkInspectorController.cc b/src/AnalyzeView/MAVLinkInspectorController.cc index f064a478493efd8a88d930d148cafefaed7c3547..0c418843665b29741a0e3e226000a8a23c5117b3 100644 --- a/src/AnalyzeView/MAVLinkInspectorController.cc +++ b/src/AnalyzeView/MAVLinkInspectorController.cc @@ -324,10 +324,26 @@ QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message) emit m->indexChanged(); } } + _checkCompID(message); } emit messagesChanged(); } +//----------------------------------------------------------------------------- +void +QGCMAVLinkVehicle::_checkCompID(QGCMAVLinkMessage* message) +{ + if(_compIDsStr.isEmpty()) { + _compIDsStr << tr("All"); + } + if(!_compIDs.contains(static_cast(message->cid()))) { + int cid = static_cast(message->cid()); + _compIDs.append(cid); + _compIDsStr << QString::number(cid); + emit compIDsChanged(); + } +} + //----------------------------------------------------------------------------- MAVLinkInspectorController::MAVLinkInspectorController() { @@ -430,9 +446,8 @@ MAVLinkInspectorController::_vehicleRemoved(Vehicle* vehicle) //----------------------------------------------------------------------------- void -MAVLinkInspectorController::_receiveMessage(LinkInterface* link, mavlink_message_t message) +MAVLinkInspectorController::_receiveMessage(LinkInterface*, mavlink_message_t message) { - Q_UNUSED(link); QGCMAVLinkMessage* m = nullptr; QGCMAVLinkVehicle* v = _findVehicle(message.sysid); if(!v) { @@ -453,7 +468,6 @@ MAVLinkInspectorController::_receiveMessage(LinkInterface* link, mavlink_message } else { m->update(&message); } - } //----------------------------------------------------------------------------- diff --git a/src/AnalyzeView/MAVLinkInspectorController.h b/src/AnalyzeView/MAVLinkInspectorController.h index 1969a2ccd1addd6a8467b08549fd7aa08d49a639..e1dfa310393af1f9396f0fb0ffcf7f85ab419027 100644 --- a/src/AnalyzeView/MAVLinkInspectorController.h +++ b/src/AnalyzeView/MAVLinkInspectorController.h @@ -87,22 +87,32 @@ class QGCMAVLinkVehicle : public QObject { Q_OBJECT Q_PROPERTY(quint8 id READ id CONSTANT) Q_PROPERTY(QmlObjectListModel* messages READ messages NOTIFY messagesChanged) + Q_PROPERTY(QList compIDs READ compIDs NOTIFY compIDsChanged) + Q_PROPERTY(QStringList compIDsStr READ compIDsStr NOTIFY compIDsChanged) public: QGCMAVLinkVehicle(QObject* parent, quint8 id); quint8 id () { return _id; } QmlObjectListModel* messages () { return &_messages; } + QList compIDs () { return _compIDs; } + QStringList compIDsStr () { return _compIDsStr; } QGCMAVLinkMessage* findMessage (uint32_t id, uint8_t cid); void append (QGCMAVLinkMessage* message); signals: void messagesChanged (); + void compIDsChanged (); + +private: + void _checkCompID (QGCMAVLinkMessage *message); private: quint8 _id; - QmlObjectListModel _messages; //-- List of QGCMAVLinkMessage + QList _compIDs; + QStringList _compIDsStr; + QmlObjectListModel _messages; //-- List of QGCMAVLinkMessage }; //----------------------------------------------------------------------------- diff --git a/src/AnalyzeView/MAVLinkInspectorPage.qml b/src/AnalyzeView/MAVLinkInspectorPage.qml index 8f9a7c6d60c27afe6e95b765d705c2cd437117e6..c43dabafa39069e86eccc629f45c7afd2c94e595 100644 --- a/src/AnalyzeView/MAVLinkInspectorPage.qml +++ b/src/AnalyzeView/MAVLinkInspectorPage.qml @@ -22,9 +22,11 @@ Item { anchors.fill: parent anchors.margins: ScreenTools.defaultFontPixelWidth - property var curVehicle: controller ? controller.activeVehicle : null - property int curMessageIndex: 0 - property var curMessage: curVehicle && curVehicle.messages.count ? curVehicle.messages.get(curMessageIndex) : null + property var curVehicle: controller ? controller.activeVehicle : null + property int curMessageIndex:0 + property var curMessage: curVehicle && curVehicle.messages.count ? curVehicle.messages.get(curMessageIndex) : null + property int curCompID: 0 + property bool selectionValid: false MAVLinkInspectorController { id: controller @@ -35,11 +37,36 @@ Item { } //-- Header - QGCLabel { + RowLayout { id: header - text: qsTr("Inspect real time MAVLink messages.") anchors.top: parent.top anchors.left: parent.left + anchors.right: parent.right + QGCLabel { + text: qsTr("Inspect real time MAVLink messages.") + } + RowLayout { + Layout.alignment: Qt.AlignRight + visible: curVehicle ? curVehicle.compIDsStr.length > 2 : false + QGCLabel { + text: qsTr("Component ID:") + } + QGCComboBox { + id: cidCombo + model: curVehicle ? curVehicle.compIDsStr : [] + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 + currentIndex: 0 + onActivated: { + if(curVehicle && curVehicle.compIDsStr.length > 1) { + selectionValid = false + if(index < 1) + curCompID = 0 + else + curCompID = curVehicle.compIDs[index - 1] + } + } + } + } } //-- Messages (Buttons) @@ -59,10 +86,15 @@ Item { model: curVehicle ? curVehicle.messages : [] delegate: MAVLinkMessageButton { text: object.name + compID: object.cid checked: curMessageIndex === index messageHz: object.messageHz - onClicked: curMessageIndex = index - Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 36 + visible: curCompID === 0 || curCompID === compID + onClicked: { + selectionValid = true + curMessageIndex = index + } + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 40 } } } @@ -70,7 +102,7 @@ Item { //-- Message Data QGCFlickable { id: messageGrid - visible: curMessage !== null + visible: curMessage !== null && selectionValid anchors.top: buttonGrid.top anchors.bottom: parent.bottom anchors.left: buttonGrid.right diff --git a/src/AutoPilotPlugins/CMakeLists.txt b/src/AutoPilotPlugins/CMakeLists.txt index 2a3776e928a7147d0b5c67c56586e350a999a9cb..3094dba189b5f437f77490d3ea533a1fbe27be47 100644 --- a/src/AutoPilotPlugins/CMakeLists.txt +++ b/src/AutoPilotPlugins/CMakeLists.txt @@ -11,6 +11,8 @@ add_library(AutoPilotPlugins APM/APMCompassCal.cc APM/APMFlightModesComponent.cc APM/APMFlightModesComponentController.cc + APM/APMFollowComponent.cc + APM/APMFollowComponentController.cc APM/APMHeliComponent.cc APM/APMLightsComponent.cc APM/APMMotorComponent.cc diff --git a/src/AutoPilotPlugins/PX4/SafetyComponent.qml b/src/AutoPilotPlugins/PX4/SafetyComponent.qml index 06e977c7fd97e6e057e0e87d896cb66a3b62bc89..40a0630293bc2cd0185867e697990ea1b83cec37 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponent.qml +++ b/src/AutoPilotPlugins/PX4/SafetyComponent.qml @@ -52,7 +52,7 @@ SetupPage { property Fact _rcLossAction: controller.getParameterFact(-1, "NAV_RCL_ACT") property Fact _dlLossAction: controller.getParameterFact(-1, "NAV_DLL_ACT") property Fact _disarmLandDelay: controller.getParameterFact(-1, "COM_DISARM_LAND") - property Fact _collisionPrevention: controller.getParameterFact(-1, "MPC_COL_PREV_D") + property Fact _collisionPrevention: controller.getParameterFact(-1, "CP_DIST") property Fact _objectAvoidance: controller.getParameterFact(-1, "COM_OBS_AVOID") property Fact _landSpeedMC: controller.getParameterFact(-1, "MPC_LAND_SPEED", false) property bool _hitlAvailable: controller.parameterExists(-1, hitlParam) diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc index 03ec7a339964428cede8ed1750b723eb3c47e5d1..30d474e26f9a1c9e3653bc883a362cf7cf577583 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc @@ -16,50 +16,58 @@ APMPlaneMode::APMPlaneMode(uint32_t mode, bool settable) : APMCustomMode(mode, settable) { setEnumToStringMapping({ - {MANUAL, "Manual"}, - {CIRCLE, "Circle"}, - {STABILIZE, "Stabilize"}, - {TRAINING, "Training"}, - {ACRO, "Acro"}, - {FLY_BY_WIRE_A, "FBW A"}, - {FLY_BY_WIRE_B, "FBW B"}, - {CRUISE, "Cruise"}, - {AUTOTUNE, "Autotune"}, - {AUTO, "Auto"}, - {RTL, "RTL"}, - {LOITER, "Loiter"}, - {GUIDED, "Guided"}, - {INITIALIZING, "Initializing"}, - {QSTABILIZE, "QuadPlane Stabilize"}, - {QHOVER, "QuadPlane Hover"}, - {QLOITER, "QuadPlane Loiter"}, - {QLAND, "QuadPlane Land"}, - {QRTL, "QuadPlane RTL"}, + { MANUAL, "Manual" }, + { CIRCLE, "Circle" }, + { STABILIZE, "Stabilize" }, + { TRAINING, "Training" }, + { ACRO, "Acro" }, + { FLY_BY_WIRE_A, "FBW A" }, + { FLY_BY_WIRE_B, "FBW B" }, + { CRUISE, "Cruise" }, + { AUTOTUNE, "Autotune" }, + { AUTO, "Auto" }, + { RTL, "RTL" }, + { LOITER, "Loiter" }, + { TAKEOFF, "Takeoff" }, + { AVOID_ADSB, "Avoid ADSB" }, + { GUIDED, "Guided" }, + { INITIALIZING, "Initializing" }, + { QSTABILIZE, "QuadPlane Stabilize" }, + { QHOVER, "QuadPlane Hover" }, + { QLOITER, "QuadPlane Loiter" }, + { QLAND, "QuadPlane Land" }, + { QRTL, "QuadPlane RTL" }, + { QAUTOTUNE, "QuadPlane AutoTune" }, + { QACRO, "QuadPlane Acro" }, }); } ArduPlaneFirmwarePlugin::ArduPlaneFirmwarePlugin(void) { setSupportedModes({ - APMPlaneMode(APMPlaneMode::MANUAL ,true), - APMPlaneMode(APMPlaneMode::CIRCLE ,true), - APMPlaneMode(APMPlaneMode::STABILIZE ,true), - APMPlaneMode(APMPlaneMode::TRAINING ,true), - APMPlaneMode(APMPlaneMode::ACRO ,true), - APMPlaneMode(APMPlaneMode::FLY_BY_WIRE_A ,true), - APMPlaneMode(APMPlaneMode::FLY_BY_WIRE_B ,true), - APMPlaneMode(APMPlaneMode::CRUISE ,true), - APMPlaneMode(APMPlaneMode::AUTOTUNE ,true), - APMPlaneMode(APMPlaneMode::AUTO ,true), - APMPlaneMode(APMPlaneMode::RTL ,true), - APMPlaneMode(APMPlaneMode::LOITER ,true), - APMPlaneMode(APMPlaneMode::GUIDED ,true), - APMPlaneMode(APMPlaneMode::INITIALIZING ,false), - APMPlaneMode(APMPlaneMode::QSTABILIZE ,true), - APMPlaneMode(APMPlaneMode::QHOVER ,true), - APMPlaneMode(APMPlaneMode::QLOITER ,true), - APMPlaneMode(APMPlaneMode::QLAND ,true), - APMPlaneMode(APMPlaneMode::QRTL ,true), + APMPlaneMode(APMPlaneMode::MANUAL, true), + APMPlaneMode(APMPlaneMode::CIRCLE, true), + APMPlaneMode(APMPlaneMode::STABILIZE, true), + APMPlaneMode(APMPlaneMode::TRAINING, true), + APMPlaneMode(APMPlaneMode::ACRO, true), + APMPlaneMode(APMPlaneMode::FLY_BY_WIRE_A, true), + APMPlaneMode(APMPlaneMode::FLY_BY_WIRE_B, true), + APMPlaneMode(APMPlaneMode::CRUISE, true), + APMPlaneMode(APMPlaneMode::AUTOTUNE, true), + APMPlaneMode(APMPlaneMode::AUTO, true), + APMPlaneMode(APMPlaneMode::RTL, true), + APMPlaneMode(APMPlaneMode::LOITER, true), + APMPlaneMode(APMPlaneMode::TAKEOFF, true), + APMPlaneMode(APMPlaneMode::AVOID_ADSB, false), + APMPlaneMode(APMPlaneMode::GUIDED, true), + APMPlaneMode(APMPlaneMode::INITIALIZING, false), + APMPlaneMode(APMPlaneMode::QSTABILIZE, true), + APMPlaneMode(APMPlaneMode::QHOVER, true), + APMPlaneMode(APMPlaneMode::QLOITER, true), + APMPlaneMode(APMPlaneMode::QLAND, true), + APMPlaneMode(APMPlaneMode::QRTL, true), + APMPlaneMode(APMPlaneMode::QAUTOTUNE, true), + APMPlaneMode(APMPlaneMode::QACRO, true), }); if (!_remapParamNameIntialized) { diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h index 0094a669dc441e98ba3896df77b9536a8dd80084..f8c934574076f689a255685766d51645358a0746 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h @@ -33,8 +33,8 @@ public: AUTO = 10, RTL = 11, LOITER = 12, - RESERVED_13 = 13, // RESERVED FOR FUTURE USE - RESERVED_14 = 14, // RESERVED FOR FUTURE USE + TAKEOFF = 13, + AVOID_ADSB = 14, GUIDED = 15, INITIALIZING = 16, QSTABILIZE = 17, @@ -42,6 +42,8 @@ public: QLOITER = 19, QLAND = 20, QRTL = 21, + QAUTOTUNE = 22, + QACRO = 23, }; APMPlaneMode(uint32_t mode, bool settable); diff --git a/src/MissionManager/CMakeLists.txt b/src/MissionManager/CMakeLists.txt index ddda11e3d53b75e2ff4ed541b84b7f404e06f40d..404fb2e19e19ccc0127e52a7a4aab64babd5697f 100644 --- a/src/MissionManager/CMakeLists.txt +++ b/src/MissionManager/CMakeLists.txt @@ -5,6 +5,7 @@ if(BUILD_TESTING) CameraCalcTest.cc CameraSectionTest.cc CorridorScanComplexItemTest.cc + FWLandingPatternTest.cc MissionCommandTreeTest.cc MissionControllerManagerTest.cc MissionControllerTest.cc @@ -30,6 +31,8 @@ add_library(MissionManager CameraSpec.cc ComplexMissionItem.cc CorridorScanComplexItem.cc + CorridorScanPlanCreator.cc + CustomPlanCreator.cc FixedWingLandingComplexItem.cc GeoFenceController.cc GeoFenceManager.cc @@ -41,6 +44,7 @@ add_library(MissionManager MissionItem.cc MissionManager.cc MissionSettingsItem.cc + PlanCreator.cc PlanElementController.cc PlanManager.cc PlanMasterController.cc @@ -55,7 +59,9 @@ add_library(MissionManager SimpleMissionItem.cc SpeedSection.cc StructureScanComplexItem.cc + StructureScanPlanCreator.cc SurveyComplexItem.cc + SurveyPlanCreator.cc TransectStyleComplexItem.cc VisualMissionItem.cc diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc deleted file mode 100644 index d9a05b9f31be6dcb9a63c95ff7aaa9ea791d488a..0000000000000000000000000000000000000000 --- a/src/MissionManager/SurveyMissionItem.cc +++ /dev/null @@ -1,1304 +0,0 @@ -/**************************************************************************** - * - * (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 "SurveyMissionItem.h" -#include "JsonHelper.h" -#include "MissionController.h" -#include "QGCGeo.h" -#include "QGroundControlQmlGlobal.h" -#include "QGCQGeoCoordinate.h" -#include "SettingsManager.h" -#include "AppSettings.h" - -#include - -QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog") - -const char* SurveyMissionItem::jsonComplexItemTypeValue = "survey"; - -const char* SurveyMissionItem::_jsonGridObjectKey = "grid"; -const char* SurveyMissionItem::_jsonGridAltitudeKey = "altitude"; -const char* SurveyMissionItem::_jsonGridAltitudeRelativeKey = "relativeAltitude"; -const char* SurveyMissionItem::_jsonGridAngleKey = "angle"; -const char* SurveyMissionItem::_jsonGridSpacingKey = "spacing"; -const char* SurveyMissionItem::_jsonGridEntryLocationKey = "entryLocation"; -const char* SurveyMissionItem::_jsonTurnaroundDistKey = "turnAroundDistance"; -const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey = "cameraTriggerDistance"; -const char* SurveyMissionItem::_jsonCameraTriggerInTurnaroundKey = "cameraTriggerInTurnaround"; -const char* SurveyMissionItem::_jsonHoverAndCaptureKey = "hoverAndCapture"; -const char* SurveyMissionItem::_jsonGroundResolutionKey = "groundResolution"; -const char* SurveyMissionItem::_jsonFrontalOverlapKey = "imageFrontalOverlap"; -const char* SurveyMissionItem::_jsonSideOverlapKey = "imageSideOverlap"; -const char* SurveyMissionItem::_jsonCameraSensorWidthKey = "sensorWidth"; -const char* SurveyMissionItem::_jsonCameraSensorHeightKey = "sensorHeight"; -const char* SurveyMissionItem::_jsonCameraResolutionWidthKey = "resolutionWidth"; -const char* SurveyMissionItem::_jsonCameraResolutionHeightKey = "resolutionHeight"; -const char* SurveyMissionItem::_jsonCameraFocalLengthKey = "focalLength"; -const char* SurveyMissionItem::_jsonCameraMinTriggerIntervalKey = "minTriggerInterval"; -const char* SurveyMissionItem::_jsonCameraObjectKey = "camera"; -const char* SurveyMissionItem::_jsonCameraNameKey = "name"; -const char* SurveyMissionItem::_jsonManualGridKey = "manualGrid"; -const char* SurveyMissionItem::_jsonCameraOrientationLandscapeKey = "orientationLandscape"; -const char* SurveyMissionItem::_jsonFixedValueIsAltitudeKey = "fixedValueIsAltitude"; -const char* SurveyMissionItem::_jsonRefly90DegreesKey = "refly90Degrees"; - -const char* SurveyMissionItem::settingsGroup = "Survey"; -const char* SurveyMissionItem::manualGridName = "ManualGrid"; -const char* SurveyMissionItem::gridAltitudeName = "GridAltitude"; -const char* SurveyMissionItem::gridAltitudeRelativeName = "GridAltitudeRelative"; -const char* SurveyMissionItem::gridAngleName = "GridAngle"; -const char* SurveyMissionItem::gridSpacingName = "GridSpacing"; -const char* SurveyMissionItem::gridEntryLocationName = "GridEntryLocation"; -const char* SurveyMissionItem::turnaroundDistName = "TurnaroundDist"; -const char* SurveyMissionItem::cameraTriggerDistanceName = "CameraTriggerDistance"; -const char* SurveyMissionItem::cameraTriggerInTurnaroundName = "CameraTriggerInTurnaround"; -const char* SurveyMissionItem::hoverAndCaptureName = "HoverAndCapture"; -const char* SurveyMissionItem::groundResolutionName = "GroundResolution"; -const char* SurveyMissionItem::frontalOverlapName = "FrontalOverlap"; -const char* SurveyMissionItem::sideOverlapName = "SideOverlap"; -const char* SurveyMissionItem::cameraSensorWidthName = "CameraSensorWidth"; -const char* SurveyMissionItem::cameraSensorHeightName = "CameraSensorHeight"; -const char* SurveyMissionItem::cameraResolutionWidthName = "CameraResolutionWidth"; -const char* SurveyMissionItem::cameraResolutionHeightName = "CameraResolutionHeight"; -const char* SurveyMissionItem::cameraFocalLengthName = "CameraFocalLength"; -const char* SurveyMissionItem::cameraTriggerName = "CameraTrigger"; -const char* SurveyMissionItem::cameraOrientationLandscapeName = "CameraOrientationLandscape"; -const char* SurveyMissionItem::fixedValueIsAltitudeName = "FixedValueIsAltitude"; -const char* SurveyMissionItem::cameraName = "Camera"; - -SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) - : ComplexMissionItem(vehicle, parent) - , _sequenceNumber(0) - , _dirty(false) - , _mapPolygon(this) - , _cameraOrientationFixed(false) - , _missionCommandCount(0) - , _refly90Degrees(false) - , _additionalFlightDelaySeconds(0) - , _cameraMinTriggerInterval(0) - , _ignoreRecalc(false) - , _surveyDistance(0.0) - , _cameraShots(0) - , _coveredArea(0.0) - , _timeBetweenShots(0.0) - , _metaDataMap(FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this)) - , _manualGridFact (settingsGroup, _metaDataMap[manualGridName]) - , _gridAltitudeFact (settingsGroup, _metaDataMap[gridAltitudeName]) - , _gridAltitudeRelativeFact (settingsGroup, _metaDataMap[gridAltitudeRelativeName]) - , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName]) - , _gridSpacingFact (settingsGroup, _metaDataMap[gridSpacingName]) - , _gridEntryLocationFact (settingsGroup, _metaDataMap[gridEntryLocationName]) - , _turnaroundDistFact (settingsGroup, _metaDataMap[turnaroundDistName]) - , _cameraTriggerDistanceFact (settingsGroup, _metaDataMap[cameraTriggerDistanceName]) - , _cameraTriggerInTurnaroundFact (settingsGroup, _metaDataMap[cameraTriggerInTurnaroundName]) - , _hoverAndCaptureFact (settingsGroup, _metaDataMap[hoverAndCaptureName]) - , _groundResolutionFact (settingsGroup, _metaDataMap[groundResolutionName]) - , _frontalOverlapFact (settingsGroup, _metaDataMap[frontalOverlapName]) - , _sideOverlapFact (settingsGroup, _metaDataMap[sideOverlapName]) - , _cameraSensorWidthFact (settingsGroup, _metaDataMap[cameraSensorWidthName]) - , _cameraSensorHeightFact (settingsGroup, _metaDataMap[cameraSensorHeightName]) - , _cameraResolutionWidthFact (settingsGroup, _metaDataMap[cameraResolutionWidthName]) - , _cameraResolutionHeightFact (settingsGroup, _metaDataMap[cameraResolutionHeightName]) - , _cameraFocalLengthFact (settingsGroup, _metaDataMap[cameraFocalLengthName]) - , _cameraOrientationLandscapeFact (settingsGroup, _metaDataMap[cameraOrientationLandscapeName]) - , _fixedValueIsAltitudeFact (settingsGroup, _metaDataMap[fixedValueIsAltitudeName]) - , _cameraFact (settingsGroup, _metaDataMap[cameraName]) -{ - _editorQml = "qrc:/qml/SurveyItemEditor.qml"; - - // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default. - // NULL check since object creation during unit testing passes NULL for vehicle - if (_vehicle && _vehicle->multiRotor() && _turnaroundDistFact.rawValue().toDouble() == _turnaroundDistFact.rawDefaultValue().toDouble()) { - // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well. - _turnaroundDistFact.setRawValue(10); - } - - // We override the grid altitude to the mission default - if (_manualGridFact.rawValue().toBool() || _fixedValueIsAltitudeFact.rawValue().toBool()) { - _gridAltitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); - } - - connect(&_gridSpacingFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_gridEntryLocationFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_turnaroundDistFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerInTurnaroundFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(this, &SurveyMissionItem::refly90DegreesChanged, this, &SurveyMissionItem::_generateGrid); - - connect(&_gridAltitudeFact, &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude); - - connect(&_gridAltitudeRelativeFact, &Fact::valueChanged, this, &SurveyMissionItem::_setDirty); - - // Signal to Qml when camera value changes so it can recalc - connect(&_groundResolutionFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_frontalOverlapFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_sideOverlapFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraSensorWidthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraSensorHeightFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraResolutionWidthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraResolutionHeightFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraFocalLengthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraOrientationLandscapeFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - - connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::timeBetweenShotsChanged); - - connect(&_mapPolygon, &QGCMapPolygon::dirtyChanged, this, &SurveyMissionItem::_polygonDirtyChanged); - connect(&_mapPolygon, &QGCMapPolygon::pathChanged, this, &SurveyMissionItem::_generateGrid); -} - -void SurveyMissionItem::_setSurveyDistance(double surveyDistance) -{ - if (!qFuzzyCompare(_surveyDistance, surveyDistance)) { - _surveyDistance = surveyDistance; - emit complexDistanceChanged(); - } -} - -void SurveyMissionItem::_setBoundingCube(QGCGeoBoundingCube bc) -{ - if (bc != _boundingCube) { - _boundingCube = bc; - emit boundingCubeChanged(); - } -} - -void SurveyMissionItem::_setCameraShots(int cameraShots) -{ - if (_cameraShots != cameraShots) { - _cameraShots = cameraShots; - emit cameraShotsChanged(this->cameraShots()); - } -} - -void SurveyMissionItem::_setCoveredArea(double coveredArea) -{ - if (!qFuzzyCompare(_coveredArea, coveredArea)) { - _coveredArea = coveredArea; - emit coveredAreaChanged(_coveredArea); - } -} - -void SurveyMissionItem::_clearInternal(void) -{ - // Bug workaround - while (_simpleGridPoints.count() > 1) { - _simpleGridPoints.takeLast(); - } - emit gridPointsChanged(); - _simpleGridPoints.clear(); - _transectSegments.clear(); - - _missionCommandCount = 0; - - setDirty(true); - - emit specifiesCoordinateChanged(); - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - -int SurveyMissionItem::lastSequenceNumber(void) const -{ - return _sequenceNumber + _missionCommandCount; -} - -void SurveyMissionItem::setCoordinate(const QGeoCoordinate& coordinate) -{ - if (_coordinate != coordinate) { - _coordinate = coordinate; - emit coordinateChanged(_coordinate); - } -} - -void SurveyMissionItem::setDirty(bool dirty) -{ - if (_dirty != dirty) { - _dirty = dirty; - emit dirtyChanged(_dirty); - } -} - -void SurveyMissionItem::save(QJsonArray& missionItems) -{ - QJsonObject saveObject; - - saveObject[JsonHelper::jsonVersionKey] = 3; - saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; - saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_jsonManualGridKey] = _manualGridFact.rawValue().toBool(); - saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitudeFact.rawValue().toBool(); - saveObject[_jsonHoverAndCaptureKey] = _hoverAndCaptureFact.rawValue().toBool(); - saveObject[_jsonRefly90DegreesKey] = _refly90Degrees; - saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble(); - saveObject[_jsonCameraTriggerInTurnaroundKey] = _cameraTriggerInTurnaroundFact.rawValue().toBool(); - - QJsonObject gridObject; - gridObject[_jsonGridAltitudeKey] = _gridAltitudeFact.rawValue().toDouble(); - gridObject[_jsonGridAltitudeRelativeKey] = _gridAltitudeRelativeFact.rawValue().toBool(); - gridObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble(); - gridObject[_jsonGridSpacingKey] = _gridSpacingFact.rawValue().toDouble(); - gridObject[_jsonGridEntryLocationKey] = _gridEntryLocationFact.rawValue().toDouble(); - gridObject[_jsonTurnaroundDistKey] = _turnaroundDistFact.rawValue().toDouble(); - - saveObject[_jsonGridObjectKey] = gridObject; - - if (!_manualGridFact.rawValue().toBool()) { - QJsonObject cameraObject; - cameraObject[_jsonCameraNameKey] = _cameraFact.rawValue().toString(); - cameraObject[_jsonCameraOrientationLandscapeKey] = _cameraOrientationLandscapeFact.rawValue().toBool(); - cameraObject[_jsonCameraSensorWidthKey] = _cameraSensorWidthFact.rawValue().toDouble(); - cameraObject[_jsonCameraSensorHeightKey] = _cameraSensorHeightFact.rawValue().toDouble(); - cameraObject[_jsonCameraResolutionWidthKey] = _cameraResolutionWidthFact.rawValue().toDouble(); - cameraObject[_jsonCameraResolutionHeightKey] = _cameraResolutionHeightFact.rawValue().toDouble(); - cameraObject[_jsonCameraFocalLengthKey] = _cameraFocalLengthFact.rawValue().toDouble(); - cameraObject[_jsonCameraMinTriggerIntervalKey] = _cameraMinTriggerInterval; - cameraObject[_jsonGroundResolutionKey] = _groundResolutionFact.rawValue().toDouble(); - cameraObject[_jsonFrontalOverlapKey] = _frontalOverlapFact.rawValue().toInt(); - cameraObject[_jsonSideOverlapKey] = _sideOverlapFact.rawValue().toInt(); - - saveObject[_jsonCameraObjectKey] = cameraObject; - } - - // Polygon shape - _mapPolygon.saveToJson(saveObject); - - missionItems.append(saveObject); -} - -void SurveyMissionItem::setSequenceNumber(int sequenceNumber) -{ - if (_sequenceNumber != sequenceNumber) { - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(sequenceNumber); - emit lastSequenceNumberChanged(lastSequenceNumber()); - } -} - -bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) -{ - QJsonObject v2Object = complexObject; - - // We need to pull version first to determine what validation/conversion needs to be performed. - QList versionKeyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - }; - if (!JsonHelper::validateKeys(v2Object, versionKeyInfoList, errorString)) { - return false; - } - - int version = v2Object[JsonHelper::jsonVersionKey].toInt(); - if (version != 2 && version != 3) { - errorString = tr("%1 does not support this version of survey items").arg(qgcApp()->applicationName()); - return false; - } - if (version == 2) { - // Convert to v3 - if (v2Object.contains(VisualMissionItem::jsonTypeKey) && v2Object[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) { - v2Object[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; - v2Object[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - } - } - - QList mainKeyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, - { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, - { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, - { _jsonGridObjectKey, QJsonValue::Object, true }, - { _jsonCameraObjectKey, QJsonValue::Object, false }, - { _jsonCameraTriggerDistanceKey, QJsonValue::Double, true }, - { _jsonManualGridKey, QJsonValue::Bool, true }, - { _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true }, - { _jsonHoverAndCaptureKey, QJsonValue::Bool, false }, - { _jsonRefly90DegreesKey, QJsonValue::Bool, false }, - { _jsonCameraTriggerInTurnaroundKey, QJsonValue::Bool, false }, // Should really be required, but it was missing from initial code due to bug - }; - if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) { - return false; - } - - QString itemType = v2Object[VisualMissionItem::jsonTypeKey].toString(); - QString complexType = v2Object[ComplexMissionItem::jsonComplexItemTypeKey].toString(); - if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { - errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); - return false; - } - - _ignoreRecalc = true; - - _mapPolygon.clear(); - - setSequenceNumber(sequenceNumber); - - _manualGridFact.setRawValue (v2Object[_jsonManualGridKey].toBool(true)); - _fixedValueIsAltitudeFact.setRawValue (v2Object[_jsonFixedValueIsAltitudeKey].toBool(true)); - _gridAltitudeRelativeFact.setRawValue (v2Object[_jsonGridAltitudeRelativeKey].toBool(true)); - _hoverAndCaptureFact.setRawValue (v2Object[_jsonHoverAndCaptureKey].toBool(false)); - _cameraTriggerInTurnaroundFact.setRawValue (v2Object[_jsonCameraTriggerInTurnaroundKey].toBool(true)); - - _refly90Degrees = v2Object[_jsonRefly90DegreesKey].toBool(false); - - QList gridKeyInfoList = { - { _jsonGridAltitudeKey, QJsonValue::Double, true }, - { _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true }, - { _jsonGridAngleKey, QJsonValue::Double, true }, - { _jsonGridSpacingKey, QJsonValue::Double, true }, - { _jsonGridEntryLocationKey, QJsonValue::Double, false }, - { _jsonTurnaroundDistKey, QJsonValue::Double, true }, - }; - QJsonObject gridObject = v2Object[_jsonGridObjectKey].toObject(); - if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) { - return false; - } - _gridAltitudeFact.setRawValue (gridObject[_jsonGridAltitudeKey].toDouble()); - _gridAngleFact.setRawValue (gridObject[_jsonGridAngleKey].toDouble()); - _gridSpacingFact.setRawValue (gridObject[_jsonGridSpacingKey].toDouble()); - _turnaroundDistFact.setRawValue (gridObject[_jsonTurnaroundDistKey].toDouble()); - _cameraTriggerDistanceFact.setRawValue (v2Object[_jsonCameraTriggerDistanceKey].toDouble()); - if (gridObject.contains(_jsonGridEntryLocationKey)) { - _gridEntryLocationFact.setRawValue(gridObject[_jsonGridEntryLocationKey].toDouble()); - } else { - _gridEntryLocationFact.setRawValue(_gridEntryLocationFact.rawDefaultValue()); - } - - if (!_manualGridFact.rawValue().toBool()) { - if (!v2Object.contains(_jsonCameraObjectKey)) { - errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera"); - return false; - } - - QJsonObject cameraObject = v2Object[_jsonCameraObjectKey].toObject(); - - // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap" - QString incorrectImageSideOverlap = "imageSizeOverlap"; - if (cameraObject.contains(incorrectImageSideOverlap)) { - cameraObject[_jsonSideOverlapKey] = cameraObject[incorrectImageSideOverlap]; - cameraObject.remove(incorrectImageSideOverlap); - } - - QList cameraKeyInfoList = { - { _jsonGroundResolutionKey, QJsonValue::Double, true }, - { _jsonFrontalOverlapKey, QJsonValue::Double, true }, - { _jsonSideOverlapKey, QJsonValue::Double, true }, - { _jsonCameraSensorWidthKey, QJsonValue::Double, true }, - { _jsonCameraSensorHeightKey, QJsonValue::Double, true }, - { _jsonCameraResolutionWidthKey, QJsonValue::Double, true }, - { _jsonCameraResolutionHeightKey, QJsonValue::Double, true }, - { _jsonCameraFocalLengthKey, QJsonValue::Double, true }, - { _jsonCameraNameKey, QJsonValue::String, true }, - { _jsonCameraOrientationLandscapeKey, QJsonValue::Bool, true }, - { _jsonCameraMinTriggerIntervalKey, QJsonValue::Double, false }, - }; - if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) { - return false; - } - - _cameraFact.setRawValue(cameraObject[_jsonCameraNameKey].toString()); - _cameraOrientationLandscapeFact.setRawValue(cameraObject[_jsonCameraOrientationLandscapeKey].toBool(true)); - - _groundResolutionFact.setRawValue (cameraObject[_jsonGroundResolutionKey].toDouble()); - _frontalOverlapFact.setRawValue (cameraObject[_jsonFrontalOverlapKey].toInt()); - _sideOverlapFact.setRawValue (cameraObject[_jsonSideOverlapKey].toInt()); - _cameraSensorWidthFact.setRawValue (cameraObject[_jsonCameraSensorWidthKey].toDouble()); - _cameraSensorHeightFact.setRawValue (cameraObject[_jsonCameraSensorHeightKey].toDouble()); - _cameraResolutionWidthFact.setRawValue (cameraObject[_jsonCameraResolutionWidthKey].toDouble()); - _cameraResolutionHeightFact.setRawValue (cameraObject[_jsonCameraResolutionHeightKey].toDouble()); - _cameraFocalLengthFact.setRawValue (cameraObject[_jsonCameraFocalLengthKey].toDouble()); - _cameraMinTriggerInterval = cameraObject[_jsonCameraMinTriggerIntervalKey].toDouble(0); - } - - // Polygon shape - /// Load a polygon from json - /// @param json Json object to load from - /// @param required true: no polygon in object will generate error - /// @param errorString Error string if return is false - /// @return true: success, false: failure (errorString set) - if (!_mapPolygon.loadFromJson(v2Object, true /* required */, errorString)) { - _mapPolygon.clear(); - return false; - } - - _ignoreRecalc = false; - _generateGrid(); - - return true; -} - -double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const -{ - double greatestDistance = 0.0; - for (int i=0; i<_simpleGridPoints.count(); i++) { - QGeoCoordinate currentCoord = _simpleGridPoints[i].value(); - double distance = currentCoord.distanceTo(other); - if (distance > greatestDistance) { - greatestDistance = distance; - } - } - return greatestDistance; -} - -void SurveyMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate) -{ - if (_exitCoordinate != coordinate) { - _exitCoordinate = coordinate; - emit exitCoordinateChanged(coordinate); - } -} - -bool SurveyMissionItem::specifiesCoordinate(void) const -{ - return _mapPolygon.count() > 2; -} - -void _calcCameraShots() -{ - -} - -void SurveyMissionItem::_convertTransectToGeo(const QList>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList>& transectSegmentsGeo) -{ - transectSegmentsGeo.clear(); - - for (int i=0; i transectCoords; - const QList& transectPoints = transectSegmentsNED[i]; - - for (int j=0; j>& transects) -{ - QList> rgReversedTransects; - for (int i=transects.count() - 1; i>=0; i--) { - rgReversedTransects.append(transects[i]); - } - transects = rgReversedTransects; -} - -/// Reverse the order of all points withing each transect, First point becomes last and so forth. -void SurveyMissionItem::_reverseInternalTransectPoints(QList>& transects) -{ - for (int i=0; i rgReversedCoords; - QList& rgOriginalCoords = transects[i]; - for (int j=rgOriginalCoords.count()-1; j>=0; j--) { - rgReversedCoords.append(rgOriginalCoords[j]); - } - transects[i] = rgReversedCoords; - } -} - -/// Reorders the transects such that the first transect is the shortest distance to the specified coordinate -/// and the first point within that transect is the shortest distance to the specified coordinate. -/// @param distanceCoord Coordinate to measure distance against -/// @param transects Transects to test and reorder -void SurveyMissionItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects) -{ - double rgTransectDistance[4]; - rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord); - rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord); - rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord); - rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord); - - int shortestIndex = 0; - double shortestDistance = rgTransectDistance[0]; - for (int i=1; i<3; i++) { - if (rgTransectDistance[i] < shortestDistance) { - shortestIndex = i; - shortestDistance = rgTransectDistance[i]; - } - } - - if (shortestIndex > 1) { - // We need to reverse the order of segments - _reverseTransectOrder(transects); - } - if (shortestIndex & 1) { - // We need to reverse the points within each segment - _reverseInternalTransectPoints(transects); - } -} - -void SurveyMissionItem::_appendGridPointsFromTransects(QList>& rgTransectSegments) -{ - qCDebug(SurveyMissionItemLog) << "Entry point _appendGridPointsFromTransects" << rgTransectSegments.first().first(); - - for (int i=0; i& points, int index1, int index2) -{ - QPointF temp = points[index1]; - points[index1] = points[index2]; - points[index2] = temp; -} - -/// Returns true if the current grid angle generates north/south oriented transects -bool SurveyMissionItem::_gridAngleIsNorthSouthTransects() -{ - // Grid angle ranges from -360<->360 - double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble()); - return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0); -} - -void SurveyMissionItem::_adjustTransectsToEntryPointLocation(QList>& transects) -{ - if (transects.count() == 0) { - return; - } - - int entryLocation = _gridEntryLocationFact.rawValue().toInt(); - bool reversePoints = false; - bool reverseTransects = false; - - if (entryLocation == EntryLocationBottomLeft || entryLocation == EntryLocationBottomRight) { - reversePoints = true; - } - if (entryLocation == EntryLocationTopRight || entryLocation == EntryLocationBottomRight) { - reverseTransects = true; - } - - if (reversePoints) { - qCDebug(SurveyMissionItemLog) << "Reverse Points"; - _reverseInternalTransectPoints(transects); - } - if (reverseTransects) { - qCDebug(SurveyMissionItemLog) << "Reverse Transects"; - _reverseTransectOrder(transects); - } - - qCDebug(SurveyMissionItemLog) << "Modified entry point" << transects.first().first(); -} - -int SurveyMissionItem::_calcMissionCommandCount(QList>& transectSegments) -{ - int missionCommandCount= 0; - for (int i=0; i& transectSegment = transectSegments[i]; - - missionCommandCount += transectSegment.count(); // This accounts for all waypoints - if (_hoverAndCaptureEnabled()) { - // Internal camera trigger points are entry point, plus all points before exit point - missionCommandCount += transectSegment.count() - (_hasTurnaround() ? 2 : 0) - 1; - } else if (_triggerCamera() && !_imagesEverywhere()) { - // Camera on/off at entry/exit of each transect - missionCommandCount += 2; - } - } - if (transectSegments.count() && _triggerCamera() && _imagesEverywhere()) { - // Camera on/off for entire survey - missionCommandCount += 2; - } - - return missionCommandCount; -} - -void SurveyMissionItem::_calcBoundingCube() -{ - // Calc bounding cube - double north = 0.0; - double south = 180.0; - double east = 0.0; - double west = 360.0; - for (int i = 0; i < _simpleGridPoints.count(); i++) { - QGeoCoordinate coord = _simpleGridPoints[i].value(); - double lat = coord.latitude() + 90.0; - double lon = coord.longitude() + 180.0; - north = fmax(north, lat); - south = fmin(south, lat); - east = fmax(east, lon); - west = fmin(west, lon); - } - _setBoundingCube(QGCGeoBoundingCube( - QGeoCoordinate(north - 90.0, west - 180.0, _gridAltitudeFact.rawValue().toDouble()), - QGeoCoordinate(south - 90.0, east - 180.0, _gridAltitudeFact.rawValue().toDouble()))); -} - -void SurveyMissionItem::_generateGrid(void) -{ - if (_ignoreRecalc) { - return; - } - - if (_mapPolygon.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) { - _clearInternal(); - return; - } - - _simpleGridPoints.clear(); - _transectSegments.clear(); - _reflyTransectSegments.clear(); - _additionalFlightDelaySeconds = 0; - - QList polygonPoints; - QList> transectSegments; - - // Convert polygon to NED - QGeoCoordinate tangentOrigin = _mapPolygon.pathModel().value(0)->coordinate(); - qCDebug(SurveyMissionItemLog) << "Convert polygon to NED - tangentOrigin" << tangentOrigin; - for (int i=0; i<_mapPolygon.count(); i++) { - double y, x, down; - QGeoCoordinate vertex = _mapPolygon.pathModel().value(i)->coordinate(); - if (i == 0) { - // This avoids a nan calculation that comes out of convertGeoToNed - x = y = 0; - } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); - } - polygonPoints += QPointF(x, y); - qCDebug(SurveyMissionItemLog) << "vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); - } - - double coveredArea = 0.0; - for (int i=0; i(); - QGeoCoordinate coord2 = _simpleGridPoints[i].value(); - surveyDistance += coord1.distanceTo(coord2); - } - _setSurveyDistance(surveyDistance); - // Calc bounding cube - _calcBoundingCube(); - - if (cameraShots == 0 && _triggerCamera()) { - cameraShots = (int)floor(surveyDistance / _triggerDistance()); - // Take into account immediate camera trigger at waypoint entry - cameraShots++; - } - _setCameraShots(cameraShots); - - if (_hoverAndCaptureEnabled()) { - _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds; - } - emit additionalTimeDelayChanged(); - - emit gridPointsChanged(); - - // Determine command count for lastSequenceNumber - _missionCommandCount = _calcMissionCommandCount(_transectSegments); - _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments); - emit lastSequenceNumberChanged(lastSequenceNumber()); - - // Set exit coordinate - if (_simpleGridPoints.count()) { - QGeoCoordinate coordinate = _simpleGridPoints.first().value(); - coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - setCoordinate(coordinate); - QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value(); - exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - _setExitCoordinate(exitCoordinate); - } - - setDirty(true); -} - -void SurveyMissionItem::_updateCoordinateAltitude(void) -{ - _coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - _exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - emit coordinateChanged(_coordinate); - emit exitCoordinateChanged(_exitCoordinate); - setDirty(true); -} - -QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle) -{ - QPointF rotated; - double radians = (M_PI / 180.0) * -angle; - - rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x()); - rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y()); - - return rotated; -} - -void SurveyMissionItem::_intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines) -{ - QLineF topLine (boundRect.topLeft(), boundRect.topRight()); - QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight()); - QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft()); - QLineF rightLine (boundRect.topRight(), boundRect.bottomRight()); - - for (int i=0; i& lineList, const QPolygonF& polygon, QList& resultLines) -{ - resultLines.clear(); - - for (int i=0; i intersections; - - // Intersect the line with all the polygon edges - for (int j=0; j 1) { - QPointF firstPoint; - QPointF secondPoint; - double currentMaxDistance = 0; - - for (int i=0; i currentMaxDistance) { - firstPoint = intersections[i]; - secondPoint = intersections[j]; - currentMaxDistance = newMaxDistance; - } - } - } - - resultLines += QLineF(firstPoint, secondPoint); - } - } -} - -/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2 -void SurveyMissionItem::_adjustLineDirection(const QList& lineList, QList& resultLines) -{ - qreal firstAngle = 0; - for (int i=0; i 1.0) { - adjustedLine.setP1(line.p2()); - adjustedLine.setP2(line.p1()); - } else { - adjustedLine = line; - } - - resultLines += adjustedLine; - } -} - -double SurveyMissionItem::_clampGridAngle90(double gridAngle) -{ - // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order. - if (gridAngle > 90.0) { - gridAngle -= 180.0; - } else if (gridAngle < -90.0) { - gridAngle += 180; - } - return gridAngle; -} - -int SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList>& transectSegments, bool refly) -{ - int cameraShots = 0; - - double gridAngle = _gridAngleFact.rawValue().toDouble(); - double gridSpacing = _gridSpacingFact.rawValue().toDouble(); - - gridAngle = _clampGridAngle90(gridAngle); - gridAngle += refly ? 90 : 0; - qCDebug(SurveyMissionItemLog) << "Clamped grid angle" << gridAngle; - - qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly; - - transectSegments.clear(); - - // Convert polygon to bounding rect - - qCDebug(SurveyMissionItemLog) << "Polygon"; - QPolygonF polygon; - for (int i=0; i lineList; - - // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor. - // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to. - // They are initially generated with the transects flowing from west to east and then points within the transect north to south. - double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0; - double halfWidth = maxWidth / 2.0; - double transectX = boundingCenter.x() - halfWidth; - double transectXMax = transectX + maxWidth; - while (transectX < transectXMax) { - double transectYTop = boundingCenter.y() - halfWidth; - double transectYBottom = boundingCenter.y() + halfWidth; - - lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); - transectX += gridSpacing; - } - - // Now intersect the lines with the polygon - QList intersectLines; -#if 1 - _intersectLinesWithPolygon(lineList, polygon, intersectLines); -#else - // This is handy for debugging grid problems, not for release - intersectLines = lineList; -#endif - - // Less than two transects intersected with the polygon: - // Create a single transect which goes through the center of the polygon - // Intersect it with the polygon - if (intersectLines.count() < 2) { - _mapPolygon.center(); - QLineF firstLine = lineList.first(); - QPointF lineCenter = firstLine.pointAt(0.5); - QPointF centerOffset = boundingCenter - lineCenter; - firstLine.translate(centerOffset); - lineList.clear(); - lineList.append(firstLine); - intersectLines = lineList; - _intersectLinesWithPolygon(lineList, polygon, intersectLines); - } - - // Make sure all lines are going to same direction. Polygon intersection leads to line which - // can be in varied directions depending on the order of the intesecting sides. - QList resultLines; - _adjustLineDirection(intersectLines, resultLines); - - // Calc camera shots here if there are no images in turnaround - if (_triggerCamera() && !_imagesEverywhere()) { - for (int i=0; i transectPoints; - const QLineF& line = resultLines[i]; - - float turnaroundPosition = _turnaroundDistance() / line.length(); - - if (i & 1) { - transectLine = QLineF(line.p2(), line.p1()); - } else { - transectLine = QLineF(line.p1(), line.p2()); - } - - // Build the points along the transect - - if (_hasTurnaround()) { - transectPoints.append(transectLine.pointAt(-turnaroundPosition)); - } - - // Polygon entry point - transectPoints.append(transectLine.p1()); - - // For hover and capture we need points for each camera location - if (_triggerCamera() && _hoverAndCaptureEnabled()) { - if (_triggerDistance() < transectLine.length()) { - int innerPoints = floor(transectLine.length() / _triggerDistance()); - qCDebug(SurveyMissionItemLog) << "innerPoints" << innerPoints; - float transectPositionIncrement = _triggerDistance() / transectLine.length(); - for (int i=0; i& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) -{ - double altitude = _gridAltitudeFact.rawValue().toDouble(); - bool altitudeRelative = _gridAltitudeRelativeFact.rawValue().toBool(); - - qCDebug(SurveyMissionItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 0.0, 0.0, - std::numeric_limits::quiet_NaN(), // Yaw unchanged - coord.latitude(), - coord.longitude(), - altitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - - switch (cameraTrigger) { - case CameraTriggerOff: - case CameraTriggerOn: - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0, - 0, // shutter integration (ignore) - cameraTrigger == CameraTriggerOn ? 1 : 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - break; - case CameraTriggerHoverAndCapture: - item = new MissionItem(seqNum++, - MAV_CMD_IMAGE_START_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - NAN, NAN, NAN, NAN, // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#if 0 - // This generates too many commands. Pulling out for now, to see if image quality is still high enough. - item = new MissionItem(seqNum++, - MAV_CMD_NAV_DELAY, - MAV_FRAME_MISSION, - 0.5, // Delay in seconds, give some time for image to be taken - -1, -1, -1, // No time - 0, 0, 0, // Param 5-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#endif - default: - break; - } - - return seqNum; -} - -bool SurveyMissionItem::_nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord) -{ - if (pointIndex > transectPoints.count()) { - qWarning() << "Bad grid generation"; - return false; - } - - coord = transectPoints[pointIndex]; - return true; -} - -/// Appends the mission items for the survey -/// @param items Mission items are appended to this list -/// @param missionItemParent Parent object for newly created MissionItem objects -/// @param seqNum[in,out] Sequence number to start from -/// @param hasRefly true: misison has a refly section -/// @param buildRefly: true: build the refly section, false: build the first section -/// @return false: Generation failed -bool SurveyMissionItem::_appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly) -{ - bool firstWaypointTrigger = false; - - qCDebug(SurveyMissionItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly); - - QList>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments; - - if (!buildRefly && _imagesEverywhere()) { - firstWaypointTrigger = true; - } - - for (int segmentIndex=0; segmentIndex& segment = transectSegments[segmentIndex]; - - qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count(); - - if (_hasTurnaround()) { - // Add entry turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent); - firstWaypointTrigger = false; - } - - // Add polygon entry point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - if (firstWaypointTrigger) { - cameraTrigger = CameraTriggerOn; - } else { - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn); - } - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - firstWaypointTrigger = false; - - // Add internal hover and capture points - if (_hoverAndCaptureEnabled()) { - int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0); - qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex; - for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) { - if (!_nextTransectCoord(segment, pointIndex, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent); - } - } - - // Add polygon exit point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff); - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - - if (_hasTurnaround()) { - // Add exit turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); - } - - qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex; - } - - if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) { - // Turn off camera at end of survey - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0.0, // trigger distance (off) - 0, 0, 0, 0, 0, 0, // param 2-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - return true; -} - -void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - int seqNum = _sequenceNumber; - - if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, false /* buildRefly */)) { - return; - } - - if (_refly90Degrees) { - _appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, true /* buildRefly */); - } -} - -int SurveyMissionItem::cameraShots(void) const -{ - return _triggerCamera() ? _cameraShots : 0; -} - -void SurveyMissionItem::_cameraValueChanged(void) -{ - emit cameraValueChanged(); -} - -double SurveyMissionItem::timeBetweenShots(void) const -{ - return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed; -} - -void SurveyMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) -{ - ComplexMissionItem::setMissionFlightStatus(missionFlightStatus); - if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) { - _cruiseSpeed = missionFlightStatus.vehicleSpeed; - emit timeBetweenShotsChanged(); - } -} - -void SurveyMissionItem::_setDirty(void) -{ - setDirty(true); -} - -bool SurveyMissionItem::hoverAndCaptureAllowed(void) const -{ - return _vehicle->multiRotor() || _vehicle->vtol(); -} - -double SurveyMissionItem::_triggerDistance(void) const { - return _cameraTriggerDistanceFact.rawValue().toDouble(); -} - -bool SurveyMissionItem::_triggerCamera(void) const -{ - return _triggerDistance() > 0; -} - -bool SurveyMissionItem::_imagesEverywhere(void) const -{ - return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool(); -} - -bool SurveyMissionItem::_hoverAndCaptureEnabled(void) const -{ - return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool(); -} - -bool SurveyMissionItem::_hasTurnaround(void) const -{ - return _turnaroundDistance() > 0; -} - -double SurveyMissionItem::_turnaroundDistance(void) const -{ - return _turnaroundDistFact.rawValue().toDouble(); -} - -void SurveyMissionItem::applyNewAltitude(double newAltitude) -{ - _fixedValueIsAltitudeFact.setRawValue(true); - _gridAltitudeFact.setRawValue(newAltitude); - _calcBoundingCube(); -} - -void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees) -{ - if (refly90Degrees != _refly90Degrees) { - _refly90Degrees = refly90Degrees; - emit refly90DegreesChanged(refly90Degrees); - } -} - -void SurveyMissionItem::_polygonDirtyChanged(bool dirty) -{ - if (dirty) { - setDirty(true); - } -} diff --git a/src/PlanView/CMakeLists.txt b/src/PlanView/CMakeLists.txt index 7c9a478489dee759c22fd91ed278be42624bd995..f04b1d15f7c368dc6f438ae0440c2bdac0163aa8 100644 --- a/src/PlanView/CMakeLists.txt +++ b/src/PlanView/CMakeLists.txt @@ -1,29 +1,33 @@ add_custom_target(PlanViewQml -SOURCES - RallyPointMapVisuals.qml - CorridorScanEditor.qml - CameraSection.qml - SurveyMapVisual.qml - CameraCalc.qml - SurveyItemEditor.qml - RallyPointEditorHeader.qml - GeoFenceMapVisuals.qml - CorridorScanMapVisual.qml - CMakeLists.txt - PlanToolBarIndicators.qml - MissionItemEditor.qml - MissionItemStatus.qml - RallyPointItemEditor.qml - PlanView.qml - FWLandingPatternMapVisual.qml - GeoFenceEditor.qml - MissionItemMapVisual.qml - SimpleItemMapVisual.qml - StructureScanEditor.qml - PlanToolBar.qml - TransectStyleComplexItemStats.qml - MissionSettingsEditor.qml - SimpleItemEditor.qml - StructureScanMapVisual.qml - FWLandingPatternEditor.qml -) \ No newline at end of file + SOURCES + CameraCalcCamera.qml + CameraCalcGrid.qml + CameraSection.qml + CorridorScanEditor.qml + CorridorScanMapVisual.qml + FWLandingPatternEditor.qml + FWLandingPatternMapVisual.qml + GeoFenceEditor.qml + GeoFenceMapVisuals.qml + MissionItemEditor.qml + MissionItemMapVisual.qml + MissionItemStatus.qml + MissionSettingsEditor.qml + PlanEditToolbar.qml + PlanStartOverlay.qml + PlanToolBarIndicators.qml + PlanToolBar.qml + PlanView.qml + RallyPointEditorHeader.qml + RallyPointItemEditor.qml + RallyPointMapVisuals.qml + SimpleItemEditor.qml + SimpleItemMapVisual.qml + StructureScanEditor.qml + StructureScanMapVisual.qml + SurveyItemEditor.qml + SurveyMapVisual.qml + TransectStyleComplexItemStats.qml + TransectStyleMapVisuals.qml +) + diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index f09d1dcf21d5913a3c7bf6d03d94c631cb662799..40c3a62d9dadb43ea2f377048b0ea78eb913e59c 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -158,7 +158,11 @@ static QObject* shapeFileHelperSingletonFactory(QQmlEngine*, QJSEngine*) } QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) + #if defined(__mobile__) : QGuiApplication (argc, argv) + #else + : QApplication (argc, argv) + #endif , _runningUnitTests (unitTesting) { _app = this; diff --git a/src/QGCApplication.h b/src/QGCApplication.h index c47b477adbdf79ff96339497f112438cb5f5fcb3..19b293883e9d3ea0a28a9d4e961bf3068c55bef3 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -51,8 +51,15 @@ class QGCFileDownload; * This class is started by the main method and provides * the central management unit of the groundstation application. * - **/ -class QGCApplication : public QGuiApplication + * Needs QApplication base to support QtCharts drawing module and + * avoid application crashing on 5.12. Enforce no widget on mobile +**/ +class QGCApplication : + #if defined(__mobile__) + public QGuiApplication + #else + public QApplication + #endif { Q_OBJECT diff --git a/src/QmlControls/MAVLinkMessageButton.qml b/src/QmlControls/MAVLinkMessageButton.qml index 2ed3fc700dc738d31e7c19326234752380ec8ed0..9ae7d1eff10b501033cc4cf2bf09b98a005e56a2 100644 --- a/src/QmlControls/MAVLinkMessageButton.qml +++ b/src/QmlControls/MAVLinkMessageButton.qml @@ -25,12 +25,18 @@ Button { } property double messageHz: 0 + property int compID: 0 contentItem: RowLayout { + QGCLabel { + text: control.compID + color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 3 + } QGCLabel { text: control.text color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText - Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 26 + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 28 } QGCLabel { color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText diff --git a/src/QmlControls/ParameterEditor.qml b/src/QmlControls/ParameterEditor.qml index c9f7d612d45eafd74040c43b3a12a6774e7f2736..f7537f3fb5f6575ef1c5dc1cbba0a90d774c345d 100644 --- a/src/QmlControls/ParameterEditor.qml +++ b/src/QmlControls/ParameterEditor.qml @@ -203,8 +203,8 @@ Item { readonly property string groupName: modelData onClicked: { + if (!checked) _rowWidth = 10 checked = true - _rowWidth = 10 controller.currentCategory = category controller.currentGroup = groupName } diff --git a/src/QtLocationPlugin/CMakeLists.txt b/src/QtLocationPlugin/CMakeLists.txt index 6b7e47788495decb69998d47cd32ce5363b45244..8b498b42ca141d6a61554767bb9e3b4d7cca5f97 100644 --- a/src/QtLocationPlugin/CMakeLists.txt +++ b/src/QtLocationPlugin/CMakeLists.txt @@ -1,5 +1,12 @@ add_library(QtLocationPlugin + BingMapProvider.cpp + ElevationMapProvider.cpp + EsriMapProvider.cpp + GenericMapProvider.cpp + GoogleMapProvider.cpp + MapboxMapProvider.cpp + MapProvider.cpp QGCMapEngine.cpp QGCMapTileSet.cpp QGCMapUrlEngine.cpp diff --git a/src/Vehicle/CMakeLists.txt b/src/Vehicle/CMakeLists.txt index d115b9c39a74cc960142a5a800fdca2a5f398a0c..e9cb7f1a2dcc47da5c8576b4e9b9bd0fc459031a 100644 --- a/src/Vehicle/CMakeLists.txt +++ b/src/Vehicle/CMakeLists.txt @@ -9,10 +9,11 @@ endif() add_library(Vehicle ADSBVehicle.cc GPSRTKFactGroup.cc - VehicleObjectAvoidance.cc MAVLinkLogManager.cc MultiVehicleManager.cc + TrajectoryPoints.cc Vehicle.cc + VehicleObjectAvoidance.cc ${EXTRA_SRC} ) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2412e85b165140b3becaacdf9b110e050f3d3b2a..55eee91eaaea5891aa20579fffef24b13ff40a52 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -12,6 +12,8 @@ #include #include +#include + #include "Vehicle.h" #include "MAVLinkProtocol.h" #include "FirmwarePluginManager.h" @@ -1132,15 +1134,25 @@ void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message) mavlink_attitude_quaternion_t attitudeQuaternion; mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion); + Eigen::Quaternionf quat(attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4); + Eigen::Vector3f rates(attitudeQuaternion.rollspeed, attitudeQuaternion.pitchspeed, attitudeQuaternion.yawspeed); + Eigen::Quaternionf repr_offset(attitudeQuaternion.repr_offset_q[0], attitudeQuaternion.repr_offset_q[1], attitudeQuaternion.repr_offset_q[2], attitudeQuaternion.repr_offset_q[3]); + + // if repr_offset is valid, rotate attitude and rates + if (repr_offset.norm() >= 0.5f) { + quat = quat * repr_offset; + rates = repr_offset * rates; + } + float roll, pitch, yaw; - float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 }; + float q[] = { quat.w(), quat.x(), quat.y(), quat.z() }; mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw); _handleAttitudeWorker(roll, pitch, yaw); - rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed)); - pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed)); - yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed)); + rollRate()->setRawValue(qRadiansToDegrees(rates[0])); + pitchRate()->setRawValue(qRadiansToDegrees(rates[1])); + yawRate()->setRawValue(qRadiansToDegrees(rates[2])); } void Vehicle::_handleGpsRawInt(mavlink_message_t& message) diff --git a/src/Vehicle/VehicleObjectAvoidance.cc b/src/Vehicle/VehicleObjectAvoidance.cc index d6b9151e9ade14bb798de52bc7f136c596e4d0dd..33be0d44ba76634ef8567fec1f375e3c95dbd7ba 100644 --- a/src/Vehicle/VehicleObjectAvoidance.cc +++ b/src/Vehicle/VehicleObjectAvoidance.cc @@ -12,7 +12,7 @@ #include "ParameterManager.h" #include -static const char* kColPrevParam = "MPC_COL_PREV_D"; +static const char* kColPrevParam = "CP_DIST"; //----------------------------------------------------------------------------- VehicleObjectAvoidance::VehicleObjectAvoidance(Vehicle *vehicle, QObject* parent)