diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 4b08542642b55a1aebc65a0678255bf13e75743d..f5f05aa3dc7f549cc52fa1b1ffbd827b6b8aeb2c 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -540,14 +540,40 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) { - // Streams are not started automatically on APM stack - vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); + if (vehicle->isOfflineEditingVehicle()) { + switch (vehicle->vehicleType()) { + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + case MAV_TYPE_COAXIAL: + case MAV_TYPE_HELICOPTER: + vehicle->setFirmwareVersion(3, 4, 0); + break; + case MAV_TYPE_FIXED_WING: + vehicle->setFirmwareVersion(3, 5, 0); + break; + case MAV_TYPE_GROUND_ROVER: + case MAV_TYPE_SURFACE_BOAT: + vehicle->setFirmwareVersion(3, 0, 0); + break; + case MAV_TYPE_SUBMARINE: + vehicle->setFirmwareVersion(3, 4, 0); + break; + default: + // No version set + break; + } + } else { + // Streams are not started automatically on APM stack + vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); + } } void APMFirmwarePlugin::setSupportedModes(QList supportedModes) diff --git a/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml b/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml index 120221ee39845ec0a81c505a25d10538b51bdc9f..f0da718fa250bf75a01a243f8bc267a193faf9c7 100644 --- a/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml +++ b/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml @@ -78,5 +78,7 @@ Column { polygon: geoFenceController.polygon sectionLabel: qsTr("Fence Polygon:") visible: geoFenceController.polygonSupported + + onPolygonEditCompleted: geoFenceController.validateBreachReturn() } } diff --git a/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml b/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml index 1db4ab444bb7609687c4dfaf1fa25005e29b49cf..39c41aafe1ce74cab080a44897b324c42702f52a 100644 --- a/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml +++ b/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml @@ -202,5 +202,7 @@ Column { flightMap: editorMap polygon: geoFenceController.polygon sectionLabel: qsTr("Fence Polygon:") + + onPolygonEditCompleted: geoFenceController.validateBreachReturn() } } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 3ee357f50928dff397a522d4d7eca8772778e5bc..4ddd29ff08634fc4b9d029fbda9b2a7d0e880524 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -60,7 +60,7 @@ public: /// value: remapParamNameMinorVersionRemapMap_t entry typedef QMap remapParamNameMajorVersionMap_t; - /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. + /// Called when Vehicle is first created to perform any firmware specific setup. virtual void initializeVehicle(Vehicle* vehicle); /// @return true: Firmware supports all specified capabilites diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 2209037516aec0c0ec217d285ed576b402b04e07..7d31add674c5074ddfec72ad6ee796e556bff82c 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -154,6 +154,12 @@ QGCView { Component.onCompleted: start(true /* editMode */) + onFenceSupportedChanged: { + if (!fenceSupported && _editingLayer == _layerGeoFence) { + _editingLayer = _layerMission + } + } + function saveToSelectedFile() { if (ScreenTools.isMobile) { qgcView.showDialog(mobileFileSaver, qsTr("Save Fence File"), qgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) @@ -170,9 +176,14 @@ QGCView { } } - onFenceSupportedChanged: { - if (!fenceSupported && _editingLayer == _layerGeoFence) { - _editingLayer = _layerMission + function validateBreachReturn() { + if (geoFenceController.polygon.path.length > 0) { + if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) { + geoFenceController.breachReturnPoint = geoFenceController.polygon.center() + } + if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) { + geoFenceController.breachReturnPoint = geoFenceController.polygon.path[0] + } } } } @@ -316,6 +327,7 @@ QGCView { break case _layerGeoFence: geoFenceController.breachReturnPoint = coordinate + geoFenceController.validateBreachReturn() break } } diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index 1e7304450884c695473c1f63ebb0326222f5a76f..f85eded7915d26364bd2faa86ed76342d53440c1 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -13,7 +13,6 @@ #include #include -#include #include const char* QGCMapPolygon::_jsonPolygonKey = "polygon"; @@ -67,24 +66,61 @@ void QGCMapPolygon::setDirty(bool dirty) } } -QGeoCoordinate QGCMapPolygon::center(void) const +QGeoCoordinate QGCMapPolygon::_coordFromPointF(const QPointF& point) const { - QPolygonF polygon; + QGeoCoordinate coord; + + if (_polygonPath.count() > 0) { + QGeoCoordinate tangentOrigin = _polygonPath[0].value(); + convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord); + } - QGeoCoordinate tangentOrigin = _polygonPath[0].value(); + return coord; +} - foreach(const QVariant& coordVar, _polygonPath) { +QPointF QGCMapPolygon::_pointFFromCoord(const QGeoCoordinate& coordinate) const +{ + if (_polygonPath.count() > 0) { double y, x, down; + QGeoCoordinate tangentOrigin = _polygonPath[0].value(); - convertGeoToNed(coordVar.value(), tangentOrigin, &y, &x, &down); - polygon << QPointF(x, -y); + convertGeoToNed(coordinate, tangentOrigin, &y, &x, &down); + return QPointF(x, -y); } - QGeoCoordinate centerCoord; - QPointF centerPoint = polygon.boundingRect().center(); - convertNedToGeo(-centerPoint.y(), centerPoint.x(), 0, tangentOrigin, ¢erCoord); + return QPointF(); +} + +QPolygonF QGCMapPolygon::_toPolygonF(void) const +{ + QPolygonF polygon; - return centerCoord; + if (_polygonPath.count() > 2) { + for (int i=0; i<_polygonPath.count(); i++) { + polygon.append(_pointFFromCoord(_polygonPath[i].value())); + } + } + + return polygon; +} + +bool QGCMapPolygon::containsCoordinate(const QGeoCoordinate& coordinate) const +{ + if (_polygonPath.count() > 2) { + return _toPolygonF().containsPoint(_pointFFromCoord(coordinate), Qt::OddEvenFill); + } else { + return false; + } +} + +QGeoCoordinate QGCMapPolygon::center(void) const +{ + if (_polygonPath.count() > 2) { + QPointF centerPoint = _toPolygonF().boundingRect().center(); + return _coordFromPointF(centerPoint); + } else { + return QGeoCoordinate(); + } } void QGCMapPolygon::setPath(const QList& path) diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h index 92810e6ff82cd010693f1f3ed9f6b95223759fb4..599fd98b090928360ea918a4e92d656fb62aee14 100644 --- a/src/MissionManager/QGCMapPolygon.h +++ b/src/MissionManager/QGCMapPolygon.h @@ -13,7 +13,11 @@ #include #include #include +#include +/// The QGCMapPolygon class provides a polygon which can be displayed on a map using a MapPolygon control. +/// It works in conjunction with the QGCMapPolygonControls control which provides the UI for drawing and +/// editing map polygons. class QGCMapPolygon : public QObject { Q_OBJECT @@ -23,24 +27,33 @@ public: const QGCMapPolygon& operator=(const QGCMapPolygon& other); - Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged) - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + QGeoCoordinate operator[](int index) const { return _polygonPath[index].value(); } + + /// The polygon path to be bound to the MapPolygon.path property + Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged) + /// true: Polygon has changed since last time dirty was false + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + + /// Remove all points from polygon Q_INVOKABLE void clear(void); + + /// Adjust the value for the specified coordinate + /// @param vertexIndex Polygon point index to modify (0-based) + /// @param coordinate New coordinate for point Q_INVOKABLE void adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate); - Q_INVOKABLE QGeoCoordinate center(void) const; - Q_INVOKABLE int count(void) const { return _polygonPath.count(); } - QVariantList path(void) const { return _polygonPath; } - void setPath(const QList& path); - void setPath(const QVariantList& path); + /// Returns the center point coordinate for the polygon + Q_INVOKABLE QGeoCoordinate center(void) const; - QList coordinateList(void) const; + /// Returns true if the specified coordinate is within the polygon + Q_INVOKABLE bool containsCoordinate(const QGeoCoordinate& coordinate) const; - QGeoCoordinate operator[](int index) const { return _polygonPath[index].value(); } + /// Returns the number of points in the polygon + Q_INVOKABLE int count(void) const { return _polygonPath.count(); } - bool dirty(void) const { return _dirty; } - void setDirty(bool dirty); + /// Returns the path in a list of QGeoCoordinate's format + QList coordinateList(void) const; /// Saves the polygon to the json object. /// @param json Json object to save to @@ -53,11 +66,24 @@ public: /// @return true: success, false: failure (errorString set) bool loadFromJson(const QJsonObject& json, bool required, QString& errorString); + // Property methods + + bool dirty(void) const { return _dirty; } + void setDirty(bool dirty); + + QVariantList path(void) const { return _polygonPath; } + void setPath(const QList& path); + void setPath(const QVariantList& path); + signals: void pathChanged(void); void dirtyChanged(bool dirty); private: + QPolygonF _toPolygonF(void) const; + QGeoCoordinate _coordFromPointF(const QPointF& point) const; + QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const; + QVariantList _polygonPath; bool _dirty; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 52496564de856a971685e589ee3457cde22673c6..f6abb0cc40502764cd997dbb4d05376b088af009 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -328,6 +328,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _vibrationFactGroup(this) { _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); + _firmwarePlugin->initializeVehicle(this); _missionManager = new MissionManager(this); connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);