Skip to content
GitLab
Commit 537463c5 authored by Don Gagne's avatar Don Gagne
Browse files

New Mavlink 2 spec GeoFence and Rally Point support

parent a11b1dbf
Expand all Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ 537463c5
......@@ -509,7 +509,11 @@ HEADERS += \
src/MissionManager/MissionManager.h \
src/MissionManager/MissionSettingsItem.h \
src/MissionManager/PlanElementController.h \
src/MissionManager/PlanManager.h \
src/MissionManager/PlanMasterController.h \
src/MissionManager/QGCFenceCircle.h \
src/MissionManager/QGCFencePolygon.h \
src/MissionManager/QGCMapCircle.h \
src/MissionManager/QGCMapPolygon.h \
src/MissionManager/RallyPoint.h \
src/MissionManager/RallyPointController.h \
......@@ -689,7 +693,11 @@ SOURCES += \
src/MissionManager/MissionManager.cc \
src/MissionManager/MissionSettingsItem.cc \
src/MissionManager/PlanElementController.cc \
src/MissionManager/PlanManager.cc \
src/MissionManager/PlanMasterController.cc \
src/MissionManager/QGCFenceCircle.cc \
src/MissionManager/QGCFencePolygon.cc \
src/MissionManager/QGCMapCircle.cc \
src/MissionManager/QGCMapPolygon.cc \
src/MissionManager/RallyPoint.cc \
src/MissionManager/RallyPointController.cc \
......@@ -906,9 +914,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.h \
src/AutoPilotPlugins/APM/APMTuningComponent.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/APMRallyPointManager.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \
......@@ -933,9 +939,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \
src/AutoPilotPlugins/APM/APMTuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/APMRallyPointManager.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \
......@@ -974,7 +978,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.h \
src/AutoPilotPlugins/PX4/SensorsComponentController.h \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
SOURCES += \
......@@ -995,7 +998,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.cc \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \
}
......
qgroundcontrol.qrc
View file @ 537463c5
......@@ -90,6 +90,7 @@
<file alias="QGroundControl/Controls/QGCLabel.qml">src/QmlControls/QGCLabel.qml</file>
<file alias="QGroundControl/Controls/QGCListView.qml">src/QmlControls/QGCListView.qml</file>
<file alias="QGroundControl/Controls/QGCMapLabel.qml">src/QmlControls/QGCMapLabel.qml</file>
<file alias="QGroundControl/Controls/QGCMapCircleVisuals.qml">src/MissionManager/QGCMapCircleVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMapPolygonVisuals.qml">src/MissionManager/QGCMapPolygonVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
......@@ -199,6 +200,7 @@
<file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
<file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.SettingsGroup.json</file>
<file alias="Guided.SettingsGroup.json">src/Settings/Guided.SettingsGroup.json</file>
<file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 537463c5
......@@ -17,8 +17,6 @@
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
#include "APMGeoFenceManager.h"
#include "APMRallyPointManager.h"
#include <QAbstractSocket>
......@@ -93,8 +91,6 @@ public:
QString internalParameterMetaDataFile (Vehicle* vehicle) final;
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) final { return new APMGeoFenceManager(vehicle); }
RallyPointManager* newRallyPointManager (Vehicle* vehicle) final { return new APMRallyPointManager(vehicle); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
......
src/FirmwarePlugin/APM/APMGeoFenceManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMGeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
#include "QmlObjectListModel.h"
#include "QGCQGeoCoordinate.h"
const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
, _fenceSupported(false)
, _circleEnabled(false)
, _polygonSupported(false)
, _polygonEnabled(false)
, _breachReturnSupported(vehicle->fixedWing())
, _firstParamLoadComplete(false)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _fenceTypeFact(NULL)
, _fenceEnableFact(NULL)
, _circleRadiusFact(NULL)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived);
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady);
if (_vehicle->parameterManager()->parametersReady()) {
_parametersReady();
}
}
APMGeoFenceManager::~APMGeoFenceManager()
{
}
void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
if (_readTransactionInProgress) {
_sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
return;
}
if (!_fenceSupported) {
return;
}
// Validate
int validatedPolygonCount = 0;
if (polygon.count() >= 3) {
validatedPolygonCount = polygon.count();
}
if (polygon.count() > std::numeric_limits<uint8_t>::max()) {
_sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
validatedPolygonCount = 0;
}
_breachReturnPoint = breachReturn;
_polygon.clear();
if (validatedPolygonCount) {
for (int i=0; i<polygon.count(); i++) {
_polygon.append(polygon.value<QGCQGeoCoordinate*>(i)->coordinate());
}
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
_cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints;
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
// FIXME: No validation of correct fence received
for (uint8_t index=0; index<_cWriteFencePoints; index++) {
_sendFencePoint(index);
}
emit sendComplete(false /* error */);
}
void APMGeoFenceManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
return;
}
_breachReturnPoint = QGeoCoordinate();
_polygon.clear();
if (!_fenceSupported) {
emit loadComplete(_breachReturnPoint, _polygon);
return;
}
// Point 0: Breach return point (always sent, but supported by ArduPlane only)
// Point [1,N]: Polygon points
// Point N+1: Close polygon point (same as point 1)
int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
int minFencePoints = 5;
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints;
if (cFencePoints == 0) {
// No fence
emit loadComplete(_breachReturnPoint, _polygon);
return;
}
if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) {
_sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints));
return;
}
if (cFencePoints > std::numeric_limits<uint8_t>::max()) {
_sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints));
return;
}
_readTransactionInProgress = true;
_cReadFencePoints = cFencePoints;
_currentFencePoint = 0;
_requestFencePoint(_currentFencePoint);
}
/// Called when a new mavlink message for our vehicle is received
void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) {
mavlink_fence_point_t fencePoint;
mavlink_msg_fence_point_decode(&message, &fencePoint);
qCDebug(GeoFenceManagerLog) << "From vehicle fence_point: idx:lat:lng" << fencePoint.idx << fencePoint.lat << fencePoint.lng;
if (fencePoint.idx != _currentFencePoint) {
// FIXME: Protocol out of whack
_readTransactionInProgress = false;
emit inProgressChanged(inProgress());
qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
return;
}
if (fencePoint.idx == 0) {
_breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng);
qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _breachReturnPoint;
_requestFencePoint(++_currentFencePoint);
} else if (fencePoint.idx < _cReadFencePoints - 1) {
QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng);
_polygon.append(polyCoord);
qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord;
if (fencePoint.idx < _cReadFencePoints - 2) {
// Still more points to request
_requestFencePoint(++_currentFencePoint);
} else {
// We've finished collecting fence points
qCDebug(GeoFenceManagerLog) << "Fence point load complete";
_readTransactionInProgress = false;
emit loadComplete(_breachReturnPoint, _polygon);
emit inProgressChanged(inProgress());
}
}
}
}
void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex;
mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
QGeoCoordinate fenceCoord;
if (pointIndex == 0) {
fenceCoord = breachReturnPoint();
} else if (pointIndex == _cWriteFencePoints - 1) {
// Polygon close point
fenceCoord = _polygon[0];
} else {
// Polygon point
fenceCoord = _polygon[pointIndex - 1];
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
uint8_t totalPointCount = _polygon.count() + 1 + 1;
mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex, // Index of point to set
totalPointCount,
fenceCoord.latitude(),
fenceCoord.longitude());
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
bool APMGeoFenceManager::inProgress(void) const
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
void APMGeoFenceManager::_setCircleEnabled(bool circleEnabled)
{
if (circleEnabled != _circleEnabled) {
_circleEnabled = circleEnabled;
emit circleEnabledChanged(circleEnabled);
}
}
void APMGeoFenceManager::_setPolygonEnabled(bool polygonEnabled)
{
if (polygonEnabled != _polygonEnabled) {
_polygonEnabled = polygonEnabled;
emit polygonEnabledChanged(polygonEnabled);
}
}
void APMGeoFenceManager::_updateEnabledFlags(void)
{
bool fenceEnabled = false;
if (_fenceSupported) {
if (_fenceEnableFact) {
fenceEnabled = _fenceEnableFact->rawValue().toBool();
} else {
fenceEnabled = true;
}
bool newCircleEnabled = fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
_setCircleEnabled(newCircleEnabled);
bool newPolygonEnabled = _fenceSupported && fenceEnabled &&
((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) ||
_vehicle->fixedWing());
_setPolygonEnabled(newPolygonEnabled);
} else {
_setCircleEnabled(false);
_setPolygonEnabled(false);
}
}
void APMGeoFenceManager::_parametersReady(void)
{
if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true;
_fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) &&
_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam) &&
!qgcApp()->runningUnitTests();
if (_fenceSupported) {
QStringList paramNames;
QStringList paramLabels;
_polygonSupported = true;
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
_fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam);
connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
}
if (_vehicle->multiRotor()) {
_breachReturnSupported = false;
_fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
_circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
paramNames << QStringLiteral("FENCE_ENABLE") << QStringLiteral("FENCE_TYPE") << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_ALT_MAX")
<< QStringLiteral("FENCE_RADIUS") << QStringLiteral("FENCE_MARGIN");
paramLabels << QStringLiteral("Enabled:") << QStringLiteral("Type:") << QStringLiteral("Breach Action:") << QStringLiteral("Max Altitude:")
<< QStringLiteral("Radius:") << QStringLiteral("Margin:");
} if (_vehicle->fixedWing()) {
_breachReturnSupported = true;
paramNames << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_MINALT") << QStringLiteral("FENCE_MAXALT") << QStringLiteral("FENCE_RETALT")
<< QStringLiteral("FENCE_AUTOENABLE") << QStringLiteral("FENCE_RET_RALLY");
paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Min Altitude:") << QStringLiteral("Max Altitude:") << QStringLiteral("Return Altitude:")
<< QStringLiteral("Auto-Enable:") << QStringLiteral("Return to Rally:");
}
_params.clear();
_paramLabels.clear();
for (int i=0; i<paramNames.count(); i++) {
QString paramName = paramNames[i];
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
_params << QVariant::fromValue(paramFact);
_paramLabels << paramLabels[i];
}
}
emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels);
emit breachReturnSupportedChanged(_breachReturnSupported);
emit polygonSupportedChanged(_polygonSupported);
}
_updateEnabledFlags();
}
}
void APMGeoFenceManager::removeAll(void)
{
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll";
QmlObjectListModel emptyPolygon;
sendToVehicle(_breachReturnPoint, emptyPolygon);
emit removeAllComplete(false /* error */);
}
src/FirmwarePlugin/APM/APMGeoFenceManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMGeoFenceManager_H
#define APMGeoFenceManager_H
#include "GeoFenceManager.h"
#include "QGCMAVLink.h"
#include "FactSystem.h"
class QmlObjectListModel;
class APMGeoFenceManager : public GeoFenceManager
{
Q_OBJECT
public:
APMGeoFenceManager(Vehicle* vehicle);
~APMGeoFenceManager();
// Overrides from GeoFenceManager
bool inProgress (void) const final;
void loadFromVehicle (void) final;
void sendToVehicle (const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) final;
bool polygonSupported (void) const final { return _polygonSupported; }
bool polygonEnabled (void) const final { return _polygonEnabled; }
bool breachReturnSupported (void) const final { return _breachReturnSupported; }
bool circleEnabled (void) const { return _circleEnabled; }
Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
QVariantList params (void) const final { return _params; }
QStringList paramLabels (void) const final { return _paramLabels; }
void removeAll (void) final;
private slots:
void _mavlinkMessageReceived (const mavlink_message_t& message);
void _parametersReady (void);
private:
void _requestFencePoint (uint8_t pointIndex);
void _sendFencePoint (uint8_t pointIndex);
void _updateEnabledFlags(void);
void _setCircleEnabled (bool circleEnabled);
void _setPolygonEnabled (bool polygonEnabled);
private:
bool _fenceSupported;
bool _circleEnabled;
bool _polygonSupported;
bool _polygonEnabled;
bool _breachReturnSupported;
bool _firstParamLoadComplete;
QVariantList _params;
QStringList _paramLabels;
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
uint8_t _cReadFencePoints;
uint8_t _cWriteFencePoints;
uint8_t _currentFencePoint;
Fact* _fenceTypeFact;
Fact* _fenceEnableFact;
Fact* _circleRadiusFact;
static const char* _fenceTotalParam;
static const char* _fenceActionParam;
static const char* _fenceEnableParam;
};
#endif
src/FirmwarePlugin/APM/APMRallyPointManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMRallyPointManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL";
APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle)
: RallyPointManager(vehicle)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived);
}
APMRallyPointManager::~APMRallyPointManager()
{
}
void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported()) {
return;
}
if (_readTransactionInProgress) {
_sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress."));
return;
}
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count());
// FIXME: No validation of correct point received
_rgPoints = rgPoints;
for (uint8_t index=0; index<_rgPoints.count(); index++) {
_sendRallyPoint(index);
}
emit sendComplete(false /* error */);
}
void APMRallyPointManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
return;
}
_rgPoints.clear();
if (!rallyPointsSupported()) {
emit loadComplete(QList<QGeoCoordinate>());
return;
}
_cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt();
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints;
if (_cReadRallyPoints == 0) {
emit loadComplete(_rgPoints);
return;
}
_currentRallyPoint = 0;
_readTransactionInProgress = true;
_requestRallyPoint(_currentRallyPoint);
}
/// Called when a new mavlink message for out vehicle is received
void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) {
mavlink_rally_point_t rallyPoint;
mavlink_msg_rally_point_decode(&message, &rallyPoint);
qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt;
if (rallyPoint.idx != _currentRallyPoint) {
// FIXME: Protocol out of whack
_readTransactionInProgress = false;
emit inProgressChanged(inProgress());
qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint;
return;
}
QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt);
_rgPoints.append(point);
if (rallyPoint.idx < _cReadRallyPoints - 1) {
// Still more points to request
_requestRallyPoint(++_currentRallyPoint);
} else {
// We've finished collecting rally points
qCDebug(RallyPointManagerLog) << "Rally point load complete";
_readTransactionInProgress = false;
emit loadComplete(_rgPoints);
emit inProgressChanged(inProgress());
}
}
}
void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex;
mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
QGeoCoordinate point = _rgPoints[pointIndex];
mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex,
_rgPoints.count(),
point.latitude() * 1e7,
point.longitude() * 1e7,
point.altitude(),
0, 0, 0); // break_alt, land_dir, flags
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
bool APMRallyPointManager::inProgress(void) const
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
bool APMRallyPointManager::rallyPointsSupported(void) const
{
return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam);
}
void APMRallyPointManager::removeAll(void)
{
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll";
QList<QGeoCoordinate> noRallyPoints;
sendToVehicle(noRallyPoints);
emit removeAllComplete(false /* error */);
}
src/FirmwarePlugin/APM/APMRallyPointManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMRallyPointManager_H
#define APMRallyPointManager_H
#include "RallyPointManager.h"
#include "QGCMAVLink.h"
class APMRallyPointManager : public RallyPointManager
{
Q_OBJECT
public:
APMRallyPointManager(Vehicle* vehicle);
~APMRallyPointManager();
// Overrides from RallyPointManager
bool inProgress (void) const final;
void loadFromVehicle (void) final;
void sendToVehicle (const QList<QGeoCoordinate>& rgPoints) final;
bool rallyPointsSupported (void) const final;
void removeAll (void);
QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); }
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
private:
void _requestRallyPoint(uint8_t pointIndex);
void _sendRallyPoint(uint8_t pointIndex);
private:
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
uint8_t _cReadRallyPoints;
uint8_t _currentRallyPoint;
static const char* _rallyTotalParam;
};
#endif
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ 537463c5
......@@ -278,12 +278,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
}
QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
{
Q_UNUSED(vehicle);
return QStringLiteral("FENCE_RADIUS");
}
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
if (vehicle->isOfflineEditingVehicle()) {
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @ 537463c5
......@@ -67,7 +67,6 @@ public:
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
bool multiRotorXConfig (Vehicle* vehicle) final;
QString geoFenceRadiusParam (Vehicle* vehicle) final;
QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Brake"); }
QString missionFlightMode (void) const override { return QString("Auto"); }
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 537463c5
......@@ -235,15 +235,6 @@ public:
/// @return true: X confiuration, false: Plus configuration
virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; }
/// Returns a newly created geofence manager for this vehicle.
virtual GeoFenceManager* newGeoFenceManager(Vehicle* vehicle) { return new GeoFenceManager(vehicle); }
/// Returns the parameter which holds the fence circle radius if supported.
virtual QString geoFenceRadiusParam(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
/// Returns a newly created rally point manager for this vehicle.
virtual RallyPointManager* newRallyPointManager(Vehicle* vehicle) { return new RallyPointManager(vehicle); }
/// Return the resource file which contains the set of params loaded for offline editing.
virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @ 537463c5
......@@ -17,7 +17,6 @@
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
#include "PX4ParameterMetaData.h"
#include "PX4GeoFenceManager.h"
class PX4FirmwarePlugin : public FirmwarePlugin
{
......@@ -62,7 +61,6 @@ public:
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override;
GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) override { return new PX4GeoFenceManager(vehicle); }
QString offlineEditingParamFile(Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/PX4/PX4.OfflineEditing.params"); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
......
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "PX4GeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
, _firstParamLoadComplete(false)
, _circleRadiusFact(NULL)
{
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady);
if (_vehicle->parameterManager()->parametersReady()) {
_parametersReady();
}
}
PX4GeoFenceManager::~PX4GeoFenceManager()
{
}
void PX4GeoFenceManager::_parametersReady(void)
{
if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true;
_circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST"));
emit circleRadiusFactChanged(_circleRadiusFact);
QStringList paramNames;
QStringList paramLabels;
paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST");
paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:");
_params.clear();
_paramLabels.clear();
for (int i=0; i<paramNames.count(); i++) {
QString paramName = paramNames[i];
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
_params << QVariant::fromValue(paramFact);
_paramLabels << paramLabels[i];
}
}
emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels);
}
}
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef PX4GeoFenceManager_H
#define PX4GeoFenceManager_H
#include "GeoFenceManager.h"
#include "QGCMAVLink.h"
#include "FactSystem.h"
class PX4GeoFenceManager : public GeoFenceManager
{
Q_OBJECT
public:
PX4GeoFenceManager(Vehicle* vehicle);
~PX4GeoFenceManager();
// Overrides from GeoFenceManager
bool circleEnabled (void) const { return true; }
Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
QVariantList params (void) const final { return _params; }
QStringList paramLabels (void) const final { return _paramLabels; }
private slots:
void _parametersReady(void);
private:
bool _firstParamLoadComplete;
Fact* _circleRadiusFact;
QVariantList _params;
QStringList _paramLabels;
};
#endif
src/FlightMap/qmldir
View file @ 537463c5
......@@ -29,6 +29,3 @@ MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml
PolygonEditor 1.0 PolygonEditor.qml
VehicleMapItem 1.0 VehicleMapItem.qml
# Editor controls
QGCMapPolygonControls 1.0 QGCMapPolygonControls.qml
src/MissionManager/GeoFenceController.cc
View file @ 537463c5
......@@ -39,11 +39,10 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q
: PlanElementController(masterController, parent)
, _geoFenceManager(_managerVehicle->geoFenceManager())
, _dirty(false)
, _mapPolygon(this)
, _itemsRequested(false)
{
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
managerVehicleChanged(_managerVehicle);
}
......@@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void)
{
emit breachReturnSupportedChanged(breachReturnSupported());
emit breachReturnPointChanged(breachReturnPoint());
emit circleEnabledChanged(circleEnabled());
emit circleRadiusFactChanged(circleRadiusFact());
emit polygonEnabledChanged(polygonEnabled());
emit polygonSupportedChanged(polygonSupported());
emit dirtyChanged(dirty());
emit paramsChanged(params());
emit paramLabelsChanged(paramLabels());
emit supportedChanged(supported());
}
void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_geoFenceManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle = NULL;
_geoFenceManager = NULL;
}
......@@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
}
_geoFenceManager = _managerVehicle->geoFenceManager();
connect(_geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged);
connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete);
connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete);
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
emit supportedChanged(supported());
_signalAll();
}
bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
{
Q_UNUSED(json);
Q_UNUSED(errorString);
#if 0
QString errorStr;
QString errorMessage = tr("GeoFence: %1");
......@@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
setDirty(false);
_signalAll();
#endif
return true;
}
void GeoFenceController::save(QJsonObject& json)
{
Q_UNUSED(json);
#if 0
json[JsonHelper::jsonVersionKey] = 1;
if (_breachReturnPoint.isValid()) {
......@@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json)
}
_mapPolygon.saveToJson(json);
#endif
}
void GeoFenceController::removeAll(void)
{
{
setBreachReturnPoint(QGeoCoordinate());
_mapPolygon.clear();
_polygons.clearAndDeleteContents();
_circles.clearAndDeleteContents();
}
void GeoFenceController::removeAllFromVehicle(void)
......@@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void)
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
} else {
qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle";
_geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
_mapPolygon.setDirty(false);
_geoFenceManager->sendToVehicle(_breachReturnPoint, _polygons, _circles);
setDirty(false);
}
}
......@@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty)
if (dirty != _dirty) {
_dirty = dirty;
if (!dirty) {
_mapPolygon.setDirty(dirty);
for (int i=0; i<_polygons.count(); i++) {
QGCFencePolygon* polygon = _polygons.value<QGCFencePolygon*>(i);
polygon->setDirty(false);
}
for (int i=0; i<_circles.count(); i++) {
QGCFenceCircle* circle = _circles.value<QGCFenceCircle*>(i);
circle->setDirty(false);
}
}
emit dirtyChanged(dirty);
}
......@@ -224,53 +231,26 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty)
}
}
bool GeoFenceController::breachReturnSupported(void) const
{
return _geoFenceManager->breachReturnSupported();
}
bool GeoFenceController::circleEnabled(void) const
{
return _geoFenceManager->circleEnabled();
}
Fact* GeoFenceController::circleRadiusFact(void) const
{
return _geoFenceManager->circleRadiusFact();
}
bool GeoFenceController::polygonSupported(void) const
{
return _geoFenceManager->polygonSupported();
}
bool GeoFenceController::polygonEnabled(void) const
{
return _geoFenceManager->polygonEnabled();
}
QVariantList GeoFenceController::params(void) const
{
return _geoFenceManager->params();
}
QStringList GeoFenceController::paramLabels(void) const
{
return _geoFenceManager->paramLabels();
}
void GeoFenceController::_setDirty(void)
{
setDirty(true);
}
void GeoFenceController::_setPolygonFromManager(const QList<QGeoCoordinate>& polygon)
void GeoFenceController::_setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles)
{
_mapPolygon.clear();
for (int i=0; i<polygon.count(); i++) {
_mapPolygon.appendVertex(polygon[i]);
_polygons.clearAndDeleteContents();
_circles.clearAndDeleteContents();
for (int i=0; i<polygons.count(); i++) {
_polygons.append(new QGCFencePolygon(polygons[i], this));
}
_mapPolygon.setDirty(false);
for (int i=0; i<circles.count(); i++) {
_circles.append(new QGCFenceCircle(circles[i], this));
}
setDirty(false);
}
void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnPoint)
......@@ -279,13 +259,13 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
emit breachReturnPointChanged(_breachReturnPoint);
}
void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
void GeoFenceController::_managerLoadComplete(void)
{
// Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) {
_setReturnPointFromManager(breachReturn);
_setPolygonFromManager(polygon);
_setReturnPointFromManager(_geoFenceManager->breachReturnPoint());
_setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles());
setDirty(false);
_signalAll();
emit loadComplete();
......@@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error)
bool GeoFenceController::containsItems(void) const
{
return _mapPolygon.count() > 2;
return _polygons.count() > 0 || _circles.count() > 0;
}
void GeoFenceController::_updateContainsItems(void)
......@@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void)
// Fake a _loadComplete with the current items
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true;
_managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon());
_managerLoadComplete();
return false;
}
}
}
void GeoFenceController::addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
{
QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
// Initial polygon is inset to take 3/4s of viewport with max width/height of 3000 meters
halfWidthMeters = qMin(halfWidthMeters * 0.75, 1500.0);
halfHeightMeters = qMin(halfHeightMeters * 0.75, 1500.0);
// Initial polygon has max width and height of 3000 meters
topLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0);
topRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0);
bottomLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180);
bottomRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180);
QGCFencePolygon* polygon = new QGCFencePolygon(true /* inclusion */, this);
polygon->appendVertex(topLeft);
polygon->appendVertex(topRight);
polygon->appendVertex(bottomRight);
polygon->appendVertex(bottomLeft);
_polygons.append(polygon);
clearAllInteractive();
polygon->setInteractive(true);
}
void GeoFenceController::addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
{
QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
// Initial radius is inset to take 3/4s of viewport and max of 1500 meters
double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
double radius = qMin(qMin(halfWidthMeters, halfHeightMeters) * 0.75, 1500.0);
QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
QGCFenceCircle* circle = new QGCFenceCircle(center, radius, true /* inclusion */, this);
_circles.append(circle);
clearAllInteractive();
circle->setInteractive(true);
}
void GeoFenceController::deletePolygon(int index)
{
if (index < 0 || index > _polygons.count() - 1) {
return;
}
QGCFencePolygon* polygon = qobject_cast<QGCFencePolygon*>(_polygons.removeAt(index));
polygon->deleteLater();
}
void GeoFenceController::deleteCircle(int index)
{
if (index < 0 || index > _circles.count() - 1) {
return;
}
QGCFenceCircle* circle = qobject_cast<QGCFenceCircle*>(_circles.removeAt(index));
circle->deleteLater();
}
void GeoFenceController::clearAllInteractive(void)
{
for (int i=0; i<_polygons.count(); i++) {
_polygons.value<QGCFencePolygon*>(i)->setInteractive(false);
}
for (int i=0; i<_circles.count(); i++) {
_circles.value<QGCFenceCircle*>(i)->setInteractive(false);
}
}
bool GeoFenceController::supported(void) const
{
return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
}
src/MissionManager/GeoFenceController.h
View file @ 537463c5
......@@ -12,7 +12,8 @@
#include "PlanElementController.h"
#include "GeoFenceManager.h"
#include "QGCMapPolygon.h"
#include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
#include "Vehicle.h"
#include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h"
......@@ -29,21 +30,32 @@ public:
GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL);
~GeoFenceController();
Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT)
Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
// The following properties are reflections of properties from GeoFenceManager
Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged)
Q_PROPERTY(Fact* circleRadiusFact READ circleRadiusFact NOTIFY circleRadiusFactChanged)
Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged)
Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged)
/// Add a new inclusion polygon to the fence
/// @param topLeft - Top left coordinate or map viewport
/// @param topLeft - Bottom right left coordinate or map viewport
Q_INVOKABLE void addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
Q_INVOKABLE void addPolygon (void) { emit addInitialFencePolygon(); }
Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); }
/// Add a new inclusion circle to the fence
/// @param topLeft - Top left coordinate or map viewport
/// @param topLeft - Bottom right left coordinate or map viewport
Q_INVOKABLE void addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
/// Deletes the specified polygon from the polygon list
/// @param index Index of poygon to delete
Q_INVOKABLE void deletePolygon(int index);
/// Deletes the specified circle from the circle list
/// @param index Index of circle to delete
Q_INVOKABLE void deleteCircle(int index);
/// Clears the interactive bit from all fence items
Q_INVOKABLE void clearAllInteractive(void);
bool supported (void) const final;
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
......@@ -58,15 +70,9 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
bool circleEnabled (void) const;
Fact* circleRadiusFact (void) const;
bool polygonSupported (void) const;
bool polygonEnabled (void) const;
bool breachReturnSupported (void) const;
QVariantList params (void) const;
QStringList paramLabels (void) const;
QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
QmlObjectListModel* polygons (void) { return &_polygons; }
QmlObjectListModel* circles (void) { return &_circles; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
......@@ -74,21 +80,14 @@ signals:
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void editorQmlChanged (QString editorQml);
void loadComplete (void);
void addInitialFencePolygon (void);
void circleEnabledChanged (bool circleEnabled);
void circleRadiusFactChanged (Fact* circleRadiusFact);
void polygonSupportedChanged (bool polygonSupported);
void polygonEnabledChanged (bool polygonEnabled);
void breachReturnSupportedChanged (bool breachReturnSupported);
void paramsChanged (QVariantList params);
void paramLabelsChanged (QStringList paramLabels);
private slots:
void _polygonDirtyChanged(bool dirty);
void _setDirty(void);
void _setPolygonFromManager(const QList<QGeoCoordinate>& polygon);
void _setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles);
void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void _managerLoadComplete(void);
void _updateContainsItems(void);
void _managerSendComplete(bool error);
void _managerRemoveAllComplete(bool error);
......@@ -99,7 +98,8 @@ private:
GeoFenceManager* _geoFenceManager;
bool _dirty;
QGCMapPolygon _mapPolygon;
QmlObjectListModel _polygons;
QmlObjectListModel _circles;
QGeoCoordinate _breachReturnPoint;
bool _itemsRequested;
......
src/MissionManager/GeoFenceManager.cc
View file @ 537463c5
......@@ -10,14 +10,22 @@
#include "GeoFenceManager.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "ParameterManager.h"
#include "QGCMapPolygon.h"
#include "QGCMapCircle.h"
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog")
GeoFenceManager::GeoFenceManager(Vehicle* vehicle)
: QObject(vehicle)
, _vehicle(vehicle)
: _vehicle (vehicle)
, _planManager (vehicle, MAV_MISSION_TYPE_FENCE)
, _firstParamLoadComplete (false)
{
connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged);
connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error);
connect(&_planManager, &PlanManager::removeAllComplete, this, &GeoFenceManager::removeAllComplete);
connect(&_planManager, &PlanManager::sendComplete, this, &GeoFenceManager::_sendComplete);
connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &GeoFenceManager::_planManagerLoadComplete);
}
GeoFenceManager::~GeoFenceManager()
......@@ -25,30 +33,161 @@ GeoFenceManager::~GeoFenceManager()
}
void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
bool GeoFenceManager::inProgress(void) const
{
qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg;
emit error(errorCode, errorMsg);
return _planManager.inProgress();
}
void GeoFenceManager::loadFromVehicle(void)
{
// No geofence support in unknown vehicle
emit loadComplete(QGeoCoordinate(), QList<QGeoCoordinate>());
_planManager.loadFromVehicle();
}
void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
QmlObjectListModel& polygons,
QmlObjectListModel& circles)
{
// No geofence support in unknown vehicle
Q_UNUSED(breachReturn);
Q_UNUSED(polygon);
emit sendComplete(false /* error */);
}
QList<MissionItem*> fenceItems;
_sendPolygons.clear();
_sendCircles.clear();
for (int i=0; i<polygons.count(); i++) {
_sendPolygons.append(*polygons.value<QGCFencePolygon*>(i));
}
for (int i=0; i<circles.count(); i++) {
_sendCircles.append(*circles.value<QGCFenceCircle*>(i));
}
for (int i=0; i<_sendPolygons.count(); i++) {
const QGCFencePolygon& polygon = _sendPolygons[i];
for (int j=0; j<polygon.count(); j++) {
const QGeoCoordinate& vertex = polygon.path()[j].value<QGeoCoordinate>();
MissionItem* item = new MissionItem(0,
polygon.inclusion() ? MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION : MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
MAV_FRAME_GLOBAL,
polygon.count(), // vertex count
0, 0, 0, // param 2-4 unused
vertex.latitude(),
vertex.longitude(),
0, // param 7 unused
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
}
for (int i=0; i<_sendCircles.count(); i++) {
QGCFenceCircle& circle = _sendCircles[i];
MissionItem* item = new MissionItem(0,
circle.inclusion() ? MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION : MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
MAV_FRAME_GLOBAL,
circle.radius()->rawValue().toDouble(),
0, 0, 0, // param 2-4 unused
circle.center().latitude(),
circle.center().longitude(),
0, // param 7 unused
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
_planManager.writeMissionItems(fenceItems);
for (int i=0; i<fenceItems.count(); i++) {
fenceItems[i]->deleteLater();
}
}
void GeoFenceManager::removeAll(void)
{
// No geofence support in unknown vehicle
emit removeAllComplete(false /* error */);
_planManager.removeAll();
}
void GeoFenceManager::_sendComplete(bool error)
{
if (error) {
_polygons.clear();
_circles.clear();
} else {
_polygons = _sendPolygons;
_circles = _sendCircles;
}
_sendPolygons.clear();
_sendCircles.clear();
emit sendComplete(error);
}
void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
{
bool loadFailed = false;
Q_UNUSED(removeAllRequested);
_polygons.clear();
_circles.clear();
MAV_CMD expectedCommand = (MAV_CMD)0;
int expectedVertexCount = 0;
QGCFencePolygon nextPolygon(true /* inclusion */);
const QList<MissionItem*>& fenceItems = _planManager.missionItems();
for (int i=0; i<fenceItems.count(); i++) {
MissionItem* item = fenceItems[i];
MAV_CMD command = item->command();
if (command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
if (nextPolygon.count() == 0) {
// Starting a new polygon
expectedVertexCount = item->param1();
expectedCommand = command;
} else if (expectedVertexCount != item->param1()){
// In the middle of a polygon, but count suddenly changed
emit error(BadPolygonItemFormat, tr("GeoFence load: Vertex count change mid-polygon - actual:expected").arg(item->param1()).arg(expectedVertexCount));
break;
} if (expectedCommand != command) {
// Command changed before last polygon was completely loaded
emit error(BadPolygonItemFormat, tr("GeoFence load: Polygon type changed before last load complete - actual:expected").arg(command).arg(expectedCommand));
break;
}
nextPolygon.appendVertex(QGeoCoordinate(item->param5(), item->param6()));
if (nextPolygon.count() == expectedVertexCount) {
// Polygon is complete
nextPolygon.setInclusion(command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION);
_polygons.append(nextPolygon);
nextPolygon.clear();
}
} else if (command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || command == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
if (nextPolygon.count() != 0) {
// Incomplete polygon
emit error(IncompletePolygonLoad, tr("GeoFence load: Incomplete polygon loaded"));
break;
}
QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */);
_circles.append(circle);
} else {
emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command()));
break;
}
}
if (loadFailed) {
_polygons.clear();
_circles.clear();
}
emit loadComplete();
}
bool GeoFenceManager::supported(void) const
{
return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200);
}
src/MissionManager/GeoFenceManager.h
View file @ 537463c5
......@@ -15,9 +15,13 @@
#include "QGCLoggingCategory.h"
#include "FactSystem.h"
#include "PlanManager.h"
#include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
class Vehicle;
class QmlObjectListModel;
class PlanManager;
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog)
......@@ -31,80 +35,67 @@ public:
GeoFenceManager(Vehicle* vehicle);
~GeoFenceManager();
/// Returns true if GeoFence is supported by this vehicle
virtual bool supported(void) const;
/// Returns true if the manager is currently communicating with the vehicle
virtual bool inProgress(void) const { return false; }
virtual bool inProgress(void) const;
/// Load the current settings from the vehicle
/// Signals loadComplete when done
virtual void loadFromVehicle(void);
/// Send the current settings to the vehicle
/// Send the geofence settings to the vehicle
/// Signals sendComplete when done
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon);
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, ///< Breach return point
QmlObjectListModel& polygons, ///< List of QGCFencePolygons
QmlObjectListModel& circles); ///< List of QGCFenceCircles
/// Remove all fence related items from vehicle (does not affect parameters)
/// Signals removeAllComplete when done
virtual void removeAll(void);
/// Returns true if this vehicle support polygon fence
/// Signal: polygonSupportedChanged
virtual bool polygonSupported(void) const { return false; }
/// Returns true if polygon fence is currently enabled on this vehicle
/// Signal: polygonEnabledChanged
virtual bool polygonEnabled(void) const { return false; }
/// Returns true if breach return is supported by this vehicle
/// Signal: breachReturnSupportedChanged
virtual bool breachReturnSupported(void) const { return false; }
/// Returns a list of parameter facts that relate to geofence support for the vehicle
/// Signal: paramsChanged
virtual QVariantList params(void) const { return QVariantList(); }
virtual bool polygonEnabled(void) const { return true; }
/// Returns the user visible labels for the paremeters returned by params method
/// Signal: paramLabelsChanged
virtual QStringList paramLabels(void) const { return QStringList(); }
/// Returns true if circular fence is currently enabled on vehicle
/// Signal: circleEnabledChanged
virtual bool circleEnabled(void) const { return false; }
/// Returns the fact which controls the fence circle radius. NULL if not supported
/// Signal: circleRadiusFactChanged
virtual Fact* circleRadiusFact(void) const { return NULL; }
QList<QGeoCoordinate> polygon (void) const { return _polygon; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
const QList<QGCFencePolygon>& polygons(void) { return _polygons; }
const QList<QGCFenceCircle>& circles(void) { return _circles; }
const QGeoCoordinate& breachReturnPoint(void) const { return _breachReturnPoint; }
/// Error codes returned in error signal
typedef enum {
InternalError,
TooFewPoints, ///< Too few points for valid geofence
TooManyPoints, ///< Too many points for valid geofence
PolygonTooFewPoints, ///< Too few points for valid fence polygon
PolygonTooManyPoints, ///< Too many points for valid fence polygon
IncompletePolygonLoad, ///< Incomplete polygon loaded
UnsupportedCommand, ///< Usupported command in mission type
BadPolygonItemFormat, ///< Error re-creating polygons from mission items
InvalidCircleRadius,
} ErrorCode_t;
signals:
void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void loadComplete (void);
void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg);
void paramsChanged (QVariantList params);
void paramLabelsChanged (QStringList paramLabels);
void circleEnabledChanged (bool circleEnabled);
void circleRadiusFactChanged (Fact* circleRadiusFact);
void polygonSupportedChanged (bool polygonSupported);
void polygonEnabledChanged (bool polygonEnabled);
void breachReturnSupportedChanged (bool breachReturnSupported);
void removeAllComplete (bool error);
void sendComplete (bool error);
protected:
private slots:
void _sendComplete (bool error);
void _planManagerLoadComplete (bool removeAllRequested);
private:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Vehicle* _vehicle;
QList<QGeoCoordinate> _polygon;
PlanManager _planManager;
QList<QGCFencePolygon> _polygons;
QList<QGCFenceCircle> _circles;
QGeoCoordinate _breachReturnPoint;
bool _firstParamLoadComplete;
QList<QGCFencePolygon> _sendPolygons;
QList<QGCFenceCircle> _sendCircles;
};
#endif
src/MissionManager/MissionController.h
View file @ 537463c5
......@@ -110,6 +110,7 @@ public:
bool loadTextFile(QFile& file, QString& errorString);
// Overrides from PlanElementController
bool supported (void) const final { return true; };
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
......
src/MissionManager/MissionManager.cc
View file @ 537463c5
This diff is collapsed.
Supports Markdown
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