diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 4b08542..f5f05aa 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<APMCustomMode> supportedModes)
diff --git a/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml b/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml
index 120221e..f0da718 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 1db4ab4..39c41aa 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 3ee357f..4ddd29f 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -60,7 +60,7 @@ public:
/// value: remapParamNameMinorVersionRemapMap_t entry
typedef QMap<int, remapParamNameMinorVersionRemapMap_t> 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 2209037..7d31add 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 1e73044..f85eded 100644
--- a/src/MissionManager/QGCMapPolygon.cc
+++ b/src/MissionManager/QGCMapPolygon.cc
@@ -13,7 +13,6 @@
#include <QGeoRectangle>
#include <QDebug>
-#include <QPolygon>
#include <QJsonArray>
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<QGeoCoordinate>();
+ convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
+ }
- QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
+ 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<QGeoCoordinate>();
- convertGeoToNed(coordVar.value<QGeoCoordinate>(), 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<QGeoCoordinate>()));
+ }
+ }
+
+ 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<QGeoCoordinate>& path)
diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h
index 92810e6..599fd98 100644
--- a/src/MissionManager/QGCMapPolygon.h
+++ b/src/MissionManager/QGCMapPolygon.h
@@ -13,7 +13,11 @@
#include <QObject>
#include <QGeoCoordinate>
#include <QVariantList>
+#include <QPolygon>
+/// 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<QGeoCoordinate>(); }
+
+ /// 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<QGeoCoordinate>& path);
- void setPath(const QVariantList& path);
+ /// Returns the center point coordinate for the polygon
+ Q_INVOKABLE QGeoCoordinate center(void) const;
- QList<QGeoCoordinate> 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<QGeoCoordinate>(); }
+ /// 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<QGeoCoordinate> 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<QGeoCoordinate>& 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 5249656..f6abb0c 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);