diff --git a/qgcresources.qrc b/qgcresources.qrc index cd510d2befb575f779a31964ff3b97eeae1fb8b2..88964444ffd829d5925372295df476f99d80cd86 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -80,6 +80,7 @@ src/AutoPilotPlugins/APM/Images/vectored6dof-frame.png src/AutoPilotPlugins/APM/Images/simple3-frame.png src/AutoPilotPlugins/APM/Images/simple4-frame.png + src/AutoPilotPlugins/APM/Images/simple5-frame.png src/AnalyzeView/LogDownloadIcon.svg src/AutoPilotPlugins/PX4/Images/LowBattery.svg src/AutoPilotPlugins/PX4/Images/LowBatteryLight.svg diff --git a/src/AutoPilotPlugins/APM/APMSubFrameComponent.qml b/src/AutoPilotPlugins/APM/APMSubFrameComponent.qml index 2a61de995d1c23b91786b9e2e9344266c35467d6..4b1b8a3e48db1fc97c8e79933cf474243118ca36 100644 --- a/src/AutoPilotPlugins/APM/APMSubFrameComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSubFrameComponent.qml @@ -103,6 +103,12 @@ SetupPage { resource: "qrc:///qmlimages/Frames/SimpleROV-4.png" paramValue: 5 } + + ListElement { + name: "SimpleROV-5" + resource: "qrc:///qmlimages/Frames/SimpleROV-5.png" + paramValue: 6 + } } Item { diff --git a/src/AutoPilotPlugins/APM/Images/simple5-frame.png b/src/AutoPilotPlugins/APM/Images/simple5-frame.png new file mode 100644 index 0000000000000000000000000000000000000000..4be0ceada877af8aafb087941c3b3ffa489fb7cf Binary files /dev/null and b/src/AutoPilotPlugins/APM/Images/simple5-frame.png differ diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index b75fb2f39dd98f7cab711dc395a59d112cd12060..01947c4c24406d87873a2ee0c624cedf31a3e860 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -22,6 +22,7 @@ APMRoverMode::APMRoverMode(uint32_t mode, bool settable) enumToString.insert(STEERING, "Steering"); enumToString.insert(HOLD, "Hold"); enumToString.insert(LOITER, "Loiter"); + enumToString.insert(FOLLOW, "Follow"); enumToString.insert(SIMPLE, "Simple"); enumToString.insert(AUTO, "Auto"); enumToString.insert(RTL, "RTL"); @@ -40,6 +41,7 @@ ArduRoverFirmwarePlugin::ArduRoverFirmwarePlugin(void) supportedFlightModes << APMRoverMode(APMRoverMode::STEERING ,true); supportedFlightModes << APMRoverMode(APMRoverMode::HOLD ,true); supportedFlightModes << APMRoverMode(APMRoverMode::LOITER ,true); + supportedFlightModes << APMRoverMode(APMRoverMode::FOLLOW ,true); supportedFlightModes << APMRoverMode(APMRoverMode::SIMPLE ,true); supportedFlightModes << APMRoverMode(APMRoverMode::AUTO ,true); supportedFlightModes << APMRoverMode(APMRoverMode::RTL ,true); diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 98d932ec329a78cec63d0bf647b0b78037f8aab8..64d61334f5963a3fc5a062968adeb168642e30d0 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -25,6 +25,7 @@ public: STEERING = 3, HOLD = 4, LOITER = 5, + FOLLOW = 6, SIMPLE = 7, AUTO = 10, RTL = 11, diff --git a/src/FirmwarePlugin/APM/MavCmdInfoCommon.json b/src/FirmwarePlugin/APM/MavCmdInfoCommon.json index 4a508187203b0f1d6ecf82914161cfc95a7fdf94..8f35edd874a2e3d8fb3b56e2b613ea50aaa716a2 100644 --- a/src/FirmwarePlugin/APM/MavCmdInfoCommon.json +++ b/src/FirmwarePlugin/APM/MavCmdInfoCommon.json @@ -9,6 +9,36 @@ "comment": "MAV_CMD_NAV_WAYPOINT", "paramRemove": "2" }, + { + "id": 17, + "comment": "MAV_CMD_NAV_LOITER_UNLIM", + "param4": { + "label": "Heading", + "units": "radians", + "default": 0, + "decimalPlaces": 2 + } + }, + { + "id": 18, + "comment": "MAV_CMD_NAV_LOITER_TURNS", + "param4": { + "label": "Heading", + "units": "radians", + "default": 0, + "decimalPlaces": 2 + } + }, + { + "id": 19, + "comment": "MAV_CMD_NAV_LOITER_TIME", + "param4": { + "label": "Heading", + "units": "radians", + "default": 0, + "decimalPlaces": 2 + } + }, { "id": 22, "comment": "MAV_CMD_NAV_TAKEOFF", diff --git a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json index 6fa8db824139abf600a73f4e648afb5db7d37621..9cca7514c414990bcf3f94092886c11c9716dc6a 100644 --- a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json +++ b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json @@ -9,6 +9,28 @@ "comment": "MAV_CMD_NAV_WAYPOINT", "paramRemove": "1,3,4" }, + { + "id": 18, + "comment": "MAV_CMD_NAV_LOITER_TURNS", + "param4": { + "label": "Exit loiter from", + "enumStrings": "Center,Tangent", + "enumValues": "0,1", + "default": 1, + "decimalPlaces": 0 + } + }, + { + "id": 19, + "comment": "MAV_CMD_NAV_LOITER_TIME", + "param4": { + "label": "Exit loiter from", + "enumStrings": "Center,Tangent", + "enumValues": "0,1", + "default": 1, + "decimalPlaces": 0 + } + }, { "id": 21, "comment": "MAV_CMD_NAV_LAND", diff --git a/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json b/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json index e8f236bbd10e99d9fe2b8e16340952a8da5fc6e9..8657261895e728cd9fdf5aa479ab99e3d4d1e8c7 100644 --- a/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json +++ b/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json @@ -9,39 +9,6 @@ "comment": "MAV_CMD_NAV_WAYPOINT", "paramRemove": "2,3" }, - { - "id": 17, - "comment": "MAV_CMD_NAV_LOITER_UNLIM", - "param4": { - "label": "Heading", - "units": "radians", - "nanUnchanged": true, - "default": null, - "decimalPlaces": 2 - } - }, - { - "id": 18, - "comment": "MAV_CMD_NAV_LOITER_TURNS", - "param4": { - "label": "Heading", - "units": "radians", - "nanUnchanged": true, - "default": null, - "decimalPlaces": 2 - } - }, - { - "id": 19, - "comment": "MAV_CMD_NAV_LOITER_TIME", - "param4": { - "label": "Heading", - "units": "radians", - "nanUnchanged": true, - "default": null, - "decimalPlaces": 2 - } - }, { "id": 21, "comment": "MAV_CMD_NAV_LAND", diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index db3ad990abe4aa4318b89a4eca11b1b112a89386..bb418830646a988c984a7707b1dd9f13dffab0d9 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -100,6 +100,12 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); + if (vehicle->apmFirmware()) { + // ArduPilot does not support camera commands + _stopTakingVideoFact.setRawValue(false); + _stopTakingPhotosFact.setRawValue(false); + } + if (_valueSetIsDistanceFact.rawValue().toBool()) { _recalcFromHeadingAndDistanceChange(); } else { diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 93cf03214027082100a264acc2ac436efddf53c9..5841b02a92b7d211ec4f705b3afef01aa1973252 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -101,7 +101,8 @@ "param4": { "label": "Heading", "units": "radians", - "default": 0, + "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -128,7 +129,8 @@ "param4": { "label": "Heading", "units": "radians", - "default": 0, + "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index d8b133259c191466364303bb87af61de6e5e69c5..b2451dee30c89c90d7b8a9ac6002d0d6774cbd77 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -32,10 +32,14 @@ Rectangle { //property real availableWidth ///< Width for control //property var missionItem ///< Mission Item for editor + property var _masterControler: masterController + property var _missionController: _masterControler.missionController + property var _missionVehicle: _masterControler.controllerVehicle property real _margin: ScreenTools.defaultFontPixelWidth / 2 property real _spacer: ScreenTools.defaultFontPixelWidth / 2 property string _setToVehicleHeadingStr: qsTr("Set to vehicle heading") property string _setToVehicleLocationStr: qsTr("Set to vehicle location") + property bool _showCameraSection: !_missionVehicle.apmFirmware ExclusiveGroup { id: distanceGlideGroup } @@ -165,15 +169,16 @@ Rectangle { } SectionHeader { - id: cameraSection - text: qsTr("Camera") + id: cameraSection + text: qsTr("Camera") + visible: _showCameraSection } Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: cameraSection.checked + visible: _showCameraSection && cameraSection.checked Item { width: 1; height: _spacer } diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index fbf84f2f79117e7bb4ce4aa84bd5db93808ea60f..4365fcc657729a10c1837001728fcf47fea08d3d 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -147,6 +147,15 @@ void QGroundControlQmlGlobal::startAPMArduSubMockLink(bool sendStatusText) #endif } +void QGroundControlQmlGlobal::startAPMArduRoverMockLink(bool sendStatusText) +{ +#ifdef QT_DEBUG + MockLink::startAPMArduRoverMockLink(sendStatusText); +#else + Q_UNUSED(sendStatusText); +#endif +} + void QGroundControlQmlGlobal::stopOneMockLink(void) { #ifdef QT_DEBUG diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 0a4fb92055a91f58db03959db449f56d1295d3aa..2267279085b302a7ae56dc3692f904ffd5308616 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -121,6 +121,7 @@ public: Q_INVOKABLE void startAPMArduCopterMockLink (bool sendStatusText); Q_INVOKABLE void startAPMArduPlaneMockLink (bool sendStatusText); Q_INVOKABLE void startAPMArduSubMockLink (bool sendStatusText); + Q_INVOKABLE void startAPMArduRoverMockLink (bool sendStatusText); Q_INVOKABLE void stopOneMockLink (void); /// Converts from meters to the user specified distance unit diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9edc2f30c3ed36b27b2783023e1283dc29053343..d24da50c8545eaba8fa5fdc12f73c55c037beda4 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1159,64 +1159,46 @@ MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) return qobject_cast(linkMgr->createConnectedLink(config)); } -MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration("PX4 MockLink"); + MockConfiguration* mockConfig = new MockConfiguration(configName); - mockConfig->setFirmwareType(MAV_AUTOPILOT_PX4); - mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); + mockConfig->setFirmwareType(firmwareType); + mockConfig->setVehicleType(vehicleType); mockConfig->setSendStatusText(sendStatusText); mockConfig->setFailureMode(failureMode); return _startMockLink(mockConfig); } -MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration("Generic MockLink"); - - mockConfig->setFirmwareType(MAV_AUTOPILOT_GENERIC); - mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); - mockConfig->setSendStatusText(sendStatusText); - mockConfig->setFailureMode(failureMode); + return _startMockLinkWorker("PX4 MultiRotor MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); +} - return _startMockLink(mockConfig); +MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +{ + return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration("APM ArduCopter MockLink"); - - mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); - mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); - mockConfig->setSendStatusText(sendStatusText); - mockConfig->setFailureMode(failureMode); - - return _startMockLink(mockConfig); + return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration("APM ArduPlane MockLink"); - - mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); - mockConfig->setVehicleType(MAV_TYPE_FIXED_WING); - mockConfig->setSendStatusText(sendStatusText); - mockConfig->setFailureMode(failureMode); - - return _startMockLink(mockConfig); + return _startMockLinkWorker("ArduPlane MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration("APM ArduSub MockLink"); - - mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); - mockConfig->setVehicleType(MAV_TYPE_SUBMARINE); - mockConfig->setSendStatusText(sendStatusText); - mockConfig->setFailureMode(failureMode); + return _startMockLinkWorker("ArduSub MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode); +} - return _startMockLink(mockConfig); +MockLink* MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +{ + return _startMockLinkWorker("ArduRover MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode); } void MockLink::_sendRCChannels(void) diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index e771cd6a2888aa0878eb4018a917a5d4168140a7..0d3bee32345a7c84e21645e57777604cf4d8dd3d 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -156,6 +156,7 @@ public: static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); private slots: virtual void _writeBytes(const QByteArray bytes); @@ -203,6 +204,7 @@ private: void _sendADSBVehicles(void); void _moveADSBVehicle(void); + static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); static MockLink* _startMockLink(MockConfiguration* mockConfig); MockLinkMissionItemHandler _missionItemHandler; diff --git a/src/ui/preferences/MockLink.qml b/src/ui/preferences/MockLink.qml index ed1073d0b48011d0a8c0eff213ff49cda8f4f6a6..753336e038bcb7f21cb120ef099f8df55331c88f 100644 --- a/src/ui/preferences/MockLink.qml +++ b/src/ui/preferences/MockLink.qml @@ -52,6 +52,10 @@ Rectangle { text: qsTr("APM ArduSub Vehicle") onClicked: QGroundControl.startAPMArduSubMockLink(sendStatusText.checked) } + QGCButton { + text: qsTr("APM ArduRover Vehicle") + onClicked: QGroundControl.startAPMArduRoverMockLink(sendStatusText.checked) + } QGCButton { text: qsTr("Generic Vehicle") onClicked: QGroundControl.startGenericMockLink(sendStatusText.checked)