Commit 03f649bd authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #4050 from DonLakeFlyer/BreachValidate

GeoFence: Breach validate
parents 245a9ea7 097cc934
......@@ -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)
......
......@@ -78,5 +78,7 @@ Column {
polygon: geoFenceController.polygon
sectionLabel: qsTr("Fence Polygon:")
visible: geoFenceController.polygonSupported
onPolygonEditCompleted: geoFenceController.validateBreachReturn()
}
}
......@@ -202,5 +202,7 @@ Column {
flightMap: editorMap
polygon: geoFenceController.polygon
sectionLabel: qsTr("Fence Polygon:")
onPolygonEditCompleted: geoFenceController.validateBreachReturn()
}
}
......@@ -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
......
......@@ -18,6 +18,10 @@ PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
, _circleRadiusFact(NULL)
{
connect(_vehicle->getParameterManager(), &ParameterManager::parametersReady, this, &PX4GeoFenceManager::_parametersReady);
if (_vehicle->getParameterManager()->parametersAreReady()) {
_parametersReady();
}
}
PX4GeoFenceManager::~PX4GeoFenceManager()
......
......@@ -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
}
}
......@@ -373,7 +385,7 @@ QGCView {
// Add the complex mission item polygon to the map
MapItemView {
model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
model: missionController.complexVisualItems
delegate: MapPolygon {
color: 'green'
......@@ -384,7 +396,7 @@ QGCView {
// Add the complex mission item grid to the map
MapItemView {
model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
model: missionController.complexVisualItems
delegate: MapPolyline {
line.color: "white"
......@@ -395,7 +407,7 @@ QGCView {
// Add the complex mission item exit coordinates
MapItemView {
model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
model: missionController.complexVisualItems
delegate: exitCoordinateComponent
}
......@@ -413,7 +425,7 @@ QGCView {
// Add the simple mission items to the map
MapItemView {
model: _editingLayer == _layerMission ? missionController.visualItems : undefined
model: missionController.visualItems
delegate: missionItemComponent
}
......
......@@ -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, &centerCoord);
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)
......
......@@ -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;
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment