Skip to content

Q
qgroundcontrol
Commit 537463c5 authored by Don Gagne's avatar Don Gagne
Browse files
Options

New Mavlink 2 spec GeoFence and Rally Point support

parent a11b1dbf
Hide whitespace changes
Inline Side-by-side
Showing with 2806 additions and 2228 deletions
qgroundcontrol.pro
View file @ 537463c5
...@@ -509,7 +509,11 @@ HEADERS += \ ...@@ -509,7 +509,11 @@ HEADERS += \
src/MissionManager/MissionManager.h \ src/MissionManager/MissionManager.h \
src/MissionManager/MissionSettingsItem.h \ src/MissionManager/MissionSettingsItem.h \
src/MissionManager/PlanElementController.h \ src/MissionManager/PlanElementController.h \
src/MissionManager/PlanManager.h \
src/MissionManager/PlanMasterController.h \ src/MissionManager/PlanMasterController.h \
src/MissionManager/QGCFenceCircle.h \
src/MissionManager/QGCFencePolygon.h \
src/MissionManager/QGCMapCircle.h \
src/MissionManager/QGCMapPolygon.h \ src/MissionManager/QGCMapPolygon.h \
src/MissionManager/RallyPoint.h \ src/MissionManager/RallyPoint.h \
src/MissionManager/RallyPointController.h \ src/MissionManager/RallyPointController.h \
...@@ -689,7 +693,11 @@ SOURCES += \ ...@@ -689,7 +693,11 @@ SOURCES += \
src/MissionManager/MissionManager.cc \ src/MissionManager/MissionManager.cc \
src/MissionManager/MissionSettingsItem.cc \ src/MissionManager/MissionSettingsItem.cc \
src/MissionManager/PlanElementController.cc \ src/MissionManager/PlanElementController.cc \
src/MissionManager/PlanManager.cc \
src/MissionManager/PlanMasterController.cc \ src/MissionManager/PlanMasterController.cc \
src/MissionManager/QGCFenceCircle.cc \
src/MissionManager/QGCFencePolygon.cc \
src/MissionManager/QGCMapCircle.cc \
src/MissionManager/QGCMapPolygon.cc \ src/MissionManager/QGCMapPolygon.cc \
src/MissionManager/RallyPoint.cc \ src/MissionManager/RallyPoint.cc \
src/MissionManager/RallyPointController.cc \ src/MissionManager/RallyPointController.cc \
...@@ -906,9 +914,7 @@ APMFirmwarePlugin { ...@@ -906,9 +914,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.h \ src/AutoPilotPlugins/APM/APMSensorsComponentController.h \
src/AutoPilotPlugins/APM/APMTuningComponent.h \ src/AutoPilotPlugins/APM/APMTuningComponent.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \ src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \ src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/APMRallyPointManager.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \
...@@ -933,9 +939,7 @@ APMFirmwarePlugin { ...@@ -933,9 +939,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \ src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \
src/AutoPilotPlugins/APM/APMTuningComponent.cc \ src/AutoPilotPlugins/APM/APMTuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \ src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/APMRallyPointManager.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \
...@@ -974,7 +978,6 @@ PX4FirmwarePlugin { ...@@ -974,7 +978,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponent.h \
src/AutoPilotPlugins/PX4/SensorsComponentController.h \ src/AutoPilotPlugins/PX4/SensorsComponentController.h \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
SOURCES += \ SOURCES += \
...@@ -995,7 +998,6 @@ PX4FirmwarePlugin { ...@@ -995,7 +998,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponent.cc \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \
} }
......
qgroundcontrol.qrc
View file @ 537463c5
...@@ -90,6 +90,7 @@ ...@@ -90,6 +90,7 @@
<file alias="QGroundControl/Controls/QGCLabel.qml">src/QmlControls/QGCLabel.qml</file> <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/QGCListView.qml">src/QmlControls/QGCListView.qml</file>
<file alias="QGroundControl/Controls/QGCMapLabel.qml">src/QmlControls/QGCMapLabel.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/QGCMapPolygonVisuals.qml">src/MissionManager/QGCMapPolygonVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file> <file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file> <file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
...@@ -199,6 +200,7 @@ ...@@ -199,6 +200,7 @@
<file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file> <file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
<file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.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="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="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.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> <file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 537463c5
...@@ -17,8 +17,6 @@ ...@@ -17,8 +17,6 @@
#include "FirmwarePlugin.h" #include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h" #include "APMParameterMetaData.h"
#include "APMGeoFenceManager.h"
#include "APMRallyPointManager.h"
#include <QAbstractSocket> #include <QAbstractSocket>
...@@ -93,8 +91,6 @@ public: ...@@ -93,8 +91,6 @@ public:
QString internalParameterMetaDataFile (Vehicle* vehicle) final; QString internalParameterMetaDataFile (Vehicle* vehicle) final;
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final; 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 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"); } 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) ...@@ -278,12 +278,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0; 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 bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{ {
if (vehicle->isOfflineEditingVehicle()) { if (vehicle->isOfflineEditingVehicle()) {
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @ 537463c5
...@@ -67,7 +67,6 @@ public: ...@@ -67,7 +67,6 @@ public:
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final; bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
bool multiRotorXConfig (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 offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Brake"); } QString pauseFlightMode (void) const override { return QString("Brake"); }
QString missionFlightMode (void) const override { return QString("Auto"); } QString missionFlightMode (void) const override { return QString("Auto"); }
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 537463c5
...@@ -235,15 +235,6 @@ public: ...@@ -235,15 +235,6 @@ public:
/// @return true: X confiuration, false: Plus configuration /// @return true: X confiuration, false: Plus configuration
virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; } 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. /// Return the resource file which contains the set of params loaded for offline editing.
virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); } virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @ 537463c5
...@@ -17,7 +17,6 @@ ...@@ -17,7 +17,6 @@
#include "FirmwarePlugin.h" #include "FirmwarePlugin.h"
#include "ParameterManager.h" #include "ParameterManager.h"
#include "PX4ParameterMetaData.h" #include "PX4ParameterMetaData.h"
#include "PX4GeoFenceManager.h"
class PX4FirmwarePlugin : public FirmwarePlugin class PX4FirmwarePlugin : public FirmwarePlugin
{ {
...@@ -62,7 +61,6 @@ public: ...@@ -62,7 +61,6 @@ public:
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final; QObject* loadParameterMetaData (const QString& metaDataFile) final;
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; 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 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 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"); } 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 ...@@ -29,6 +29,3 @@ MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml MissionLineView 1.0 MissionLineView.qml
PolygonEditor 1.0 PolygonEditor.qml PolygonEditor 1.0 PolygonEditor.qml
VehicleMapItem 1.0 VehicleMapItem.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 ...@@ -39,11 +39,10 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q
: PlanElementController(masterController, parent) : PlanElementController(masterController, parent)
, _geoFenceManager(_managerVehicle->geoFenceManager()) , _geoFenceManager(_managerVehicle->geoFenceManager())
, _dirty(false) , _dirty(false)
, _mapPolygon(this)
, _itemsRequested(false) , _itemsRequested(false)
{ {
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
managerVehicleChanged(_managerVehicle); managerVehicleChanged(_managerVehicle);
} }
...@@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn ...@@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void) void GeoFenceController::_signalAll(void)
{ {
emit breachReturnSupportedChanged(breachReturnSupported());
emit breachReturnPointChanged(breachReturnPoint()); emit breachReturnPointChanged(breachReturnPoint());
emit circleEnabledChanged(circleEnabled());
emit circleRadiusFactChanged(circleRadiusFact());
emit polygonEnabledChanged(polygonEnabled());
emit polygonSupportedChanged(polygonSupported());
emit dirtyChanged(dirty()); emit dirtyChanged(dirty());
emit paramsChanged(params()); emit supportedChanged(supported());
emit paramLabelsChanged(paramLabels());
} }
void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
{ {
if (_managerVehicle) { if (_managerVehicle) {
_geoFenceManager->disconnect(this); _geoFenceManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle = NULL; _managerVehicle = NULL;
_geoFenceManager = NULL; _geoFenceManager = NULL;
} }
...@@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) ...@@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
} }
_geoFenceManager = _managerVehicle->geoFenceManager(); _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::loadComplete, this, &GeoFenceController::_managerLoadComplete);
connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete); connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete);
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete); connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
emit supportedChanged(supported());
_signalAll(); _signalAll();
} }
bool GeoFenceController::load(const QJsonObject& json, QString& errorString) bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
{ {
Q_UNUSED(json);
Q_UNUSED(errorString);
#if 0
QString errorStr; QString errorStr;
QString errorMessage = tr("GeoFence: %1"); QString errorMessage = tr("GeoFence: %1");
...@@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) ...@@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
setDirty(false); setDirty(false);
_signalAll(); _signalAll();
#endif
return true; return true;
} }
void GeoFenceController::save(QJsonObject& json) void GeoFenceController::save(QJsonObject& json)
{ {
Q_UNUSED(json);
#if 0
json[JsonHelper::jsonVersionKey] = 1; json[JsonHelper::jsonVersionKey] = 1;
if (_breachReturnPoint.isValid()) { if (_breachReturnPoint.isValid()) {
...@@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json) ...@@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json)
} }
_mapPolygon.saveToJson(json); _mapPolygon.saveToJson(json);
#endif
} }
void GeoFenceController::removeAll(void) void GeoFenceController::removeAll(void)
{ {
setBreachReturnPoint(QGeoCoordinate()); setBreachReturnPoint(QGeoCoordinate());
_mapPolygon.clear(); _polygons.clearAndDeleteContents();
_circles.clearAndDeleteContents();
} }
void GeoFenceController::removeAllFromVehicle(void) void GeoFenceController::removeAllFromVehicle(void)
...@@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void) ...@@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void)
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress"; qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
} else { } else {
qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle"; qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle";
_geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); _geoFenceManager->sendToVehicle(_breachReturnPoint, _polygons, _circles);
_mapPolygon.setDirty(false);
setDirty(false); setDirty(false);
} }
} }
...@@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty) ...@@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty)
if (dirty != _dirty) { if (dirty != _dirty) {
_dirty = dirty; _dirty = dirty;
if (!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); emit dirtyChanged(dirty);
} }
...@@ -224,53 +231,26 @@ void GeoFenceController::_polygonDirtyChanged(bool 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) void GeoFenceController::_setDirty(void)
{ {
setDirty(true); setDirty(true);
} }
void GeoFenceController::_setPolygonFromManager(const QList<QGeoCoordinate>& polygon) void GeoFenceController::_setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles)
{ {
_mapPolygon.clear(); _polygons.clearAndDeleteContents();
for (int i=0; i<polygon.count(); i++) { _circles.clearAndDeleteContents();
_mapPolygon.appendVertex(polygon[i]);
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) void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnPoint)
...@@ -279,13 +259,13 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP ...@@ -279,13 +259,13 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
emit breachReturnPointChanged(_breachReturnPoint); emit breachReturnPointChanged(_breachReturnPoint);
} }
void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon) void GeoFenceController::_managerLoadComplete(void)
{ {
// Fly view always reloads on _loadComplete // Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested // Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) { if (!_editMode || _itemsRequested) {
_setReturnPointFromManager(breachReturn); _setReturnPointFromManager(_geoFenceManager->breachReturnPoint());
_setPolygonFromManager(polygon); _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles());
setDirty(false); setDirty(false);
_signalAll(); _signalAll();
emit loadComplete(); emit loadComplete();
...@@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error) ...@@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error)
bool GeoFenceController::containsItems(void) const bool GeoFenceController::containsItems(void) const
{ {
return _mapPolygon.count() > 2; return _polygons.count() > 0 || _circles.count() > 0;
} }
void GeoFenceController::_updateContainsItems(void) void GeoFenceController::_updateContainsItems(void)
...@@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void) ...@@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void)
// Fake a _loadComplete with the current items // Fake a _loadComplete with the current items
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true; _itemsRequested = true;
_managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon()); _managerLoadComplete();
return false; 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 @@ ...@@ -12,7 +12,8 @@
#include "PlanElementController.h" #include "PlanElementController.h"
#include "GeoFenceManager.h" #include "GeoFenceManager.h"
#include "QGCMapPolygon.h" #include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
#include "Vehicle.h" #include "Vehicle.h"
#include "MultiVehicleManager.h" #include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
...@@ -29,21 +30,32 @@ public: ...@@ -29,21 +30,32 @@ public:
GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL); GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL);
~GeoFenceController(); ~GeoFenceController();
Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT) Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) 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 /// Add a new inclusion polygon to the fence
Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged) /// @param topLeft - Top left coordinate or map viewport
Q_PROPERTY(Fact* circleRadiusFact READ circleRadiusFact NOTIFY circleRadiusFactChanged) /// @param topLeft - Bottom right left coordinate or map viewport
Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) Q_INVOKABLE void addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
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)
Q_INVOKABLE void addPolygon (void) { emit addInitialFencePolygon(); } /// Add a new inclusion circle to the fence
Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); } /// @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 start (bool editMode) final;
void save (QJsonObject& json) final; void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final; bool load (const QJsonObject& json, QString& errorString) final;
...@@ -58,15 +70,9 @@ public: ...@@ -58,15 +70,9 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final; void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final; bool showPlanFromManagerVehicle (void) final;
bool circleEnabled (void) const; QmlObjectListModel* polygons (void) { return &_polygons; }
Fact* circleRadiusFact (void) const; QmlObjectListModel* circles (void) { return &_circles; }
bool polygonSupported (void) const; QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
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; }
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
...@@ -74,21 +80,14 @@ signals: ...@@ -74,21 +80,14 @@ signals:
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void editorQmlChanged (QString editorQml); void editorQmlChanged (QString editorQml);
void loadComplete (void); 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: private slots:
void _polygonDirtyChanged(bool dirty); void _polygonDirtyChanged(bool dirty);
void _setDirty(void); void _setDirty(void);
void _setPolygonFromManager(const QList<QGeoCoordinate>& polygon); void _setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles);
void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon); void _managerLoadComplete(void);
void _updateContainsItems(void); void _updateContainsItems(void);
void _managerSendComplete(bool error); void _managerSendComplete(bool error);
void _managerRemoveAllComplete(bool error); void _managerRemoveAllComplete(bool error);
...@@ -99,7 +98,8 @@ private: ...@@ -99,7 +98,8 @@ private:
GeoFenceManager* _geoFenceManager; GeoFenceManager* _geoFenceManager;
bool _dirty; bool _dirty;
QGCMapPolygon _mapPolygon; QmlObjectListModel _polygons;
QmlObjectListModel _circles;
QGeoCoordinate _breachReturnPoint; QGeoCoordinate _breachReturnPoint;
bool _itemsRequested; bool _itemsRequested;
......
src/MissionManager/GeoFenceManager.cc
View file @ 537463c5
...@@ -10,14 +10,22 @@ ...@@ -10,14 +10,22 @@
#include "GeoFenceManager.h" #include "GeoFenceManager.h"
#include "Vehicle.h" #include "Vehicle.h"
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include "ParameterManager.h"
#include "QGCMapPolygon.h"
#include "QGCMapCircle.h"
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog")
GeoFenceManager::GeoFenceManager(Vehicle* vehicle) 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() GeoFenceManager::~GeoFenceManager()
...@@ -25,30 +33,161 @@ 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; return _planManager.inProgress();
emit error(errorCode, errorMsg);
} }
void GeoFenceManager::loadFromVehicle(void) void GeoFenceManager::loadFromVehicle(void)
{ {
// No geofence support in unknown vehicle _planManager.loadFromVehicle();
emit loadComplete(QGeoCoordinate(), QList<QGeoCoordinate>());
} }
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(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) void GeoFenceManager::removeAll(void)
{ {
// No geofence support in unknown vehicle _planManager.removeAll();
emit removeAllComplete(false /* error */); }
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 @@ ...@@ -15,9 +15,13 @@
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "FactSystem.h" #include "FactSystem.h"
#include "PlanManager.h"
#include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
class Vehicle; class Vehicle;
class QmlObjectListModel; class QmlObjectListModel;
class PlanManager;
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog)
...@@ -31,80 +35,67 @@ public: ...@@ -31,80 +35,67 @@ public:
GeoFenceManager(Vehicle* vehicle); GeoFenceManager(Vehicle* vehicle);
~GeoFenceManager(); ~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 /// 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 /// Load the current settings from the vehicle
/// Signals loadComplete when done /// Signals loadComplete when done
virtual void loadFromVehicle(void); virtual void loadFromVehicle(void);
/// Send the current settings to the vehicle /// Send the geofence settings to the vehicle
/// Signals sendComplete when done /// 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) /// Remove all fence related items from vehicle (does not affect parameters)
/// Signals removeAllComplete when done /// Signals removeAllComplete when done
virtual void removeAll(void); 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 /// Returns true if polygon fence is currently enabled on this vehicle
/// Signal: polygonEnabledChanged /// Signal: polygonEnabledChanged
virtual bool polygonEnabled(void) const { return false; } virtual bool polygonEnabled(void) const { return true; }
/// 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(); }
/// Returns the user visible labels for the paremeters returned by params method const QList<QGCFencePolygon>& polygons(void) { return _polygons; }
/// Signal: paramLabelsChanged const QList<QGCFenceCircle>& circles(void) { return _circles; }
virtual QStringList paramLabels(void) const { return QStringList(); } const QGeoCoordinate& breachReturnPoint(void) const { return _breachReturnPoint; }
/// 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; }
/// Error codes returned in error signal /// Error codes returned in error signal
typedef enum { typedef enum {
InternalError, InternalError,
TooFewPoints, ///< Too few points for valid geofence PolygonTooFewPoints, ///< Too few points for valid fence polygon
TooManyPoints, ///< Too many points for valid geofence 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, InvalidCircleRadius,
} ErrorCode_t; } ErrorCode_t;
signals: signals:
void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon); void loadComplete (void);
void inProgressChanged (bool inProgress); void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg); 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 removeAllComplete (bool error);
void sendComplete (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); void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Vehicle* _vehicle; Vehicle* _vehicle;
QList<QGeoCoordinate> _polygon; PlanManager _planManager;
QList<QGCFencePolygon> _polygons;
QList<QGCFenceCircle> _circles;
QGeoCoordinate _breachReturnPoint; QGeoCoordinate _breachReturnPoint;
bool _firstParamLoadComplete;
QList<QGCFencePolygon> _sendPolygons;
QList<QGCFenceCircle> _sendCircles;
}; };
#endif #endif
src/MissionManager/MissionController.h
View file @ 537463c5
...@@ -110,6 +110,7 @@ public: ...@@ -110,6 +110,7 @@ public:
bool loadTextFile(QFile& file, QString& errorString); bool loadTextFile(QFile& file, QString& errorString);
// Overrides from PlanElementController // Overrides from PlanElementController
bool supported (void) const final { return true; };
void start (bool editMode) final; void start (bool editMode) final;
void save (QJsonObject& json) final; void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final; bool load (const QJsonObject& json, QString& errorString) final;
......
src/MissionManager/MissionManager.cc
View file @ 537463c5
...@@ -22,110 +22,15 @@ ...@@ -22,110 +22,15 @@
QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle) MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle) : PlanManager(vehicle, MAV_MISSION_TYPE_MISSION)
, _dedicatedLink(NULL)
, _ackTimeoutTimer(NULL)
, _expectedAck(AckNone)
, _transactionInProgress(TransactionNone)
, _resumeMission(false)
, _lastMissionRequest(-1)
, _currentMissionIndex(-1)
, _lastCurrentIndex(-1)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
_ackTimeoutTimer = new QTimer(this);
_ackTimeoutTimer->setSingleShot(true);
_ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout);
}
MissionManager::~MissionManager()
{ {
} }
void MissionManager::_writeMissionItemsWorker(void) MissionManager::~MissionManager()
{
_lastMissionRequest = -1;
emit progressPct(0);
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _writeMissionItems.count();
// Prime write list
_itemIndicesToWrite.clear();
for (int i=0; i<_writeMissionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_transactionInProgress = TransactionWrite;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
}
void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
if (inProgress()) {
qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
return;
}
_clearAndDeleteWriteMissionItems();
bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
int firstIndex = skipFirstItem ? 1 : 0;
for (int i=firstIndex; i<missionItems.count(); i++) {
MissionItem* item = new MissionItem(*missionItems[i]);
_writeMissionItems.append(item);
item->setIsCurrentItem(i == firstIndex);
if (skipFirstItem) {
// Home is in sequence 0, remainder of items start at sequence 1
item->setSequenceNumber(item->sequenceNumber() - 1);
if (item->command() == MAV_CMD_DO_JUMP) {
item->setParam1((int)item->param1() - 1);
}
}
}
_writeMissionItemsWorker();
}
/// This begins the write sequence with the vehicle. This may be called during a retry.
void MissionManager::_writeMissionCount(void)
{ {
qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _writeMissionItems.count() << _retryCount;
mavlink_message_t message;
mavlink_mission_count_t missionCount;
memset(&missionCount, 0, sizeof(missionCount));
missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _writeMissionItems.count();
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionCount);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionRequest);
} }
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly) void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
{ {
if (inProgress()) { if (inProgress()) {
...@@ -166,809 +71,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC ...@@ -166,809 +71,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
emit inProgressChanged(true); emit inProgressChanged(true);
} }
void MissionManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
qCDebug(MissionManagerLog) << "loadFromVehicle read sequence";
if (inProgress()) {
qCDebug(MissionManagerLog) << "loadFromVehicle called while transaction in progress";
return;
}
_retryCount = 0;
_transactionInProgress = TransactionRead;
emit inProgressChanged(true);
_requestList();
}
/// Internal call to request list of mission items. May be called during a retry sequence.
void MissionManager::_requestList(void)
{
qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount;
mavlink_message_t message;
mavlink_mission_request_list_t request;
memset(&request, 0, sizeof(request));
_itemIndicesToRead.clear();
_clearMissionItems();
request.target_system = _vehicle->id();
request.target_component = MAV_COMP_ID_MISSIONPLANNER;
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&request);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionCount);
}
void MissionManager::_ackTimeout(void)
{
if (_expectedAck == AckNone) {
return;
}
switch (_expectedAck) {
case AckNone:
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
break;
case AckMissionCount:
// MISSION_COUNT message expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount;
_requestList();
}
break;
case AckMissionItem:
// MISSION_ITEM expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount;
_requestNextMissionItem();
}
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (_itemIndicesToWrite.count() == 0) {
// Vehicle did not send final MISSION_ACK at end of sequence
_sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
_finishTransaction(false);
} else if (_itemIndicesToWrite[0] == 0) {
// Vehicle did not respond to MISSION_COUNT, try again
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount;
_writeMissionCount();
}
} else {
// Vehicle did not request all items from ground station
_sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_CLEAR_ALL retry Count" << _retryCount;
_removeAllWorker();
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
default:
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
}
void MissionManager::_startAckTimeout(AckType_t ack)
{
_expectedAck = ack;
_ackTimeoutTimer->start();
}
/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
/// @return true: received ack matches expected ack
bool MissionManager::_checkForExpectedAck(AckType_t receivedAck)
{
if (receivedAck == _expectedAck) {
_expectedAck = AckNone;
_ackTimeoutTimer->stop();
return true;
} else {
if (_expectedAck == AckNone) {
// Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
// spurious MISSION_ITEMs.
} else {
// We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
// Whatever it is we let the ack timeout handle any error output to the user.
qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck));
}
return false;
}
}
void MissionManager::_readTransactionComplete(void)
{
qCDebug(MissionManagerLog) << "_readTransactionComplete read sequence complete";
mavlink_message_t message;
mavlink_mission_ack_t missionAck;
memset(&missionAck, 0, sizeof(missionAck));
missionAck.target_system = _vehicle->id();
missionAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionAck.type = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionAck);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_finishTransaction(true);
}
void MissionManager::_handleMissionCount(const mavlink_message_t& message)
{
mavlink_mission_count_t missionCount;
if (!_checkForExpectedAck(AckMissionCount)) {
return;
}
_retryCount = 0;
mavlink_msg_mission_count_decode(&message, &missionCount);
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
if (missionCount.count == 0) {
_readTransactionComplete();
} else {
// Prime read list
for (int i=0; i<missionCount.count; i++) {
_itemIndicesToRead << i;
}
_requestNextMissionItem();
}
}
void MissionManager::_requestNextMissionItem(void)
{
if (_itemIndicesToRead.count() == 0) {
_sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
return;
}
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message;
if (_vehicle->supportsMissionItemInt()) {
mavlink_mission_request_int_t missionRequest;
memset(&missionRequest, 0, sizeof(missionRequest));
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionRequest);
} else {
mavlink_mission_request_t missionRequest;
memset(&missionRequest, 0, sizeof(missionRequest));
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionRequest);
}
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionItem);
}
void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
{
MAV_CMD command;
MAV_FRAME frame;
double param1;
double param2;
double param3;
double param4;
double param5;
double param6;
double param7;
bool autoContinue;
bool isCurrentItem;
int seq;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = (double)missionItem.x / qPow(10.0, 7.0);
param6 = (double)missionItem.y / qPow(10.0, 7.0);
param7 = (double)missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
} else {
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = missionItem.x;
param6 = missionItem.y;
param7 = missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
}
// We don't support editing ALT_INT frames so change on the way in.
if (frame == MAV_FRAME_GLOBAL_INT) {
frame = MAV_FRAME_GLOBAL;
} else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
bool ardupilotHomePositionUpdate = false;
if (!_checkForExpectedAck(AckMissionItem)) {
if (_vehicle->apmFirmware() && seq == 0) {
ardupilotHomePositionUpdate = true;
} else {
qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem;
return;
}
}
qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
if (ardupilotHomePositionUpdate) {
QGeoCoordinate newHomePosition(param5, param6, param7);
_vehicle->_setHomePosition(newHomePosition);
return;
}
if (_itemIndicesToRead.contains(seq)) {
_itemIndicesToRead.removeOne(seq);
MissionItem* item = new MissionItem(seq,
command,
frame,
param1,
param2,
param3,
param4,
param5,
param6,
param7,
autoContinue,
isCurrentItem,
this);
if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
// Home is in position 0
item->setParam1((int)item->param1() + 1);
}
_missionItems.append(item);
} else {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq;
// We have to put the ack timeout back since it was removed above
_startAckTimeout(AckMissionItem);
return;
}
emit progressPct((double)seq / (double)_missionItems.count());
_retryCount = 0;
if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete();
} else {
_requestNextMissionItem();
}
}
void MissionManager::_clearMissionItems(void)
{
_itemIndicesToRead.clear();
_clearAndDeleteMissionItems();
}
void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{
mavlink_mission_request_t missionRequest;
if (!_checkForExpectedAck(AckMissionRequest)) {
return;
}
mavlink_msg_mission_request_decode(&message, &missionRequest);
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq;
if (missionRequest.seq > _writeMissionItems.count() - 1) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq));
_finishTransaction(false);
return;
}
emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count());
_lastMissionRequest = missionRequest.seq;
if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
}
MissionItem* item = _writeMissionItems[missionRequest.seq];
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command();
mavlink_message_t messageOut;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
memset(&missionItem, 0, sizeof(missionItem));
missionItem.target_system = _vehicle->id();
missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionItem.seq = missionRequest.seq;
missionItem.command = item->command();
missionItem.param1 = item->param1();
missionItem.param2 = item->param2();
missionItem.param3 = item->param3();
missionItem.param4 = item->param4();
missionItem.x = item->param5() * qPow(10.0, 7.0);
missionItem.y = item->param6() * qPow(10.0, 7.0);
missionItem.z = item->param7();
missionItem.frame = item->frame();
missionItem.current = missionRequest.seq == 0;
missionItem.autocontinue = item->autoContinue();
mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
&missionItem);
} else {
mavlink_mission_item_t missionItem;
memset(&missionItem, 0, sizeof(missionItem));
missionItem.target_system = _vehicle->id();
missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionItem.seq = missionRequest.seq;
missionItem.command = item->command();
missionItem.param1 = item->param1();
missionItem.param2 = item->param2();
missionItem.param3 = item->param3();
missionItem.param4 = item->param4();
missionItem.x = item->param5();
missionItem.y = item->param6();
missionItem.z = item->param7();
missionItem.frame = item->frame();
missionItem.current = missionRequest.seq == 0;
missionItem.autocontinue = item->autoContinue();
mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
&missionItem);
}
_vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
_startAckTimeout(AckMissionRequest);
}
void MissionManager::_handleMissionAck(const mavlink_message_t& message)
{
mavlink_mission_ack_t missionAck;
// Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
// type of a protocol sequence we are in.
AckType_t savedExpectedAck = _expectedAck;
// We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
// a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
// matter what.
if (!_checkForExpectedAck(_expectedAck)) {
return;
}
mavlink_msg_mission_ack_decode(&message, &missionAck);
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedExpectedAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionItem:
// MISSION_ITEM expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
_finishTransaction(true);
} else {
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count()));
_finishTransaction(false);
}
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (missionAck.type != MAV_MISSION_ACCEPTED) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
_finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED);
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
}
}
/// Called when a new mavlink message for out vehicle is received
void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
_handleMissionRequest(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
_handleMissionAck(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
// FIXME: NYI
break;
case MAVLINK_MSG_ID_MISSION_CURRENT:
_handleMissionCurrent(message);
break;
}
}
void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
emit error(errorCode, errorMsg);
}
QString MissionManager::_ackTypeToString(AckType_t ackType)
{
switch (ackType) {
case AckNone:
return QString("No Ack");
case AckMissionCount:
return QString("MISSION_COUNT");
case AckMissionItem:
return QString("MISSION_ITEM");
case AckMissionRequest:
return QString("MISSION_REQUEST");
case AckGuidedItem:
return QString("Guided Mode Item");
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
}
}
QString MissionManager::_lastMissionReqestString(MAV_MISSION_RESULT result)
{
if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) {
MissionItem* item = _writeMissionItems[_lastMissionRequest];
switch (result) {
case MAV_MISSION_UNSUPPORTED_FRAME:
return QString(". Frame: %1").arg(item->frame());
case MAV_MISSION_UNSUPPORTED:
{
const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command());
QString friendlyName;
QString rawName;
if (uiInfo) {
friendlyName = uiInfo->friendlyName();
rawName = uiInfo->rawName();
}
return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command());
}
case MAV_MISSION_INVALID_PARAM1:
return QString(". Param1: %1").arg(item->param1());
case MAV_MISSION_INVALID_PARAM2:
return QString(". Param2: %1").arg(item->param2());
case MAV_MISSION_INVALID_PARAM3:
return QString(". Param3: %1").arg(item->param3());
case MAV_MISSION_INVALID_PARAM4:
return QString(". Param4: %1").arg(item->param4());
case MAV_MISSION_INVALID_PARAM5_X:
return QString(". Param5: %1").arg(item->param5());
case MAV_MISSION_INVALID_PARAM6_Y:
return QString(". Param6: %1").arg(item->param6());
case MAV_MISSION_INVALID_PARAM7:
return QString(". Param7: %1").arg(item->param7());
case MAV_MISSION_INVALID_SEQUENCE:
return QString(". Sequence: %1").arg(item->sequenceNumber());
default:
break;
}
}
return QString();
}
QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
{
QString resultString;
QString lastRequestString = _lastMissionReqestString(result);
switch (result) {
case MAV_MISSION_ACCEPTED:
resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)");
break;
case MAV_MISSION_ERROR:
resultString = QString("Unspecified error (MAV_MISSION_ERROR)");
break;
case MAV_MISSION_UNSUPPORTED_FRAME:
resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
break;
case MAV_MISSION_UNSUPPORTED:
resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
break;
case MAV_MISSION_NO_SPACE:
resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
break;
case MAV_MISSION_INVALID:
resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
break;
case MAV_MISSION_INVALID_PARAM1:
resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
break;
case MAV_MISSION_INVALID_PARAM2:
resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
break;
case MAV_MISSION_INVALID_PARAM3:
resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
break;
case MAV_MISSION_INVALID_PARAM4:
resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
break;
case MAV_MISSION_INVALID_PARAM5_X:
resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
break;
case MAV_MISSION_INVALID_PARAM6_Y:
resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
break;
case MAV_MISSION_INVALID_PARAM7:
resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
break;
case MAV_MISSION_INVALID_SEQUENCE:
resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
break;
case MAV_MISSION_DENIED:
resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
break;
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
resultString = QString("QGC Internal Error");
}
return resultString + lastRequestString;
}
void MissionManager::_finishTransaction(bool success)
{
emit progressPct(1);
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
// First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete.
TransactionType_t currentTransactionType = _transactionInProgress;
_transactionInProgress = TransactionNone;
if (currentTransactionType != TransactionNone) {
_transactionInProgress = TransactionNone;
qDebug() << "inProgressChanged";
emit inProgressChanged(false);
}
switch (currentTransactionType) {
case TransactionRead:
if (!success) {
// Read from vehicle failed, clear partial list
_clearAndDeleteMissionItems();
}
emit newMissionItemsAvailable(false);
break;
case TransactionWrite:
if (success) {
// Write succeeded, update internal list to be current
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
_clearAndDeleteMissionItems();
for (int i=0; i<_writeMissionItems.count(); i++) {
_missionItems.append(_writeMissionItems[i]);
}
_writeMissionItems.clear();
} else {
// Write failed, throw out the write list
_clearAndDeleteWriteMissionItems();
}
emit sendComplete(!success /* error */);
break;
case TransactionRemoveAll:
emit removeAllComplete(!success /* error */);
break;
default:
break;
}
if (_resumeMission) {
_resumeMission = false;
if (success) {
emit resumeMissionReady();
} else {
emit resumeMissionUploadFail();
}
}
}
bool MissionManager::inProgress(void)
{
return _transactionInProgress != TransactionNone;
}
void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
{
mavlink_mission_current_t missionCurrent;
mavlink_msg_mission_current_decode(&message, &missionCurrent);
if (missionCurrent.seq != _currentMissionIndex) {
qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq;
_currentMissionIndex = missionCurrent.seq;
emit currentIndexChanged(_currentMissionIndex);
}
if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex;
_lastCurrentIndex = _currentMissionIndex;
emit lastCurrentIndexChanged(_lastCurrentIndex);
}
}
void MissionManager::_removeAllWorker(void)
{
mavlink_message_t message;
qCDebug(MissionManagerLog) << "_removeAllWorker";
emit progressPct(0);
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
MAV_MISSION_TYPE_MISSION);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
_startAckTimeout(AckMissionClearAll);
}
void MissionManager::removeAll(void)
{
if (inProgress()) {
return;
}
qCDebug(MissionManagerLog) << "removeAll";
_clearAndDeleteMissionItems();
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
_transactionInProgress = TransactionRemoveAll;
_retryCount = 0;
emit inProgressChanged(true);
_removeAllWorker();
}
void MissionManager::generateResumeMission(int resumeIndex) void MissionManager::generateResumeMission(int resumeIndex)
{ {
if (_vehicle->isOfflineEditingVehicle()) { if (_vehicle->isOfflineEditingVehicle()) {
...@@ -1129,20 +231,3 @@ void MissionManager::generateResumeMission(int resumeIndex) ...@@ -1129,20 +231,3 @@ void MissionManager::generateResumeMission(int resumeIndex)
resumeMission[i]->deleteLater(); resumeMission[i]->deleteLater();
} }
} }
void MissionManager::_clearAndDeleteMissionItems(void)
{
for (int i=0; i<_missionItems.count(); i++) {
_missionItems[i]->deleteLater();
}
_missionItems.clear();
}
void MissionManager::_clearAndDeleteWriteMissionItems(void)
{
for (int i=0; i<_writeMissionItems.count(); i++) {
_writeMissionItems[i]->deleteLater();
}
_writeMissionItems.clear();
}
src/MissionManager/MissionManager.h
View file @ 537463c5
...@@ -11,22 +11,11 @@ ...@@ -11,22 +11,11 @@
#ifndef MissionManager_H #ifndef MissionManager_H
#define MissionManager_H #define MissionManager_H
#include <QObject> #include "PlanManager.h"
#include <QLoggingCategory>
#include <QThread>
#include <QMutex>
#include <QTimer>
#include "MissionItem.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"
class Vehicle;
Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog) Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog)
class MissionManager : public QObject class MissionManager : public PlanManager
{ {
Q_OBJECT Q_OBJECT
...@@ -34,128 +23,14 @@ public: ...@@ -34,128 +23,14 @@ public:
MissionManager(Vehicle* vehicle); MissionManager(Vehicle* vehicle);
~MissionManager(); ~MissionManager();
bool inProgress(void);
const QList<MissionItem*>& missionItems(void) { return _missionItems; }
/// Current mission item as reported by MISSION_CURRENT
int currentIndex(void) const { return _currentMissionIndex; }
/// Last current mission item reported while in Mission flight mode
int lastCurrentIndex(void) const { return _lastCurrentIndex; }
/// Load the mission items from the vehicle
/// Signals newMissionItemsAvailable when done
void loadFromVehicle(void);
/// Writes the specified set of mission items to the vehicle
/// @param missionItems Items to send to vehicle
/// Signals sendComplete when done
void writeMissionItems(const QList<MissionItem*>& missionItems);
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item. /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
/// @param gotoCoord Coordinate to move to /// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change /// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly); void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);
/// Removes all mission items from vehicle
/// Signals removeAllComplete when done
void removeAll(void);
/// Generates a new mission which starts from the specified index. It will include all the CMD_DO items /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
/// from mission start to resumeIndex in the generate mission. /// from mission start to resumeIndex in the generate mission.
void generateResumeMission(int resumeIndex); void generateResumeMission(int resumeIndex);
/// Error codes returned in error signal
typedef enum {
InternalError,
AckTimeoutError, ///< Timed out waiting for response from vehicle
ProtocolOrderError, ///< Incorrect protocol sequence from vehicle
RequestRangeError, ///< Vehicle requested item out of range
ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence
MaxRetryExceeded, ///< Retry failed
} ErrorCode_t;
// These values are public so the unit test can set appropriate signal wait times
static const int _ackTimeoutMilliseconds = 1000;
static const int _maxRetryCount = 5;
signals:
void newMissionItemsAvailable(bool removeAllRequested);
void inProgressChanged(bool inProgress);
void error(int errorCode, const QString& errorMsg);
void currentIndexChanged(int currentIndex);
void lastCurrentIndexChanged(int lastCurrentIndex);
void resumeMissionReady(void);
void resumeMissionUploadFail(void);
void progressPct(double progressPercentPct);
void removeAllComplete (bool error);
void sendComplete (bool error);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
void _ackTimeout(void);
private:
typedef enum {
AckNone, ///< State machine is idle
AckMissionCount, ///< MISSION_COUNT message expected
AckMissionItem, ///< MISSION_ITEM expected
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
} AckType_t;
typedef enum {
TransactionNone,
TransactionRead,
TransactionWrite,
TransactionRemoveAll
} TransactionType_t;
void _startAckTimeout(AckType_t ack);
bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionAck(const mavlink_message_t& message);
void _handleMissionCurrent(const mavlink_message_t& message);
void _requestNextMissionItem(void);
void _clearMissionItems(void);
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
void _requestList(void);
void _writeMissionCount(void);
void _writeMissionItemsWorker(void);
void _clearAndDeleteMissionItems(void);
void _clearAndDeleteWriteMissionItems(void);
QString _lastMissionReqestString(MAV_MISSION_RESULT result);
void _removeAllWorker(void);
private:
Vehicle* _vehicle;
LinkInterface* _dedicatedLink;
QTimer* _ackTimeoutTimer;
AckType_t _expectedAck;
int _retryCount;
TransactionType_t _transactionInProgress;
bool _resumeMission;
QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
QMutex _dataMutex;
QList<MissionItem*> _missionItems; ///< Set of mission items on vehicle
QList<MissionItem*> _writeMissionItems; ///< Set of mission items currently being written to vehicle
int _currentMissionIndex;
int _lastCurrentIndex;
}; };
#endif #endif
src/MissionManager/PlanElementController.h
View file @ 537463c5
...@@ -27,6 +27,7 @@ public: ...@@ -27,6 +27,7 @@ public:
PlanElementController(PlanMasterController* masterController, QObject* parent = NULL); PlanElementController(PlanMasterController* masterController, QObject* parent = NULL);
~PlanElementController(); ~PlanElementController();
Q_PROPERTY(bool supported READ supported NOTIFY supportedChanged) ///< true: Element is supported by Vehicle
Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty
Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send
...@@ -41,10 +42,11 @@ public: ...@@ -41,10 +42,11 @@ public:
virtual void removeAll (void) = 0; ///< Removes all from controller only virtual void removeAll (void) = 0; ///< Removes all from controller only
virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete
virtual bool containsItems (void) const = 0; virtual bool supported (void) const = 0;
virtual bool syncInProgress (void) const = 0; virtual bool containsItems (void) const = 0;
virtual bool dirty (void) const = 0; virtual bool syncInProgress (void) const = 0;
virtual void setDirty (bool dirty) = 0; virtual bool dirty (void) const = 0;
virtual void setDirty (bool dirty) = 0;
/// Sends the current plan element to the vehicle /// Sends the current plan element to the vehicle
/// Signals sendComplete when done /// Signals sendComplete when done
...@@ -58,6 +60,7 @@ public: ...@@ -58,6 +60,7 @@ public:
virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0; virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0;
signals: signals:
void supportedChanged (bool supported);
void containsItemsChanged (bool containsItems); void containsItemsChanged (bool containsItems);
void syncInProgressChanged (bool syncInProgress); void syncInProgressChanged (bool syncInProgress);
void dirtyChanged (bool dirty); void dirtyChanged (bool dirty);
......
src/MissionManager/PlanManager.cc 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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.
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "PlanManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "MissionCommandTree.h"
#include "MissionCommandUIInfo.h"
QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog")
PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType)
: _vehicle(vehicle)
, _planType(planType)
, _dedicatedLink(NULL)
, _ackTimeoutTimer(NULL)
, _expectedAck(AckNone)
, _transactionInProgress(TransactionNone)
, _resumeMission(false)
, _lastMissionRequest(-1)
, _currentMissionIndex(-1)
, _lastCurrentIndex(-1)
{
_ackTimeoutTimer = new QTimer(this);
_ackTimeoutTimer->setSingleShot(true);
_ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
connect(_ackTimeoutTimer, &QTimer::timeout, this, &PlanManager::_ackTimeout);
}
PlanManager::~PlanManager()
{
}
void PlanManager::_writeMissionItemsWorker(void)
{
_lastMissionRequest = -1;
emit progressPct(0);
qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 count:").arg(_planTypeString()) << _writeMissionItems.count();
// Prime write list
_itemIndicesToWrite.clear();
for (int i=0; i<_writeMissionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_transactionInProgress = TransactionWrite;
_retryCount = 0;
emit inProgressChanged(true);
_connectToMavlink();
_writeMissionCount();
}
void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
if (inProgress()) {
qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 called while transaction in progress").arg(_planTypeString());
return;
}
_clearAndDeleteWriteMissionItems();
bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
int firstIndex = skipFirstItem ? 1 : 0;
for (int i=firstIndex; i<missionItems.count(); i++) {
MissionItem* item = new MissionItem(*missionItems[i]);
_writeMissionItems.append(item);
item->setIsCurrentItem(i == firstIndex);
if (skipFirstItem) {
// Home is in sequence 0, remainder of items start at sequence 1
item->setSequenceNumber(item->sequenceNumber() - 1);
if (item->command() == MAV_CMD_DO_JUMP) {
item->setParam1((int)item->param1() - 1);
}
}
}
_writeMissionItemsWorker();
}
/// This begins the write sequence with the vehicle. This may be called during a retry.
void PlanManager::_writeMissionCount(void)
{
qCDebug(PlanManagerLog) << QStringLiteral("_writeMissionCount %1 count:_retryCount").arg(_planTypeString()) << _writeMissionItems.count() << _retryCount;
mavlink_message_t message;
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_count_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
_writeMissionItems.count(),
_planType);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionRequest);
}
void PlanManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 read sequence").arg(_planTypeString());
if (inProgress()) {
qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 called while transaction in progress").arg(_planTypeString());
return;
}
_retryCount = 0;
_transactionInProgress = TransactionRead;
emit inProgressChanged(true);
_connectToMavlink();
_requestList();
}
/// Internal call to request list of mission items. May be called during a retry sequence.
void PlanManager::_requestList(void)
{
qCDebug(PlanManagerLog) << QStringLiteral("_requestList %1 _planType:_retryCount").arg(_planTypeString()) << _planType << _retryCount;
mavlink_message_t message;
_itemIndicesToRead.clear();
_clearMissionItems();
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_request_list_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
_planType);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionCount);
}
void PlanManager::_ackTimeout(void)
{
if (_expectedAck == AckNone) {
return;
}
switch (_expectedAck) {
case AckNone:
qCWarning(PlanManagerLog) << QStringLiteral("_ackTimeout %1 timeout with AckNone").arg(_planTypeString());
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
break;
case AckMissionCount:
// MISSION_COUNT message expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 REQUEST_LIST retry Count").arg(_planTypeString()) << _retryCount;
_requestList();
}
break;
case AckMissionItem:
// MISSION_ITEM expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_REQUEST retry Count").arg(_planTypeString()) << _retryCount;
_requestNextMissionItem();
}
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (_itemIndicesToWrite.count() == 0) {
// Vehicle did not send final MISSION_ACK at end of sequence
_sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
_finishTransaction(false);
} else if (_itemIndicesToWrite[0] == 0) {
// Vehicle did not respond to MISSION_COUNT, try again
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_COUNT retry Count").arg(_planTypeString()) << _retryCount;
_writeMissionCount();
}
} else {
// Vehicle did not request all items from ground station
_sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_CLEAR_ALL retry Count").arg(_planTypeString()) << _retryCount;
_removeAllWorker();
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
default:
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
}
void PlanManager::_startAckTimeout(AckType_t ack)
{
_expectedAck = ack;
_ackTimeoutTimer->start();
}
/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
/// @return true: received ack matches expected ack
bool PlanManager::_checkForExpectedAck(AckType_t receivedAck)
{
if (receivedAck == _expectedAck) {
_expectedAck = AckNone;
_ackTimeoutTimer->stop();
return true;
} else {
if (_expectedAck == AckNone) {
// Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
// spurious MISSION_ITEMs.
} else {
// We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
// Whatever it is we let the ack timeout handle any error output to the user.
qCDebug(PlanManagerLog) << QString("Out of sequence ack expected:received %1:%2 %1").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck)).arg(_planTypeString());
}
return false;
}
}
void PlanManager::_readTransactionComplete(void)
{
qCDebug(PlanManagerLog) << "_readTransactionComplete read sequence complete";
mavlink_message_t message;
mavlink_msg_mission_ack_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
MAV_MISSION_ACCEPTED,
_planType);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_finishTransaction(true);
}
void PlanManager::_handleMissionCount(const mavlink_message_t& message)
{
mavlink_mission_count_t missionCount;
mavlink_msg_mission_count_decode(&message, &missionCount);
if (missionCount.mission_type != _planType) {
// if there was a previous transaction with a different mission_type, it can happen that we receive
// a stale message here, for example when the MAV ran into a timeout and sent a message twice
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionCount.mission_type;
return;
}
if (!_checkForExpectedAck(AckMissionCount)) {
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 count:").arg(_planTypeString()) << missionCount.count;
_retryCount = 0;
if (missionCount.count == 0) {
_readTransactionComplete();
} else {
// Prime read list
for (int i=0; i<missionCount.count; i++) {
_itemIndicesToRead << i;
}
_requestNextMissionItem();
}
}
void PlanManager::_requestNextMissionItem(void)
{
if (_itemIndicesToRead.count() == 0) {
_sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("_requestNextMissionItem %1 sequenceNumber:retry").arg(_planTypeString()) << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message;
if (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
mavlink_msg_mission_request_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
_itemIndicesToRead[0],
_planType);
} else {
mavlink_msg_mission_request_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
_itemIndicesToRead[0],
_planType);
}
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionItem);
}
void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
{
MAV_CMD command;
MAV_FRAME frame;
double param1;
double param2;
double param3;
double param4;
double param5;
double param6;
double param7;
bool autoContinue;
bool isCurrentItem;
int seq;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = (double)missionItem.x / qPow(10.0, 7.0);
param6 = (double)missionItem.y / qPow(10.0, 7.0);
param7 = (double)missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
} else {
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = missionItem.x;
param6 = missionItem.y;
param7 = missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
}
// We don't support editing ALT_INT frames so change on the way in.
if (frame == MAV_FRAME_GLOBAL_INT) {
frame = MAV_FRAME_GLOBAL;
} else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
bool ardupilotHomePositionUpdate = false;
if (!_checkForExpectedAck(AckMissionItem)) {
if (_vehicle->apmFirmware() && seq == 0 && _planType == MAV_MISSION_TYPE_MISSION) {
ardupilotHomePositionUpdate = true;
} else {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 dropping spurious item seq:command:current").arg(_planTypeString()) << seq << command << isCurrentItem;
return;
}
}
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 seq:command:current:ardupilotHomePositionUpdate").arg(_planTypeString()) << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
if (ardupilotHomePositionUpdate) {
QGeoCoordinate newHomePosition(param5, param6, param7);
_vehicle->_setHomePosition(newHomePosition);
return;
}
if (_itemIndicesToRead.contains(seq)) {
_itemIndicesToRead.removeOne(seq);
MissionItem* item = new MissionItem(seq,
command,
frame,
param1,
param2,
param3,
param4,
param5,
param6,
param7,
autoContinue,
isCurrentItem,
this);
if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
// Home is in position 0
item->setParam1((int)item->param1() + 1);
}
_missionItems.append(item);
} else {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 mission item received item index which was not requested, disregrarding:").arg(_planTypeString()) << seq;
// We have to put the ack timeout back since it was removed above
_startAckTimeout(AckMissionItem);
return;
}
emit progressPct((double)seq / (double)_missionItems.count());
_retryCount = 0;
if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete();
} else {
_requestNextMissionItem();
}
}
void PlanManager::_clearMissionItems(void)
{
_itemIndicesToRead.clear();
_clearAndDeleteMissionItems();
}
void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{
mavlink_mission_request_t missionRequest;
mavlink_msg_mission_request_decode(&message, &missionRequest);
if (missionRequest.mission_type != _planType) {
// if there was a previous transaction with a different mission_type, it can happen that we receive
// a stale message here, for example when the MAV ran into a timeout and sent a message twice
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequest.mission_type;
return;
}
if (!_checkForExpectedAck(AckMissionRequest)) {
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequest.seq;
if (missionRequest.seq > _writeMissionItems.count() - 1) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq));
_finishTransaction(false);
return;
}
emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count());
_lastMissionRequest = missionRequest.seq;
if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequest.seq;
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
}
MissionItem* item = _writeMissionItems[missionRequest.seq];
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequest.seq << item->command();
mavlink_message_t messageOut;
if (missionItemInt) {
mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
missionRequest.seq,
item->frame(),
item->command(),
missionRequest.seq == 0,
item->autoContinue(),
item->param1(),
item->param2(),
item->param3(),
item->param4(),
item->param5() * qPow(10.0, 7.0),
item->param6() * qPow(10.0, 7.0),
item->param7(),
_planType);
} else {
mavlink_msg_mission_item_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
missionRequest.seq,
item->frame(),
item->command(),
missionRequest.seq == 0,
item->autoContinue(),
item->param1(),
item->param2(),
item->param3(),
item->param4(),
item->param5(),
item->param6(),
item->param7(),
_planType);
}
_vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
_startAckTimeout(AckMissionRequest);
}
void PlanManager::_handleMissionAck(const mavlink_message_t& message)
{
mavlink_mission_ack_t missionAck;
mavlink_msg_mission_ack_decode(&message, &missionAck);
if (missionAck.mission_type != _planType) {
// if there was a previous transaction with a different mission_type, it can happen that we receive
// a stale message here, for example when the MAV ran into a timeout and sent a message twice
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionAck.mission_type;
return;
}
// Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
// type of a protocol sequence we are in.
AckType_t savedExpectedAck = _expectedAck;
// We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
// a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
// matter what.
if (!_checkForExpectedAck(_expectedAck)) {
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 type:").arg(_planTypeString()) << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedExpectedAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionItem:
// MISSION_ITEM expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete").arg(_planTypeString());
_finishTransaction(true);
} else {
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count()));
_finishTransaction(false);
}
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (missionAck.type != MAV_MISSION_ACCEPTED) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
_finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED);
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 guided mode item accepted").arg(_planTypeString());
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
}
}
/// Called when a new mavlink message for out vehicle is received
void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
_handleMissionRequest(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
_handleMissionAck(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
// FIXME: NYI
break;
case MAVLINK_MSG_ID_MISSION_CURRENT:
_handleMissionCurrent(message);
break;
}
}
void PlanManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
qCDebug(PlanManagerLog) << QStringLiteral("Sending %1 error").arg(_planTypeString()) << errorCode << errorMsg;
emit error(errorCode, errorMsg);
}
QString PlanManager::_ackTypeToString(AckType_t ackType)
{
switch (ackType) {
case AckNone:
return QString("No Ack");
case AckMissionCount:
return QString("MISSION_COUNT");
case AckMissionItem:
return QString("MISSION_ITEM");
case AckMissionRequest:
return QString("MISSION_REQUEST");
case AckGuidedItem:
return QString("Guided Mode Item");
default:
qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString());
return QString("QGC Internal Error");
}
}
QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result)
{
if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) {
MissionItem* item = _writeMissionItems[_lastMissionRequest];
switch (result) {
case MAV_MISSION_UNSUPPORTED_FRAME:
return QString(". Frame: %1").arg(item->frame());
case MAV_MISSION_UNSUPPORTED:
{
const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command());
QString friendlyName;
QString rawName;
if (uiInfo) {
friendlyName = uiInfo->friendlyName();
rawName = uiInfo->rawName();
}
return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command());
}
case MAV_MISSION_INVALID_PARAM1:
return QString(". Param1: %1").arg(item->param1());
case MAV_MISSION_INVALID_PARAM2:
return QString(". Param2: %1").arg(item->param2());
case MAV_MISSION_INVALID_PARAM3:
return QString(". Param3: %1").arg(item->param3());
case MAV_MISSION_INVALID_PARAM4:
return QString(". Param4: %1").arg(item->param4());
case MAV_MISSION_INVALID_PARAM5_X:
return QString(". Param5: %1").arg(item->param5());
case MAV_MISSION_INVALID_PARAM6_Y:
return QString(". Param6: %1").arg(item->param6());
case MAV_MISSION_INVALID_PARAM7:
return QString(". Param7: %1").arg(item->param7());
case MAV_MISSION_INVALID_SEQUENCE:
return QString(". Sequence: %1").arg(item->sequenceNumber());
default:
break;
}
}
return QString();
}
QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result)
{
QString resultString;
QString lastRequestString = _lastMissionReqestString(result);
switch (result) {
case MAV_MISSION_ACCEPTED:
resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)");
break;
case MAV_MISSION_ERROR:
resultString = QString("Unspecified error (MAV_MISSION_ERROR)");
break;
case MAV_MISSION_UNSUPPORTED_FRAME:
resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
break;
case MAV_MISSION_UNSUPPORTED:
resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
break;
case MAV_MISSION_NO_SPACE:
resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
break;
case MAV_MISSION_INVALID:
resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
break;
case MAV_MISSION_INVALID_PARAM1:
resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
break;
case MAV_MISSION_INVALID_PARAM2:
resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
break;
case MAV_MISSION_INVALID_PARAM3:
resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
break;
case MAV_MISSION_INVALID_PARAM4:
resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
break;
case MAV_MISSION_INVALID_PARAM5_X:
resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
break;
case MAV_MISSION_INVALID_PARAM6_Y:
resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
break;
case MAV_MISSION_INVALID_PARAM7:
resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
break;
case MAV_MISSION_INVALID_SEQUENCE:
resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
break;
case MAV_MISSION_DENIED:
resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
break;
default:
qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString());
resultString = QString("QGC Internal Error");
}
return resultString + lastRequestString;
}
void PlanManager::_finishTransaction(bool success)
{
emit progressPct(1);
_disconnectFromMavlink();
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
// First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete.
TransactionType_t currentTransactionType = _transactionInProgress;
_transactionInProgress = TransactionNone;
if (currentTransactionType != TransactionNone) {
_transactionInProgress = TransactionNone;
qCDebug(PlanManagerLog) << QStringLiteral("inProgressChanged %1").arg(_planTypeString());
emit inProgressChanged(false);
}
switch (currentTransactionType) {
case TransactionRead:
if (!success) {
// Read from vehicle failed, clear partial list
_clearAndDeleteMissionItems();
}
emit newMissionItemsAvailable(false);
break;
case TransactionWrite:
if (success) {
// Write succeeded, update internal list to be current
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
_clearAndDeleteMissionItems();
for (int i=0; i<_writeMissionItems.count(); i++) {
_missionItems.append(_writeMissionItems[i]);
}
_writeMissionItems.clear();
} else {
// Write failed, throw out the write list
_clearAndDeleteWriteMissionItems();
}
emit sendComplete(!success /* error */);
break;
case TransactionRemoveAll:
emit removeAllComplete(!success /* error */);
break;
default:
break;
}
if (_resumeMission) {
_resumeMission = false;
if (success) {
emit resumeMissionReady();
} else {
emit resumeMissionUploadFail();
}
}
}
bool PlanManager::inProgress(void) const
{
return _transactionInProgress != TransactionNone;
}
void PlanManager::_handleMissionCurrent(const mavlink_message_t& message)
{
mavlink_mission_current_t missionCurrent;
mavlink_msg_mission_current_decode(&message, &missionCurrent);
if (missionCurrent.seq != _currentMissionIndex) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 currentIndex:").arg(_planTypeString()) << missionCurrent.seq;
_currentMissionIndex = missionCurrent.seq;
emit currentIndexChanged(_currentMissionIndex);
}
if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 lastCurrentIndex:").arg(_planTypeString()) << _currentMissionIndex;
_lastCurrentIndex = _currentMissionIndex;
emit lastCurrentIndexChanged(_lastCurrentIndex);
}
}
void PlanManager::_removeAllWorker(void)
{
mavlink_message_t message;
qCDebug(PlanManagerLog) << "_removeAllWorker";
emit progressPct(0);
_connectToMavlink();
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
_planType);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
_startAckTimeout(AckMissionClearAll);
}
void PlanManager::removeAll(void)
{
if (inProgress()) {
return;
}
qCDebug(PlanManagerLog) << QStringLiteral("removeAll").arg(_planTypeString());
_clearAndDeleteMissionItems();
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
_transactionInProgress = TransactionRemoveAll;
_retryCount = 0;
emit inProgressChanged(true);
_removeAllWorker();
}
void PlanManager::_clearAndDeleteMissionItems(void)
{
for (int i=0; i<_missionItems.count(); i++) {
_missionItems[i]->deleteLater();
}
_missionItems.clear();
}
void PlanManager::_clearAndDeleteWriteMissionItems(void)
{
for (int i=0; i<_writeMissionItems.count(); i++) {
_writeMissionItems[i]->deleteLater();
}
_writeMissionItems.clear();
}
void PlanManager::_connectToMavlink(void)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
}
void PlanManager::_disconnectFromMavlink(void)
{
disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
}
QString PlanManager::_planTypeString(void)
{
switch (_planType) {
case MAV_MISSION_TYPE_MISSION:
return QStringLiteral("T:Mission");
case MAV_MISSION_TYPE_FENCE:
return QStringLiteral("T:GeoFence");
case MAV_MISSION_TYPE_RALLY:
return QStringLiteral("T:Rally");
default:
qWarning() << "Unknown plan type" << _planType;
return QStringLiteral("T:Unknown");
}
}
src/MissionManager/PlanManager.h 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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 PlanManager_H
#define PlanManager_H
#include <QObject>
#include <QLoggingCategory>
#include <QTimer>
#include "MissionItem.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"
class Vehicle;
Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog)
/// The PlanManager class is the base class for the Mission, GeoFence and Rally Point managers. All of which use the
/// new mavlink v2 mission protocol.
class PlanManager : public QObject
{
Q_OBJECT
public:
PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType);
~PlanManager();
bool inProgress(void) const;
const QList<MissionItem*>& missionItems(void) { return _missionItems; }
/// Current mission item as reported by MISSION_CURRENT
int currentIndex(void) const { return _currentMissionIndex; }
/// Last current mission item reported while in Mission flight mode
int lastCurrentIndex(void) const { return _lastCurrentIndex; }
/// Load the mission items from the vehicle
/// Signals newMissionItemsAvailable when done
void loadFromVehicle(void);
/// Writes the specified set of mission items to the vehicle
/// @param missionItems Items to send to vehicle
/// Signals sendComplete when done
void writeMissionItems(const QList<MissionItem*>& missionItems);
/// Removes all mission items from vehicle
/// Signals removeAllComplete when done
void removeAll(void);
/// Error codes returned in error signal
typedef enum {
InternalError,
AckTimeoutError, ///< Timed out waiting for response from vehicle
ProtocolOrderError, ///< Incorrect protocol sequence from vehicle
RequestRangeError, ///< Vehicle requested item out of range
ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence
MaxRetryExceeded, ///< Retry failed
MissionTypeMismatch, ///< MAV_MISSION_TYPE does not match _planType
} ErrorCode_t;
// These values are public so the unit test can set appropriate signal wait times
static const int _ackTimeoutMilliseconds = 1000;
static const int _maxRetryCount = 5;
signals:
void newMissionItemsAvailable (bool removeAllRequested);
void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg);
void currentIndexChanged (int currentIndex);
void lastCurrentIndexChanged (int lastCurrentIndex);
void progressPct (double progressPercentPct);
void removeAllComplete (bool error);
void sendComplete (bool error);
void resumeMissionReady (void);
void resumeMissionUploadFail (void);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
void _ackTimeout(void);
protected:
typedef enum {
AckNone, ///< State machine is idle
AckMissionCount, ///< MISSION_COUNT message expected
AckMissionItem, ///< MISSION_ITEM expected
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
} AckType_t;
typedef enum {
TransactionNone,
TransactionRead,
TransactionWrite,
TransactionRemoveAll
} TransactionType_t;
void _startAckTimeout(AckType_t ack);
bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionAck(const mavlink_message_t& message);
void _handleMissionCurrent(const mavlink_message_t& message);
void _requestNextMissionItem(void);
void _clearMissionItems(void);
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
void _requestList(void);
void _writeMissionCount(void);
void _writeMissionItemsWorker(void);
void _clearAndDeleteMissionItems(void);
void _clearAndDeleteWriteMissionItems(void);
QString _lastMissionReqestString(MAV_MISSION_RESULT result);
void _removeAllWorker(void);
void _connectToMavlink(void);
void _disconnectFromMavlink(void);
QString _planTypeString(void);
protected:
Vehicle* _vehicle;
MAV_MISSION_TYPE _planType;
LinkInterface* _dedicatedLink;
QTimer* _ackTimeoutTimer;
AckType_t _expectedAck;
int _retryCount;
TransactionType_t _transactionInProgress;
bool _resumeMission;
QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
QList<MissionItem*> _missionItems; ///< Set of mission items on vehicle
QList<MissionItem*> _writeMissionItems; ///< Set of mission items currently being written to vehicle
int _currentMissionIndex;
int _lastCurrentIndex;
};
#endif
src/MissionManager/PlanMasterController.cc
View file @ 537463c5
...@@ -177,8 +177,14 @@ void PlanMasterController::_loadMissionComplete(void) ...@@ -177,8 +177,14 @@ void PlanMasterController::_loadMissionComplete(void)
if (_editMode && _loadGeoFence) { if (_editMode && _loadGeoFence) {
_loadGeoFence = false; _loadGeoFence = false;
_loadRallyPoints = true; _loadRallyPoints = true;
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle"; if (_geoFenceController.supported()) {
_geoFenceController.loadFromVehicle(); qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
_geoFenceController.loadFromVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete GeoFence not supported skipping";
_geoFenceController.removeAll();
_loadGeoFenceComplete();
}
setDirty(false); setDirty(false);
} }
} }
...@@ -187,8 +193,14 @@ void PlanMasterController::_loadGeoFenceComplete(void) ...@@ -187,8 +193,14 @@ void PlanMasterController::_loadGeoFenceComplete(void)
{ {
if (_editMode && _loadRallyPoints) { if (_editMode && _loadRallyPoints) {
_loadRallyPoints = false; _loadRallyPoints = false;
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle"; if (_rallyPointController.supported()) {
_rallyPointController.loadFromVehicle(); qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
_rallyPointController.loadFromVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete Rally Points not supported skipping";
_rallyPointController.removeAll();
_loadRallyPointsComplete();
}
setDirty(false); setDirty(false);
} }
} }
...@@ -204,10 +216,15 @@ void PlanMasterController::_loadRallyPointsComplete(void) ...@@ -204,10 +216,15 @@ void PlanMasterController::_loadRallyPointsComplete(void)
void PlanMasterController::_sendMissionComplete(void) void PlanMasterController::_sendMissionComplete(void)
{ {
if (_sendGeoFence) { if (_sendGeoFence) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle";
_sendGeoFence = false; _sendGeoFence = false;
_sendRallyPoints = true; _sendRallyPoints = true;
_geoFenceController.sendToVehicle(); if (_geoFenceController.supported()) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start GeoFence sendToVehicle";
_geoFenceController.sendToVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle GeoFence not supported skipping";
_sendGeoFenceComplete();
}
setDirty(false); setDirty(false);
} }
} }
...@@ -215,16 +232,21 @@ void PlanMasterController::_sendMissionComplete(void) ...@@ -215,16 +232,21 @@ void PlanMasterController::_sendMissionComplete(void)
void PlanMasterController::_sendGeoFenceComplete(void) void PlanMasterController::_sendGeoFenceComplete(void)
{ {
if (_sendRallyPoints) { if (_sendRallyPoints) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
_sendRallyPoints = false; _sendRallyPoints = false;
_rallyPointController.sendToVehicle(); if (_rallyPointController.supported()) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
_rallyPointController.sendToVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Points not support skipping";
_sendRallyPointsComplete();
}
} }
} }
void PlanMasterController::_sendRallyPointsComplete(void) void PlanMasterController::_sendRallyPointsComplete(void)
{ {
if (_syncInProgress) { if (_syncInProgress) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete"; qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Point send complete";
_syncInProgress = false; _syncInProgress = false;
emit syncInProgressChanged(false); emit syncInProgressChanged(false);
} }
...@@ -361,8 +383,12 @@ void PlanMasterController::removeAllFromVehicle(void) ...@@ -361,8 +383,12 @@ void PlanMasterController::removeAllFromVehicle(void)
{ {
if (!offline()) { if (!offline()) {
_missionController.removeAllFromVehicle(); _missionController.removeAllFromVehicle();
_geoFenceController.removeAllFromVehicle(); if (_geoFenceController.supported()) {
_rallyPointController.removeAllFromVehicle(); _geoFenceController.removeAllFromVehicle();
}
if (_rallyPointController.supported()) {
_rallyPointController.removeAllFromVehicle();
}
setDirty(false); setDirty(false);
} else { } else {
qWarning() << "PlanMasterController::removeAllFromVehicle called while offline"; qWarning() << "PlanMasterController::removeAllFromVehicle called while offline";
......
src/MissionManager/QGCFenceCircle.cc 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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 "QGCFenceCircle.h"
#include "JsonHelper.h"
const char* QGCFenceCircle::_jsonInclusionKey = "inclusion";
QGCFenceCircle::QGCFenceCircle(QObject* parent)
: QGCMapCircle (parent)
, _inclusion (true)
{
_init();
}
QGCFenceCircle::QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent)
: QGCMapCircle (center, radius, parent)
, _inclusion (inclusion)
{
_init();
}
QGCFenceCircle::QGCFenceCircle(const QGCFenceCircle& other, QObject* parent)
: QGCMapCircle (other, parent)
, _inclusion (other._inclusion)
{
_init();
}
void QGCFenceCircle::_init(void)
{
connect(this, &QGCFenceCircle::inclusionChanged, this, &QGCFenceCircle::_setDirty);
}
const QGCFenceCircle& QGCFenceCircle::operator=(const QGCFenceCircle& other)
{
QGCMapCircle::operator=(other);
setInclusion(other._inclusion);
return *this;
}
void QGCFenceCircle::_setDirty(void)
{
setDirty(true);
}
void QGCFenceCircle::saveToJson(QJsonObject& json)
{
QGCMapCircle::saveToJson(json);
json[_jsonInclusionKey] = _inclusion;
}
bool QGCFenceCircle::loadFromJson(const QJsonObject& json, QString& errorString)
{
if (!QGCMapCircle::loadFromJson(json, errorString)) {
return false;
}
errorString.clear();
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ _jsonInclusionKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
return false;
}
setInclusion(json[_jsonInclusionKey].toBool());
return true;
}
void QGCFenceCircle::setInclusion(bool inclusion)
{
if (inclusion != _inclusion) {
_inclusion = inclusion;
emit inclusionChanged(inclusion);
}
}
src/MissionManager/QGCFenceCircle.h 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include "QGCMapCircle.h"
/// The QGCFenceCircle class provides a cicle used by GeoFence support.
class QGCFenceCircle : public QGCMapCircle
{
Q_OBJECT
public:
QGCFenceCircle(QObject* parent = NULL);
QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent = NULL);
QGCFenceCircle(const QGCFenceCircle& other, QObject* parent = NULL);
const QGCFenceCircle& operator=(const QGCFenceCircle& other);
Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged)
/// Saves the QGCFenceCircle to the json object.
/// @param json Json object to save to
void saveToJson(QJsonObject& json);
/// Load a QGCFenceCircle from json
/// @param json Json object to load from
/// @param errorString Error string if return is false
/// @return true: success, false: failure (errorString set)
bool loadFromJson(const QJsonObject& json, QString& errorString);
// Property methods
bool inclusion (void) const { return _inclusion; }
void setInclusion (bool inclusion);
signals:
void inclusionChanged(bool inclusion);
private slots:
void _setDirty(void);
private:
void _init(void);
bool _inclusion;
static const char* _jsonInclusionKey;
};
src/MissionManager/QGCFencePolygon.cc 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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 "QGCFencePolygon.h"
#include "JsonHelper.h"
const char* QGCFencePolygon::_jsonInclusionKey = "inclusion";
QGCFencePolygon::QGCFencePolygon(bool inclusion, QObject* parent)
: QGCMapPolygon (parent)
, _inclusion (inclusion)
{
_init();
}
QGCFencePolygon::QGCFencePolygon(const QGCFencePolygon& other, QObject* parent)
: QGCMapPolygon (other, parent)
, _inclusion (other._inclusion)
{
_init();
}
void QGCFencePolygon::_init(void)
{
connect(this, &QGCFencePolygon::inclusionChanged, this, &QGCFencePolygon::_setDirty);
}
const QGCFencePolygon& QGCFencePolygon::operator=(const QGCFencePolygon& other)
{
QGCMapPolygon::operator=(other);
setInclusion(other._inclusion);
return *this;
}
void QGCFencePolygon::_setDirty(void)
{
setDirty(true);
}
void QGCFencePolygon::saveToJson(QJsonObject& json)
{
QGCMapPolygon::saveToJson(json);
json[_jsonInclusionKey] = _inclusion;
}
bool QGCFencePolygon::loadFromJson(const QJsonObject& json, bool required, QString& errorString)
{
if (!QGCMapPolygon::loadFromJson(json, required, errorString)) {
return false;
}
errorString.clear();
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ _jsonInclusionKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
return false;
}
setInclusion(json[_jsonInclusionKey].toBool());
return true;
}
void QGCFencePolygon::setInclusion(bool inclusion)
{
if (inclusion != _inclusion) {
_inclusion = inclusion;
emit inclusionChanged(inclusion);
}
}
src/MissionManager/QGCFencePolygon.h 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include "QGCMapPolygon.h"
/// The QGCFencePolygon class provides a polygon used by GeoFence support.
class QGCFencePolygon : public QGCMapPolygon
{
Q_OBJECT
public:
QGCFencePolygon(bool inclusion, QObject* parent = NULL);
QGCFencePolygon(const QGCFencePolygon& other, QObject* parent = NULL);
const QGCFencePolygon& operator=(const QGCFencePolygon& other);
Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged)
/// Saves the QGCFencePolygon to the json object.
/// @param json Json object to save to
void saveToJson(QJsonObject& json);
/// Load a QGCFencePolygon from json
/// @param json Json object to load from
/// @param required true: no polygon in object will generate error
/// @param errorString Error string if return is false
/// @return true: success, false: failure (errorString set)
bool loadFromJson(const QJsonObject& json, bool required, QString& errorString);
// Property methods
bool inclusion (void) const { return _inclusion; }
void setInclusion (bool inclusion);
signals:
void inclusionChanged (bool inclusion);
private slots:
void _setDirty(void);
private:
void _init(void);
bool _inclusion;
static const char* _jsonInclusionKey;
};
src/MissionManager/QGCMapCircle.Facts.json 0 → 100644
View file @ 537463c5
[
{
"name": "Radius",
"shortDescription": "Radius for geofence circle.",
"type": "double",
"decimalPlaces": 2,
"min": 0.1,
"units": "m"
}
]
src/MissionManager/QGCMapCircle.cc 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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 "QGCMapCircle.h"
#include "QGCGeo.h"
#include "JsonHelper.h"
#include "QGCQGeoCoordinate.h"
#include <QGeoRectangle>
#include <QDebug>
#include <QJsonArray>
const char* QGCMapCircle::jsonCircleKey = "circle";
const char* QGCMapCircle::_jsonCenterKey = "center";
const char* QGCMapCircle::_jsonRadiusKey = "radius";
const char* QGCMapCircle::_radiusFactName = "Radius";
QGCMapCircle::QGCMapCircle(QObject* parent)
: QObject (parent)
, _dirty (false)
, _interactive (false)
{
_init();
}
QGCMapCircle::QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent)
: QObject (parent)
, _dirty (false)
, _center (center)
, _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble)
, _interactive (false)
{
_radius.setRawValue(radius);
_init();
}
QGCMapCircle::QGCMapCircle(const QGCMapCircle& other, QObject* parent)
: QObject (parent)
, _dirty (false)
, _center (other._center)
, _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble)
, _interactive (false)
{
_radius.setRawValue(other._radius.rawValue());
_init();
}
const QGCMapCircle& QGCMapCircle::operator=(const QGCMapCircle& other)
{
setCenter(other._center);
_radius.setRawValue(other._radius.rawValue());
setDirty(true);
return *this;
}
void QGCMapCircle::_init(void)
{
_nameToMetaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/QGCMapCircle.Facts.json"), this);
_radius.setMetaData(_nameToMetaDataMap[_radiusFactName]);
connect(this, &QGCMapCircle::centerChanged, this, &QGCMapCircle::_setDirty);
connect(&_radius, &Fact::rawValueChanged, this, &QGCMapCircle::_setDirty);
}
void QGCMapCircle::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(dirty);
}
}
void QGCMapCircle::saveToJson(QJsonObject& json)
{
QJsonValue jsonValue;
QJsonObject circleObject;
JsonHelper::saveGeoCoordinate(_center, false /* writeAltitude*/, jsonValue);
circleObject.insert(_jsonCenterKey, jsonValue);
circleObject.insert(_jsonRadiusKey, _radius.rawValue().toDouble());
json.insert(jsonCircleKey, circleObject);
}
bool QGCMapCircle::loadFromJson(const QJsonObject& json, QString& errorString)
{
errorString.clear();
QList<JsonHelper::KeyValidateInfo> circleKeyInfo = {
{ jsonCircleKey, QJsonValue::Object, true },
};
if (!JsonHelper::validateKeys(json, circleKeyInfo, errorString)) {
return false;
}
QJsonObject circleObject = json[jsonCircleKey].toObject();
QList<JsonHelper::KeyValidateInfo> circleObjectKeyInfo = {
{ _jsonCenterKey, QJsonValue::Array, true },
{ _jsonRadiusKey, QJsonValue::Double, true },
};
if (!JsonHelper::validateKeys(circleObject, circleObjectKeyInfo, errorString)) {
return false;
}
QGeoCoordinate center;
if (!JsonHelper::loadGeoCoordinate(json[_jsonCenterKey], false /* altitudeRequired */, center, errorString)) {
return false;
}
setCenter(center);
_radius.setRawValue(json[_jsonRadiusKey].toDouble());
return true;
}
void QGCMapCircle::setCenter(QGeoCoordinate newCenter)
{
if (newCenter != _center) {
_center = newCenter;
setDirty(true);
emit centerChanged(newCenter);
}
}
void QGCMapCircle::_setDirty(void)
{
setDirty(true);
}
void QGCMapCircle::setInteractive(bool interactive)
{
if (_interactive != interactive) {
_interactive = interactive;
emit interactiveChanged(interactive);
}
}
src/MissionManager/QGCMapCircle.h 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include <QObject>
#include <QGeoCoordinate>
#include <QVariantList>
#include <QPolygon>
#include "QmlObjectListModel.h"
#include "FactSystem.h"
/// The QGCMapCircle represents a circular area which can be displayed on a Map control.
class QGCMapCircle : public QObject
{
Q_OBJECT
public:
QGCMapCircle(QObject* parent = NULL);
QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent = NULL);
QGCMapCircle(const QGCMapCircle& other, QObject* parent = NULL);
const QGCMapCircle& operator=(const QGCMapCircle& other);
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged)
Q_PROPERTY(Fact* radius READ radius CONSTANT)
Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged)
/// Saves the polygon to the json object.
/// @param json Json object to save to
void saveToJson(QJsonObject& json);
/// Load a circle from json
/// @param json Json object to load from
/// @param errorString Error string if return is false
/// @return true: success, false: failure (errorString set)
bool loadFromJson(const QJsonObject& json, QString& errorString);
// Property methods
bool dirty (void) const { return _dirty; }
QGeoCoordinate center (void) const { return _center; }
Fact* radius (void) { return &_radius; }
bool interactive (void) const { return _interactive; }
void setDirty (bool dirty);
void setCenter (QGeoCoordinate newCenter);
void setInteractive (bool interactive);
static const char* jsonCircleKey;
signals:
void dirtyChanged (bool dirty);
void centerChanged (QGeoCoordinate center);
void interactiveChanged (bool interactive);
private slots:
void _setDirty(void);
private:
void _init(void);
bool _dirty;
QGeoCoordinate _center;
Fact _radius;
bool _interactive;
QMap<QString, FactMetaData*> _nameToMetaDataMap;
static const char* _jsonCenterKey;
static const char* _jsonRadiusKey;
static const char* _radiusFactName;
};
src/MissionManager/QGCMapCircleVisuals.qml 0 → 100644
View file @ 537463c5
/****************************************************************************
*
* (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.
*
****************************************************************************/
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtLocation 5.3
import QtPositioning 5.3
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
import QGroundControl.Controls 1.0
import QGroundControl.FlightMap 1.0
/// QGCMapCircle map visuals
Item {
id: _root
property var mapControl ///< Map control to place item in
property var mapCircle ///< QGCMapCircle object
property bool interactive: mapCircle.interactive /// true: user can manipulate polygon
property color interiorColor: "transparent"
property real interiorOpacity: 1
property int borderWidth: 0
property color borderColor: "black"
property var _circleComponent
property var _centerDragHandleComponent
function addVisuals() {
_circleComponent = circleComponent.createObject(mapControl)
mapControl.addMapItem(_circleComponent)
}
function removeVisuals() {
_circleComponent.destroy()
}
function addHandles() {
if (!_centerDragHandleComponent) {
_centerDragHandleComponent = centerDragHandleComponent.createObject(mapControl)
}
}
function removeHandles() {
if (_centerDragHandleComponent) {
_centerDragHandleComponent.destroy()
_centerDragHandleComponent = undefined
}
}
onInteractiveChanged: {
if (interactive) {
addHandles()
} else {
removeHandles()
}
}
Component.onCompleted: {
addVisuals()
if (interactive) {
addHandles()
}
}
Component.onDestruction: {
removeVisuals()
removeHandles()
}
Component {
id: circleComponent
MapCircle {
color: interiorColor
opacity: interiorOpacity
border.color: borderColor
border.width: borderWidth
center: mapCircle.center
radius: mapCircle.radius.rawValue
}
}
Component {
id: dragHandleComponent
MapQuickItem {
id: mapQuickItem
anchorPoint.x: dragHandle.width / 2
anchorPoint.y: dragHandle.height / 2
z: QGroundControl.zOrderMapItems + 2
sourceItem: Rectangle {
id: dragHandle
width: ScreenTools.defaultFontPixelHeight * 1.5
height: width
radius: width / 2
color: "white"
opacity: .90
}
}
}
Component {
id: centerDragAreaComponent
MissionItemIndicatorDrag {
onItemCoordinateChanged: mapCircle.center = itemCoordinate
}
}
Component {
id: centerDragHandleComponent
Item {
property var dragHandle
property var dragArea
Component.onCompleted: {
dragHandle = dragHandleComponent.createObject(mapControl)
dragHandle.coordinate = Qt.binding(function() { return mapCircle.center })
mapControl.addMapItem(dragHandle)
dragArea = centerDragAreaComponent.createObject(mapControl, { "itemIndicator": dragHandle, "itemCoordinate": mapCircle.center })
}
Component.onDestruction: {
dragHandle.destroy()
dragArea.destroy()
}
}
}
}
src/MissionManager/QGCMapPolygon.cc
View file @ 537463c5
...@@ -18,18 +18,49 @@ ...@@ -18,18 +18,49 @@
const char* QGCMapPolygon::jsonPolygonKey = "polygon"; const char* QGCMapPolygon::jsonPolygonKey = "polygon";
QGCMapPolygon::QGCMapPolygon(QObject* newCoordParent, QObject* parent) QGCMapPolygon::QGCMapPolygon(QObject* parent)
: QObject(parent) : QObject (parent)
, _newCoordParent(newCoordParent) , _dirty (false)
, _dirty(false) , _centerDrag (false)
, _centerDrag(false) , _ignoreCenterUpdates (false)
, _ignoreCenterUpdates(false) , _interactive (false)
{
_init();
}
QGCMapPolygon::QGCMapPolygon(const QGCMapPolygon& other, QObject* parent)
: QObject (parent)
, _dirty (false)
, _centerDrag (false)
, _ignoreCenterUpdates (false)
, _interactive (false)
{
*this = other;
_init();
}
void QGCMapPolygon::_init(void)
{ {
connect(&_polygonModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolygon::_polygonModelDirtyChanged); connect(&_polygonModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolygon::_polygonModelDirtyChanged);
connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged); connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged);
connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_updateCenter); connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_updateCenter);
} }
const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other)
{
clear();
QVariantList vertices = other.path();
for (int i=0; i<vertices.count(); i++) {
appendVertex(vertices[i].value<QGeoCoordinate>());
}
setDirty(true);
return *this;
}
void QGCMapPolygon::clear(void) void QGCMapPolygon::clear(void)
{ {
// Bug workaround, see below // Bug workaround, see below
...@@ -132,7 +163,7 @@ void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path) ...@@ -132,7 +163,7 @@ void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path)
_polygonModel.clearAndDeleteContents(); _polygonModel.clearAndDeleteContents();
foreach(const QGeoCoordinate& coord, path) { foreach(const QGeoCoordinate& coord, path) {
_polygonPath.append(QVariant::fromValue(coord)); _polygonPath.append(QVariant::fromValue(coord));
_polygonModel.append(new QGCQGeoCoordinate(coord, _newCoordParent)); _polygonModel.append(new QGCQGeoCoordinate(coord, this));
} }
setDirty(true); setDirty(true);
...@@ -145,7 +176,7 @@ void QGCMapPolygon::setPath(const QVariantList& path) ...@@ -145,7 +176,7 @@ void QGCMapPolygon::setPath(const QVariantList& path)
_polygonModel.clearAndDeleteContents(); _polygonModel.clearAndDeleteContents();
for (int i=0; i<_polygonPath.count(); i++) { for (int i=0; i<_polygonPath.count(); i++) {
_polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), _newCoordParent)); _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), this));
} }
setDirty(true); setDirty(true);
...@@ -179,7 +210,7 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString ...@@ -179,7 +210,7 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString
} }
for (int i=0; i<_polygonPath.count(); i++) { for (int i=0; i<_polygonPath.count(); i++) {
_polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), _newCoordParent)); _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), this));
} }
setDirty(false); setDirty(false);
...@@ -225,7 +256,7 @@ void QGCMapPolygon::splitPolygonSegment(int vertexIndex) ...@@ -225,7 +256,7 @@ void QGCMapPolygon::splitPolygonSegment(int vertexIndex)
void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate) void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate)
{ {
_polygonPath.append(QVariant::fromValue(coordinate)); _polygonPath.append(QVariant::fromValue(coordinate));
_polygonModel.append(new QGCQGeoCoordinate(coordinate, _newCoordParent)); _polygonModel.append(new QGCQGeoCoordinate(coordinate, this));
emit pathChanged(); emit pathChanged();
} }
...@@ -315,3 +346,11 @@ void QGCMapPolygon::setCenterDrag(bool centerDrag) ...@@ -315,3 +346,11 @@ void QGCMapPolygon::setCenterDrag(bool centerDrag)
emit centerDragChanged(centerDrag); emit centerDragChanged(centerDrag);
} }
} }
void QGCMapPolygon::setInteractive(bool interactive)
{
if (_interactive != interactive) {
_interactive = interactive;
emit interactiveChanged(interactive);
}
}
src/MissionManager/QGCMapPolygon.h
View file @ 537463c5
...@@ -24,14 +24,18 @@ class QGCMapPolygon : public QObject ...@@ -24,14 +24,18 @@ class QGCMapPolygon : public QObject
Q_OBJECT Q_OBJECT
public: public:
QGCMapPolygon(QObject* newCoordParent, QObject* parent = NULL); QGCMapPolygon(QObject* parent = NULL);
QGCMapPolygon(const QGCMapPolygon& other, QObject* parent = NULL);
Q_PROPERTY(int count READ count NOTIFY countChanged) const QGCMapPolygon& operator=(const QGCMapPolygon& other);
Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT) Q_PROPERTY(int count READ count NOTIFY countChanged)
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged) Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT)
Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged) Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged)
Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged)
Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged)
Q_INVOKABLE void clear(void); Q_INVOKABLE void clear(void);
Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate); Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate);
...@@ -69,6 +73,7 @@ public: ...@@ -69,6 +73,7 @@ public:
void setDirty (bool dirty); void setDirty (bool dirty);
QGeoCoordinate center (void) const { return _center; } QGeoCoordinate center (void) const { return _center; }
bool centerDrag (void) const { return _centerDrag; } bool centerDrag (void) const { return _centerDrag; }
bool interactive (void) const { return _interactive; }
QVariantList path (void) const { return _polygonPath; } QVariantList path (void) const { return _polygonPath; }
QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; } QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; }
...@@ -78,6 +83,7 @@ public: ...@@ -78,6 +83,7 @@ public:
void setPath (const QVariantList& path); void setPath (const QVariantList& path);
void setCenter (QGeoCoordinate newCenter); void setCenter (QGeoCoordinate newCenter);
void setCenterDrag (bool centerDrag); void setCenterDrag (bool centerDrag);
void setInteractive (bool interactive);
static const char* jsonPolygonKey; static const char* jsonPolygonKey;
...@@ -88,6 +94,7 @@ signals: ...@@ -88,6 +94,7 @@ signals:
void cleared (void); void cleared (void);
void centerChanged (QGeoCoordinate center); void centerChanged (QGeoCoordinate center);
void centerDragChanged (bool centerDrag); void centerDragChanged (bool centerDrag);
void interactiveChanged (bool interactive);
private slots: private slots:
void _polygonModelCountChanged(int count); void _polygonModelCountChanged(int count);
...@@ -95,17 +102,18 @@ private slots: ...@@ -95,17 +102,18 @@ private slots:
void _updateCenter(void); void _updateCenter(void);
private: private:
void _init(void);
QPolygonF _toPolygonF(void) const; QPolygonF _toPolygonF(void) const;
QGeoCoordinate _coordFromPointF(const QPointF& point) const; QGeoCoordinate _coordFromPointF(const QPointF& point) const;
QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const; QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const;
QObject* _newCoordParent;
QVariantList _polygonPath; QVariantList _polygonPath;
QmlObjectListModel _polygonModel; QmlObjectListModel _polygonModel;
bool _dirty; bool _dirty;
QGeoCoordinate _center; QGeoCoordinate _center;
bool _centerDrag; bool _centerDrag;
bool _ignoreCenterUpdates; bool _ignoreCenterUpdates;
bool _interactive;
}; };
#endif #endif
src/MissionManager/QGCMapPolygonTest.cc
View file @ 537463c5
...@@ -32,7 +32,7 @@ void QGCMapPolygonTest::init(void) ...@@ -32,7 +32,7 @@ void QGCMapPolygonTest::init(void)
_rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int)); _rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int));
_rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); _rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
_mapPolygon = new QGCMapPolygon(this, this); _mapPolygon = new QGCMapPolygon(this);
_pathModel = _mapPolygon->qmlPathModel(); _pathModel = _mapPolygon->qmlPathModel();
QVERIFY(_pathModel); QVERIFY(_pathModel);
......
src/MissionManager/QGCMapPolygonVisuals.qml
View file @ 537463c5
...@@ -24,16 +24,16 @@ Item { ...@@ -24,16 +24,16 @@ Item {
property var mapControl ///< Map control to place item in property var mapControl ///< Map control to place item in
property var mapPolygon ///< QGCMapPolygon object property var mapPolygon ///< QGCMapPolygon object
property bool interactive: true /// true: user can manipulate polygon property bool interactive: mapPolygon.interactive
property color interiorColor: "transparent" property color interiorColor: "transparent"
property real interiorOpacity: 1 property real interiorOpacity: 1
property int borderWidth: 0 property int borderWidth: 0
property color borderColor: "black" property color borderColor: "black"
property var _polygonComponent property var _polygonComponent
property var _dragHandlesComponent property var _dragHandlesComponent
property var _splitHandlesComponent property var _splitHandlesComponent
property var _centerDragHandleComponent property var _centerDragHandleComponent
property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap
property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2 property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2
...@@ -111,7 +111,7 @@ Item { ...@@ -111,7 +111,7 @@ Item {
} }
Component.onCompleted: { Component.onCompleted: {
mapPolygonVisuals.addVisuals() addVisuals()
if (interactive) { if (interactive) {
addHandles() addHandles()
} }
......
src/MissionManager/RallyPointController.cc
View file @ 537463c5
...@@ -58,6 +58,7 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) ...@@ -58,6 +58,7 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
{ {
if (_managerVehicle) { if (_managerVehicle) {
_rallyPointManager->disconnect(this); _rallyPointManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle = NULL; _managerVehicle = NULL;
_rallyPointManager = NULL; _rallyPointManager = NULL;
} }
...@@ -74,7 +75,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) ...@@ -74,7 +75,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete); connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete);
connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged);
emit rallyPointsSupportedChanged(rallyPointsSupported()); connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
emit supportedChanged(supported());
} }
bool RallyPointController::load(const QJsonObject& json, QString& errorString) bool RallyPointController::load(const QJsonObject& json, QString& errorString)
...@@ -234,9 +237,9 @@ void RallyPointController::addPoint(QGeoCoordinate point) ...@@ -234,9 +237,9 @@ void RallyPointController::addPoint(QGeoCoordinate point)
setDirty(true); setDirty(true);
} }
bool RallyPointController::rallyPointsSupported(void) const bool RallyPointController::supported(void) const
{ {
return _rallyPointManager->rallyPointsSupported(); return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200);
} }
void RallyPointController::removePoint(QObject* rallyPoint) void RallyPointController::removePoint(QObject* rallyPoint)
......
src/MissionManager/RallyPointController.h
View file @ 537463c5
...@@ -29,7 +29,6 @@ public: ...@@ -29,7 +29,6 @@ public:
RallyPointController(PlanMasterController* masterController, QObject* parent = NULL); RallyPointController(PlanMasterController* masterController, QObject* parent = NULL);
~RallyPointController(); ~RallyPointController();
Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged)
Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT) Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged) Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged)
...@@ -37,6 +36,7 @@ public: ...@@ -37,6 +36,7 @@ public:
Q_INVOKABLE void addPoint(QGeoCoordinate point); Q_INVOKABLE void addPoint(QGeoCoordinate point);
Q_INVOKABLE void removePoint(QObject* rallyPoint); Q_INVOKABLE void removePoint(QObject* rallyPoint);
bool supported (void) const final;
void save (QJsonObject& json) final; void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final; bool load (const QJsonObject& json, QString& errorString) final;
void loadFromVehicle (void) final; void loadFromVehicle (void) final;
...@@ -50,7 +50,6 @@ public: ...@@ -50,7 +50,6 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final; void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final; bool showPlanFromManagerVehicle (void) final;
bool rallyPointsSupported (void) const;
QmlObjectListModel* points (void) { return &_points; } QmlObjectListModel* points (void) { return &_points; }
QString editorQml (void) const; QString editorQml (void) const;
QObject* currentRallyPoint (void) const { return _currentRallyPoint; } QObject* currentRallyPoint (void) const { return _currentRallyPoint; }
...@@ -58,7 +57,6 @@ public: ...@@ -58,7 +57,6 @@ public:
void setCurrentRallyPoint(QObject* rallyPoint); void setCurrentRallyPoint(QObject* rallyPoint);
signals: signals:
void rallyPointsSupportedChanged(bool rallyPointsSupported);
void currentRallyPointChanged(QObject* rallyPoint); void currentRallyPointChanged(QObject* rallyPoint);
void loadComplete(void); void loadComplete(void);
......
src/MissionManager/RallyPointManager.cc
View file @ 537463c5
...@@ -49,3 +49,8 @@ void RallyPointManager::removeAll(void) ...@@ -49,3 +49,8 @@ void RallyPointManager::removeAll(void)
// No support in generic vehicle // No support in generic vehicle
emit removeAllComplete(false /* error */); emit removeAllComplete(false /* error */);
} }
bool RallyPointManager::supported(void) const
{
return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
}
src/MissionManager/RallyPointManager.h
View file @ 537463c5
...@@ -29,6 +29,9 @@ public: ...@@ -29,6 +29,9 @@ public:
RallyPointManager(Vehicle* vehicle); RallyPointManager(Vehicle* vehicle);
~RallyPointManager(); ~RallyPointManager();
/// 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 /// Returns true if the manager is currently communicating with the vehicle
virtual bool inProgress(void) const { return false; } virtual bool inProgress(void) const { return false; }
...@@ -44,8 +47,6 @@ public: ...@@ -44,8 +47,6 @@ public:
/// Signals removeAllCompleted when done /// Signals removeAllCompleted when done
virtual void removeAll(void); virtual void removeAll(void);
virtual bool rallyPointsSupported(void) const { return false; }
QList<QGeoCoordinate> points(void) const { return _rgPoints; } QList<QGeoCoordinate> points(void) const { return _rgPoints; }
virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); } virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); }
......
src/PlanView/GeoFenceEditor.qml
View file @ 537463c5
import QtQuick 2.3 import QtQuick 2.3
import QtQuick.Controls 1.2 import QtQuick.Controls 1.2
import QtQuick.Layouts 1.2
import QGroundControl 1.0 import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0 import QGroundControl.ScreenTools 1.0
...@@ -10,8 +11,8 @@ import QGroundControl.FactControls 1.0 ...@@ -10,8 +11,8 @@ import QGroundControl.FactControls 1.0
QGCFlickable { QGCFlickable {
id: root id: root
width: availableWidth width: availableWidth
height: Math.min(availableHeight, geoFenceEditorRect.height) height: availableHeight
contentHeight: geoFenceEditorRect.height contentHeight: editorColumn.height
clip: true clip: true
property real availableWidth property real availableWidth
...@@ -23,99 +24,317 @@ QGCFlickable { ...@@ -23,99 +24,317 @@ QGCFlickable {
readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2
readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2
property var polygon: myGeoFenceController.polygon Column {
id: editorColumn
anchors.left: parent.left
anchors.right: parent.right
Rectangle { Rectangle {
id: geoFenceEditorRect id: geoFenceEditorRect
width: parent.width width: parent.width
height: geoFenceItems.y + geoFenceItems.height + (_margin * 2) height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
radius: _radius radius: _radius
color: qgcPal.missionItemEditor color: qgcPal.missionItemEditor
QGCLabel { QGCLabel {
id: geoFenceLabel id: geoFenceLabel
anchors.margins: _margin anchors.margins: _margin
anchors.left: parent.left anchors.left: parent.left
anchors.top: parent.top anchors.top: parent.top
text: qsTr("GeoFence") text: qsTr("GeoFence")
color: "black" color: "black"
} }
Rectangle { Rectangle {
id: geoFenceItems id: geoFenceItems
anchors.margins: _margin
anchors.left: parent.left
anchors.right: parent.right
anchors.top: geoFenceLabel.bottom
height: fenceColumn.y + fenceColumn.height + (_margin * 2)
color: qgcPal.windowShadeDark
radius: _radius
Column {
id: fenceColumn
anchors.margins: _margin anchors.margins: _margin
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight / 2 anchors.top: geoFenceLabel.bottom
height: fenceColumn.y + fenceColumn.height + (_margin * 2)
color: qgcPal.windowShadeDark
radius: _radius
QGCLabel { Column {
id: geoLabel id: fenceColumn
anchors.margins: _margin
anchors.top: parent.top
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
wrapMode: Text.WordWrap spacing: ScreenTools.defaultFontPixelHeight / 2
font.pointSize: ScreenTools.smallFontPointSize
text: qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.")
}
Repeater { QGCLabel {
model: myGeoFenceController.params anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
font.pointSize: myGeoFenceController.supported ? ScreenTools.smallFontPointSize : ScreenTools.defaultFontPointSize
text: myGeoFenceController.supported ?
qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.") :
qsTr("This vehicle does not support GeoFence.")
}
Item { Column {
width: fenceColumn.width anchors.left: parent.left
height: textField.height anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight / 2
visible: myGeoFenceController.supported
property bool showCombo: modelData.enumStrings.length > 0 Repeater {
model: myGeoFenceController.params
QGCLabel { Item {
id: textFieldLabel width: fenceColumn.width
anchors.baseline: textField.baseline height: textField.height
text: myGeoFenceController.paramLabels[index]
property bool showCombo: modelData.enumStrings.length > 0
QGCLabel {
id: textFieldLabel
anchors.baseline: textField.baseline
text: myGeoFenceController.paramLabels[index]
}
FactTextField {
id: textField
anchors.right: parent.right
width: _editFieldWidth
showUnits: true
fact: modelData
visible: !parent.showCombo
}
FactComboBox {
id: comboField
anchors.right: parent.right
width: _editFieldWidth
indexModel: false
fact: showCombo ? modelData : _nullFact
visible: parent.showCombo
property var _nullFact: Fact { }
}
}
}
SectionHeader {
id: insertSection
text: qsTr("Insert GeoFence")
} }
FactTextField { QGCButton {
id: textField anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
width: _editFieldWidth text: qsTr("Polygon Fence")
showUnits: true
fact: modelData onClicked: {
visible: !parent.showCombo var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height)
var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
myGeoFenceController.addInclusionPolygon(topLeftCoord, bottomRightCoord)
}
} }
FactComboBox { QGCButton {
id: comboField anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
width: _editFieldWidth text: qsTr("Circular Fence")
indexModel: false
fact: showCombo ? modelData : _nullFact
visible: parent.showCombo
property var _nullFact: Fact { } onClicked: {
var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height)
var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
myGeoFenceController.addInclusionCircle(topLeftCoord, bottomRightCoord)
}
} }
}
}
QGCButton { SectionHeader {
text: qsTr("Add fence polygon") id: polygonSection
visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count === 0 text: qsTr("Polygon Fences")
onClicked: myGeoFenceController.addPolygon() }
}
QGCLabel {
text: qsTr("None")
visible: polygonSection.checked && myGeoFenceController.polygons.count == 0
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
columns: 3
flow: GridLayout.TopToBottom
visible: polygonSection.checked && myGeoFenceController.polygons.count > 0
QGCLabel {
text: qsTr("Inclusion")
Layout.column: 0
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.polygons
QGCCheckBox {
checked: object.inclusion
onClicked: object.inclusion = checked
Layout.alignment: Qt.AlignHCenter
}
}
QGCLabel {
text: qsTr("Edit")
Layout.column: 1
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.polygons
QGCRadioButton {
checked: _interactive
Layout.alignment: Qt.AlignHCenter
property bool _interactive: object.interactive
QGCButton { on_InteractiveChanged: checked = _interactive
text: qsTr("Remove fence polygon")
visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count > 0 onClicked: {
onClicked: myGeoFenceController.removePolygon() myGeoFenceController.clearAllInteractive()
object.interactive = checked
}
}
}
QGCLabel {
text: qsTr("Delete")
Layout.column: 2
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.polygons
QGCColoredImage {
width: ScreenTools.defaultFontPixelHeight
height: width
sourceSize.height: width
source: "/res/XDelete.svg"
fillMode: Image.PreserveAspectFit
color: qgcPal.text
Layout.alignment: Qt.AlignHCenter
property int _polygonIndex: index
QGCMouseArea {
fillItem: parent
onClicked: myGeoFenceController.deletePolygon(parent._polygonIndex)
}
}
}
} // GridLayout
SectionHeader {
id: circleSection
text: qsTr("Circular Fences")
}
QGCLabel {
text: qsTr("None")
visible: circleSection.checked && myGeoFenceController.circles.count == 0
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
columns: 4
flow: GridLayout.TopToBottom
visible: polygonSection.checked && myGeoFenceController.circles.count > 0
QGCLabel {
text: qsTr("Inclusion")
Layout.column: 0
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.circles
QGCCheckBox {
checked: object.inclusion
onClicked: object.inclusion = checked
Layout.alignment: Qt.AlignHCenter
}
}
QGCLabel {
text: qsTr("Edit")
Layout.column: 1
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.circles
QGCRadioButton {
checked: _interactive
Layout.alignment: Qt.AlignHCenter
property bool _interactive: object.interactive
on_InteractiveChanged: checked = _interactive
onClicked: {
myGeoFenceController.clearAllInteractive()
object.interactive = checked
}
}
}
QGCLabel {
text: qsTr("Radius")
Layout.column: 2
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.circles
FactTextField {
fact: object.radius
Layout.fillWidth: true
Layout.alignment: Qt.AlignHCenter
}
}
QGCLabel {
text: qsTr("Delete")
Layout.column: 3
Layout.alignment: Qt.AlignHCenter
}
Repeater {
model: myGeoFenceController.circles
QGCColoredImage {
width: ScreenTools.defaultFontPixelHeight
height: width
sourceSize.height: width
source: "/res/XDelete.svg"
fillMode: Image.PreserveAspectFit
color: qgcPal.text
Layout.alignment: Qt.AlignHCenter
property int _circleIndex: index
QGCMouseArea {
fillItem: parent
onClicked: myGeoFenceController.deleteCircle(parent._polygonIndex)
}
}
}
} // GridLayout
}
} }
} }
} } // Rectangle
} }
} }
src/PlanView/GeoFenceMapVisuals.qml
View file @ 537463c5
...@@ -30,34 +30,53 @@ Item { ...@@ -30,34 +30,53 @@ Item {
property var _breachReturnPointComponent property var _breachReturnPointComponent
property var _mouseAreaComponent property var _mouseAreaComponent
property var _circleComponent property var _polygons: myGeoFenceController.polygons
property var _mapPolygon: myGeoFenceController.mapPolygon property var _circles: myGeoFenceController.circles
property bool _interactive: interactive
property bool _circleSupported: myGeoFenceController.circleRadiusFact !== null function addPolygon(inclusionPolygon) {
property bool _circleEnabled: myGeoFenceController.circleEnabled // Initial polygon is inset to take 2/3rds space
property real _circleRadius: _circleSupported ? myGeoFenceController.circleRadiusFact.rawValue : 0 var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height)
property bool _polygonSupported: myGeoFenceController.polygonSupported rect.x += (rect.width * 0.25) / 2
property bool _polygonEnabled: myGeoFenceController.polygonEnabled rect.y += (rect.height * 0.25) / 2
rect.width *= 0.75
rect.height *= 0.75
var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */)
var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */)
var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */)
var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
// Initial polygon has max width and height of 3000 meters
var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2
var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2
topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0)
topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0)
bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180)
bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180)
console.log(map.center)
console.log(topLeftCoord)
console.log(bottomRightCoord)
if (inclusionPolygon) {
myGeoFenceController.addInclusion(topLeftCoord, bottomRightCoord)
} else {
myGeoFenceController.addExclusion(topLeftCoord, bottomRightCoord)
}
}
Component.onCompleted: { Component.onCompleted: {
_circleComponent = circleComponent.createObject(map)
map.addMapItem(_circleComponent)
_breachReturnPointComponent = breachReturnPointComponent.createObject(map) _breachReturnPointComponent = breachReturnPointComponent.createObject(map)
map.addMapItem(_breachReturnPointComponent) map.addMapItem(_breachReturnPointComponent)
_mouseAreaComponent = mouseAreaComponent.createObject(map) _mouseAreaComponent = mouseAreaComponent.createObject(map)
} }
Component.onDestruction: { Component.onDestruction: {
_circleComponent.destroy()
_breachReturnPointComponent.destroy() _breachReturnPointComponent.destroy()
_mouseAreaComponent.destroy() _mouseAreaComponent.destroy()
} }
Connections {
target: myGeoFenceController
onAddInitialFencePolygon: mapPolygonVisuals.addInitialPolygon()
}
// Mouse area to capture breach return point coordinate // Mouse area to capture breach return point coordinate
Component { Component {
id: mouseAreaComponent id: mouseAreaComponent
...@@ -69,28 +88,29 @@ Item { ...@@ -69,28 +88,29 @@ Item {
} }
} }
QGCMapPolygonVisuals { Instantiator {
id: mapPolygonVisuals model: _polygons
mapControl: map
mapPolygon: _mapPolygon delegate : QGCMapPolygonVisuals {
interactive: _interactive mapControl: map
borderWidth: 2 mapPolygon: object
borderColor: "orange" borderWidth: object.inclusion ? 2 : 0
visible: _polygonSupported && (planView || _polygonEnabled) borderColor: "orange"
interiorColor: object.inclusion ? "transparent" : "orange"
interiorOpacity: object.inclusion ? 1: 0.2
}
} }
// GeoFence circle Instantiator {
Component { model: _circles
id: circleComponent
MapCircle { delegate : QGCMapCircleVisuals {
z: QGroundControl.zOrderMapItems mapControl: map
border.width: 2 mapCircle: object
border.color: "orange" borderWidth: object.inclusion ? 2 : 0
color: "transparent" borderColor: "orange"
center: homePosition ? homePosition : QtPositioning.coordinate() interiorColor: object.inclusion ? "transparent" : "orange"
radius: _circleRadius interiorOpacity: object.inclusion ? 1: 0.2
visible: _circleSupported && _circleRadius > 0 && (planView || _circleEnabled)
} }
} }
......
src/PlanView/PlanView.qml
View file @ 537463c5
...@@ -335,7 +335,7 @@ QGCView { ...@@ -335,7 +335,7 @@ QGCView {
} }
break break
case _layerRallyPoints: case _layerRallyPoints:
if (_rallyPointController.rallyPointsSupported) { if (_rallyPointController.supported) {
_rallyPointController.addPoint(coordinate) _rallyPointController.addPoint(coordinate)
} }
break break
......
src/PlanView/RallyPointEditorHeader.qml
View file @ 537463c5
...@@ -59,7 +59,7 @@ QGCFlickable { ...@@ -59,7 +59,7 @@ QGCFlickable {
anchors.right: parent.right anchors.right: parent.right
anchors.top: infoLabel.bottom anchors.top: infoLabel.bottom
wrapMode: Text.WordWrap wrapMode: Text.WordWrap
text: controller.rallyPointsSupported ? text: controller.supported ?
qsTr("Click in the map to add new rally points.") : qsTr("Click in the map to add new rally points.") :
qsTr("This vehicle does not support Rally Points.") qsTr("This vehicle does not support Rally Points.")
} }
......
src/PlanView/RallyPointMapVisuals.qml
View file @ 537463c5
...@@ -29,7 +29,7 @@ Item { ...@@ -29,7 +29,7 @@ Item {
property bool _interactive: interactive property bool _interactive: interactive
property var _rallyPointsComponent property var _rallyPointsComponent
property bool _rallyPointsSupported: myRallyPointController.rallyPointsSupported property bool _rallyPointsSupported: myRallyPointController.supported
property var _rallyPoints: myRallyPointController.points property var _rallyPoints: myRallyPointController.points
Component.onCompleted: { Component.onCompleted: {
......
src/QmlControls/QGroundControl.Controls.qmldir
View file @ 537463c5
...@@ -43,6 +43,7 @@ QGCGroupBox 1.0 QGCGroupBox.qml ...@@ -43,6 +43,7 @@ QGCGroupBox 1.0 QGCGroupBox.qml
QGCLabel 1.0 QGCLabel.qml QGCLabel 1.0 QGCLabel.qml
QGCListView 1.0 QGCListView.qml QGCListView 1.0 QGCListView.qml
QGCMapLabel 1.0 QGCMapLabel.qml QGCMapLabel 1.0 QGCMapLabel.qml
QGCMapCircleVisuals 1.0 QGCMapCircleVisuals.qml
QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml
QGCMouseArea 1.0 QGCMouseArea.qml QGCMouseArea 1.0 QGCMouseArea.qml
QGCMovableItem 1.0 QGCMovableItem.qml QGCMovableItem 1.0 QGCMovableItem.qml
......
src/Vehicle/Vehicle.cc
View file @ 537463c5
...@@ -116,7 +116,7 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -116,7 +116,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _telemetryRNoise(0) , _telemetryRNoise(0)
, _maxProtoVersion(0) , _maxProtoVersion(0)
, _vehicleCapabilitiesKnown(false) , _vehicleCapabilitiesKnown(false)
, _supportsMissionItemInt(false) , _capabilityBits(0)
, _connectionLost(false) , _connectionLost(false)
, _connectionLostEnabled(true) , _connectionLostEnabled(true)
, _initialPlanRequestComplete(false) , _initialPlanRequestComplete(false)
...@@ -281,7 +281,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, ...@@ -281,7 +281,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble()) , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
, _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
, _vehicleCapabilitiesKnown(true) , _vehicleCapabilitiesKnown(true)
, _supportsMissionItemInt(false) , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
, _connectionLost(false) , _connectionLost(false)
, _connectionLostEnabled(true) , _connectionLostEnabled(true)
, _initialPlanRequestComplete(false) , _initialPlanRequestComplete(false)
...@@ -350,11 +350,11 @@ void Vehicle::_commonInit(void) ...@@ -350,11 +350,11 @@ void Vehicle::_commonInit(void)
connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
// GeoFenceManager needs to access ParameterManager so make sure to create after // GeoFenceManager needs to access ParameterManager so make sure to create after
_geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); _geoFenceManager = new GeoFenceManager(this);
connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete); connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete);
_rallyPointManager = _firmwarePlugin->newRallyPointManager(this); _rallyPointManager = new RallyPointManager(this);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete); connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete);
...@@ -416,6 +416,12 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value) ...@@ -416,6 +416,12 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{ {
_firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt()); _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
emit firmwareTypeChanged(); emit firmwareTypeChanged();
if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
_capabilityBits = 0;
} else {
_capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
}
emit capabilityBitsChanged(_capabilityBits);
} }
void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value) void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value)
...@@ -749,11 +755,10 @@ void Vehicle::_handleAltitude(mavlink_message_t& message) ...@@ -749,11 +755,10 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
void Vehicle::_setCapabilities(uint64_t capabilityBits) void Vehicle::_setCapabilities(uint64_t capabilityBits)
{ {
if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { _capabilityBits = capabilityBits;
_supportsMissionItemInt = true;
}
_vehicleCapabilitiesKnown = true; _vehicleCapabilitiesKnown = true;
emit capabilitiesKnownChanged(true); emit capabilitiesKnownChanged(true);
emit capabilityBitsChanged(_capabilityBits);
// This should potentially be turned into a user-facing warning // This should potentially be turned into a user-facing warning
// if the general experience after deployment is that users want MAVLink 2 // if the general experience after deployment is that users want MAVLink 2
...@@ -762,7 +767,13 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) ...@@ -762,7 +767,13 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits)
qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it."); qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it.");
} }
qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support")); QString supports("supports");
QString doesNotSupport("does not support");
qCDebug(VehicleLog) << QString("Vehicle %1 Mavlink 2.0").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? supports : doesNotSupport);
qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT ? supports : doesNotSupport);
qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport);
qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport);
} }
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
...@@ -1908,8 +1919,13 @@ void Vehicle::_missionLoadComplete(void) ...@@ -1908,8 +1919,13 @@ void Vehicle::_missionLoadComplete(void)
// After the initial mission request completes we ask for the geofence // After the initial mission request completes we ask for the geofence
if (!_geoFenceManagerInitialRequestSent) { if (!_geoFenceManagerInitialRequestSent) {
_geoFenceManagerInitialRequestSent = true; _geoFenceManagerInitialRequestSent = true;
qCDebug(VehicleLog) << "_missionLoadComplete requesting geoFence"; if (_geoFenceManager->supported()) {
_geoFenceManager->loadFromVehicle(); qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
_geoFenceManager->loadFromVehicle();
} else {
qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
_geoFenceLoadComplete();
}
} }
} }
...@@ -1918,15 +1934,23 @@ void Vehicle::_geoFenceLoadComplete(void) ...@@ -1918,15 +1934,23 @@ void Vehicle::_geoFenceLoadComplete(void)
// After geofence request completes we ask for the rally points // After geofence request completes we ask for the rally points
if (!_rallyPointManagerInitialRequestSent) { if (!_rallyPointManagerInitialRequestSent) {
_rallyPointManagerInitialRequestSent = true; _rallyPointManagerInitialRequestSent = true;
qCDebug(VehicleLog) << "_missionLoadComplete requesting rally points"; if (_rallyPointManager->supported()) {
_rallyPointManager->loadFromVehicle(); qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
_rallyPointManager->loadFromVehicle();
} else {
qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
_rallyPointLoadComplete();
}
} }
} }
void Vehicle::_rallyPointLoadComplete(void) void Vehicle::_rallyPointLoadComplete(void)
{ {
qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true"; qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
_initialPlanRequestComplete = true; if (!_initialPlanRequestComplete) {
_initialPlanRequestComplete = true;
emit initialPlanRequestCompleted();
}
} }
void Vehicle::_parametersReady(bool parametersReady) void Vehicle::_parametersReady(bool parametersReady)
......
src/Vehicle/Vehicle.h
View file @ 537463c5
...@@ -7,12 +7,7 @@ ...@@ -7,12 +7,7 @@
* *
****************************************************************************/ ****************************************************************************/
#pragma once
/// @file
/// @author Don Gagne <don@thegagnes.com>
#ifndef Vehicle_H
#define Vehicle_H
#include <QObject> #include <QObject>
#include <QGeoCoordinate> #include <QGeoCoordinate>
...@@ -687,8 +682,8 @@ public: ...@@ -687,8 +682,8 @@ public:
const QVariantList& toolBarIndicators (); const QVariantList& toolBarIndicators ();
const QVariantList& cameraList (void) const; const QVariantList& cameraList (void) const;
bool capabilitiesKnown(void) const { return _vehicleCapabilitiesKnown; } bool capabilitiesKnown (void) const { return _vehicleCapabilitiesKnown; }
bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; } uint64_t capabilityBits (void) const { return _capabilityBits; } // Change signalled by capabilityBitsChanged
/// @true: When flying a mission the vehicle is always facing towards the next waypoint /// @true: When flying a mission the vehicle is always facing towards the next waypoint
bool vehicleYawsToNextWaypointInMission(void) const; bool vehicleYawsToNextWaypointInMission(void) const;
...@@ -731,6 +726,8 @@ signals: ...@@ -731,6 +726,8 @@ signals:
void firmwareTypeChanged(void); void firmwareTypeChanged(void);
void vehicleTypeChanged(void); void vehicleTypeChanged(void);
void capabilitiesKnownChanged(bool capabilitiesKnown); void capabilitiesKnownChanged(bool capabilitiesKnown);
void initialPlanRequestCompleted(void);
void capabilityBitsChanged(uint64_t capabilityBits);
void messagesReceivedChanged (); void messagesReceivedChanged ();
void messagesSentChanged (); void messagesSentChanged ();
...@@ -932,7 +929,7 @@ private: ...@@ -932,7 +929,7 @@ private:
int _telemetryRNoise; int _telemetryRNoise;
unsigned _maxProtoVersion; unsigned _maxProtoVersion;
bool _vehicleCapabilitiesKnown; bool _vehicleCapabilitiesKnown;
bool _supportsMissionItemInt; uint64_t _capabilityBits;
typedef struct { typedef struct {
int component; int component;
...@@ -1073,4 +1070,3 @@ private: ...@@ -1073,4 +1070,3 @@ private:
static const char* _joystickEnabledSettingsKey; static const char* _joystickEnabledSettingsKey;
}; };
#endif
src/comm/MockLink.cc
View file @ 537463c5
...@@ -892,7 +892,7 @@ void MockLink::_respondWithAutopilotVersion(void) ...@@ -892,7 +892,7 @@ void MockLink::_respondWithAutopilotVersion(void)
_vehicleComponentId, _vehicleComponentId,
_mavlinkChannel, _mavlinkChannel,
&msg, &msg,
0, // capabilities, MAV_PROTOCOL_CAPABILITY_MAVLINK2 | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY),
flightVersion, // flight_sw_version, flightVersion, // flight_sw_version,
0, // middleware_sw_version, 0, // middleware_sw_version,
0, // os_sw_version, 0, // os_sw_version,
......
src/comm/MockLinkMissionItemHandler.cc
View file @ 537463c5
...@@ -45,37 +45,37 @@ void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void) ...@@ -45,37 +45,37 @@ void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{ {
switch (msg.msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
_handleMissionRequestList(msg); _handleMissionRequestList(msg);
break; break;
case MAVLINK_MSG_ID_MISSION_REQUEST: case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(msg); _handleMissionRequest(msg);
break; break;
case MAVLINK_MSG_ID_MISSION_ITEM: case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg); _handleMissionItem(msg);
break; break;
case MAVLINK_MSG_ID_MISSION_COUNT: case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(msg); _handleMissionCount(msg);
break; break;
case MAVLINK_MSG_ID_MISSION_ACK: case MAVLINK_MSG_ID_MISSION_ACK:
// Acks are received back for each MISSION_ITEM message // Acks are received back for each MISSION_ITEM message
break; break;
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
// Sets the currently active mission item // Sets the currently active mission item
break; break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
// Delete all mission items // Delete all mission items
_missionItems.clear(); _missionItems.clear();
break; break;
default: default:
return false; return false;
} }
return true; return true;
...@@ -100,9 +100,24 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message ...@@ -100,9 +100,24 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.target_system == _mockLink->vehicleId());
int itemCount = _missionItems.count(); _requestType = (MAV_MISSION_TYPE)request.mission_type;
if (itemCount == 0 && _sendHomePositionOnEmptyList) {
itemCount = 1; int itemCount;
switch (_requestType) {
case MAV_MISSION_TYPE_MISSION:
itemCount = _missionItems.count();
if (itemCount == 0 && _sendHomePositionOnEmptyList) {
itemCount = 1;
}
break;
case MAV_MISSION_TYPE_FENCE:
itemCount = _fenceItems.count();
break;
case MAV_MISSION_TYPE_RALLY:
itemCount = _rallyItems.count();
break;
default:
Q_ASSERT(false);
} }
mavlink_message_t responseMsg; mavlink_message_t responseMsg;
...@@ -114,7 +129,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message ...@@ -114,7 +129,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
msg.sysid, // Target is original sender msg.sysid, // Target is original sender
msg.compid, // Target is original sender msg.compid, // Target is original sender
itemCount, // Number of mission items itemCount, // Number of mission items
MAV_MISSION_TYPE_MISSION); _requestType);
_mockLink->respondWithMavlinkMessage(responseMsg); _mockLink->respondWithMavlinkMessage(responseMsg);
} }
} }
...@@ -128,8 +143,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -128,8 +143,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
mavlink_msg_mission_request_decode(&msg, &request); mavlink_msg_mission_request_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count());
if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) { if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse"; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
} else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) { } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
...@@ -148,20 +162,32 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -148,20 +162,32 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
} }
if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR); _sendAck(MAV_MISSION_ERROR);
} else { } else {
mavlink_message_t responseMsg; mavlink_mission_item_t item;
mavlink_message_t responseMsg;
mavlink_mission_item_t item;
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { switch (request.mission_type) {
item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; case MAV_MISSION_TYPE_MISSION:
item.command = MAV_CMD_NAV_WAYPOINT; if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
item.current = false; item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
item.autocontinue = true; item.command = MAV_CMD_NAV_WAYPOINT;
item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0; item.current = false;
} else { item.autocontinue = true;
item = _missionItems[request.seq]; item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
} else {
item = _missionItems[request.seq];
}
break;
case MAV_MISSION_TYPE_FENCE:
item = _fenceItems[request.seq];
break;
case MAV_MISSION_TYPE_RALLY:
item = _rallyItems[request.seq];
break;
default:
Q_ASSERT(false);
} }
mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(), mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
...@@ -177,7 +203,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -177,7 +203,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
item.autocontinue, item.autocontinue,
item.param1, item.param2, item.param3, item.param4, item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z, item.x, item.y, item.z,
MAV_MISSION_TYPE_MISSION); _requestType);
_mockLink->respondWithMavlinkMessage(responseMsg); _mockLink->respondWithMavlinkMessage(responseMsg);
} }
} }
...@@ -190,12 +216,23 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms ...@@ -190,12 +216,23 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
mavlink_msg_mission_count_decode(&msg, &missionCount); mavlink_msg_mission_count_decode(&msg, &missionCount);
Q_ASSERT(missionCount.target_system == _mockLink->vehicleId()); Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
_requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
_writeSequenceCount = missionCount.count; _writeSequenceCount = missionCount.count;
Q_ASSERT(_writeSequenceCount >= 0); Q_ASSERT(_writeSequenceCount >= 0);
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
_missionItems.clear(); switch (missionCount.mission_type) {
case MAV_MISSION_TYPE_MISSION:
_missionItems.clear();
break;
case MAV_MISSION_TYPE_FENCE:
_fenceItems.clear();
break;
case MAV_MISSION_TYPE_RALLY:
_rallyItems.clear();
break;
}
if (_writeSequenceCount == 0) { if (_writeSequenceCount == 0) {
_sendAck(MAV_MISSION_ACCEPTED); _sendAck(MAV_MISSION_ACCEPTED);
...@@ -228,28 +265,25 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) ...@@ -228,28 +265,25 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
} }
if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) || if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) { (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
sequenceNumber ++; sequenceNumber ++;
} }
if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
_sendAck(MAV_MISSION_ERROR); _sendAck(MAV_MISSION_ERROR);
} else { } else {
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_request_t missionRequest;
mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
memset(&missionRequest, 0, sizeof(missionRequest)); MAV_COMP_ID_MISSIONPLANNER,
missionRequest.target_system = _mavlinkProtocol->getSystemId(); _mockLink->mavlinkChannel(),
missionRequest.target_component = _mavlinkProtocol->getComponentId(); &message,
missionRequest.seq = sequenceNumber; _mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(), sequenceNumber,
MAV_COMP_ID_MISSIONPLANNER, _requestType);
_mockLink->mavlinkChannel(),
&message,
&missionRequest);
_mockLink->respondWithMavlinkMessage(message); _mockLink->respondWithMavlinkMessage(message);
// If response with Mission Item doesn't come before timer fires it's an error // If response with Mission Item doesn't come before timer fires it's an error
...@@ -262,19 +296,16 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) ...@@ -262,19 +296,16 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{ {
qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType; qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_ack_t missionAck;
memset(&missionAck, 0, sizeof(missionAck));
missionAck.target_system = _mavlinkProtocol->getSystemId();
missionAck.target_component = _mavlinkProtocol->getComponentId();
missionAck.type = ackType;
mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(), mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER, MAV_COMP_ID_MISSIONPLANNER,
_mockLink->mavlinkChannel(), _mockLink->mavlinkChannel(),
&message, &message,
&missionAck); _mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
ackType,
_requestType);
_mockLink->respondWithMavlinkMessage(message); _mockLink->respondWithMavlinkMessage(message);
} }
...@@ -290,8 +321,18 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg ...@@ -290,8 +321,18 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId()); Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
_missionItems[missionItem.seq] = missionItem; switch (missionItem.mission_type) {
case MAV_MISSION_TYPE_MISSION:
_missionItems[missionItem.seq] = missionItem;
break;
case MAV_MISSION_TYPE_FENCE:
_fenceItems[missionItem.seq] = missionItem;
break;
case MAV_MISSION_TYPE_RALLY:
_rallyItems[missionItem.seq] = missionItem;
break;
}
_writeSequenceIndex++; _writeSequenceIndex++;
if (_writeSequenceIndex < _writeSequenceCount) { if (_writeSequenceIndex < _writeSequenceCount) {
if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
......
src/comm/MockLinkMissionItemHandler.h
View file @ 537463c5
...@@ -98,9 +98,13 @@ private: ...@@ -98,9 +98,13 @@ private:
int _writeSequenceCount; ///< Numbers of items about to be written int _writeSequenceCount; ///< Numbers of items about to be written
int _writeSequenceIndex; ///< Current index being reqested int _writeSequenceIndex; ///< Current index being reqested
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t; typedef QMap<uint16_t, mavlink_mission_item_t> MissionItemList_t;
MissionList_t _missionItems;
MAV_MISSION_TYPE _requestType;
MissionItemList_t _missionItems;
MissionItemList_t _fenceItems;
MissionItemList_t _rallyItems;
QTimer* _missionItemResponseTimer; QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode; FailureMode_t _failureMode;
bool _sendHomePositionOnEmptyList; bool _sendHomePositionOnEmptyList;
......
src/qgcunittest/UnitTest.cc
View file @ 537463c5
...@@ -391,6 +391,12 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot) ...@@ -391,6 +391,12 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable()); QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable());
_vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
QVERIFY(_vehicle); QVERIFY(_vehicle);
// Wait for plan request to complete
if (!_vehicle->initialPlanRequestComplete()) {
QSignalSpy spyPlan(_vehicle, SIGNAL(initialPlanRequestCompleted()));
QCOMPARE(spyPlan.wait(10000), true);
}
} }
void UnitTest::_disconnectMockLink(void) void UnitTest::_disconnectMockLink(void)
......
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