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 += \
src/MissionManager/MissionManager.h \
src/MissionManager/MissionSettingsItem.h \
src/MissionManager/PlanElementController.h \
src/MissionManager/PlanManager.h \
src/MissionManager/PlanMasterController.h \
src/MissionManager/QGCFenceCircle.h \
src/MissionManager/QGCFencePolygon.h \
src/MissionManager/QGCMapCircle.h \
src/MissionManager/QGCMapPolygon.h \
src/MissionManager/RallyPoint.h \
src/MissionManager/RallyPointController.h \
......@@ -689,7 +693,11 @@ SOURCES += \
src/MissionManager/MissionManager.cc \
src/MissionManager/MissionSettingsItem.cc \
src/MissionManager/PlanElementController.cc \
src/MissionManager/PlanManager.cc \
src/MissionManager/PlanMasterController.cc \
src/MissionManager/QGCFenceCircle.cc \
src/MissionManager/QGCFencePolygon.cc \
src/MissionManager/QGCMapCircle.cc \
src/MissionManager/QGCMapPolygon.cc \
src/MissionManager/RallyPoint.cc \
src/MissionManager/RallyPointController.cc \
......@@ -906,9 +914,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.h \
src/AutoPilotPlugins/APM/APMTuningComponent.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/APMRallyPointManager.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \
......@@ -933,9 +939,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \
src/AutoPilotPlugins/APM/APMTuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/APMRallyPointManager.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \
......@@ -974,7 +978,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.h \
src/AutoPilotPlugins/PX4/SensorsComponentController.h \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
SOURCES += \
......@@ -995,7 +998,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.cc \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \
}
......
qgroundcontrol.qrc
View file @ 537463c5
......@@ -90,6 +90,7 @@
<file alias="QGroundControl/Controls/QGCLabel.qml">src/QmlControls/QGCLabel.qml</file>
<file alias="QGroundControl/Controls/QGCListView.qml">src/QmlControls/QGCListView.qml</file>
<file alias="QGroundControl/Controls/QGCMapLabel.qml">src/QmlControls/QGCMapLabel.qml</file>
<file alias="QGroundControl/Controls/QGCMapCircleVisuals.qml">src/MissionManager/QGCMapCircleVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMapPolygonVisuals.qml">src/MissionManager/QGCMapPolygonVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
......@@ -199,6 +200,7 @@
<file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
<file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.SettingsGroup.json</file>
<file alias="Guided.SettingsGroup.json">src/Settings/Guided.SettingsGroup.json</file>
<file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 537463c5
......@@ -17,8 +17,6 @@
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
#include "APMGeoFenceManager.h"
#include "APMRallyPointManager.h"
#include <QAbstractSocket>
......@@ -93,8 +91,6 @@ public:
QString internalParameterMetaDataFile (Vehicle* vehicle) final;
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) final { return new APMGeoFenceManager(vehicle); }
RallyPointManager* newRallyPointManager (Vehicle* vehicle) final { return new APMRallyPointManager(vehicle); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
......
src/FirmwarePlugin/APM/APMGeoFenceManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMGeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
#include "QmlObjectListModel.h"
#include "QGCQGeoCoordinate.h"
const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
, _fenceSupported(false)
, _circleEnabled(false)
, _polygonSupported(false)
, _polygonEnabled(false)
, _breachReturnSupported(vehicle->fixedWing())
, _firstParamLoadComplete(false)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _fenceTypeFact(NULL)
, _fenceEnableFact(NULL)
, _circleRadiusFact(NULL)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived);
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady);
if (_vehicle->parameterManager()->parametersReady()) {
_parametersReady();
}
}
APMGeoFenceManager::~APMGeoFenceManager()
{
}
void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
if (_readTransactionInProgress) {
_sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
return;
}
if (!_fenceSupported) {
return;
}
// Validate
int validatedPolygonCount = 0;
if (polygon.count() >= 3) {
validatedPolygonCount = polygon.count();
}
if (polygon.count() > std::numeric_limits<uint8_t>::max()) {
_sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
validatedPolygonCount = 0;
}
_breachReturnPoint = breachReturn;
_polygon.clear();
if (validatedPolygonCount) {
for (int i=0; i<polygon.count(); i++) {
_polygon.append(polygon.value<QGCQGeoCoordinate*>(i)->coordinate());
}
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
_cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints;
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
// FIXME: No validation of correct fence received
for (uint8_t index=0; index<_cWriteFencePoints; index++) {
_sendFencePoint(index);
}
emit sendComplete(false /* error */);
}
void APMGeoFenceManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
return;
}
_breachReturnPoint = QGeoCoordinate();
_polygon.clear();
if (!_fenceSupported) {
emit loadComplete(_breachReturnPoint, _polygon);
return;
}
// Point 0: Breach return point (always sent, but supported by ArduPlane only)
// Point [1,N]: Polygon points
// Point N+1: Close polygon point (same as point 1)
int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
int minFencePoints = 5;
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints;
if (cFencePoints == 0) {
// No fence
emit loadComplete(_breachReturnPoint, _polygon);
return;
}
if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) {
_sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints));
return;
}
if (cFencePoints > std::numeric_limits<uint8_t>::max()) {
_sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints));
return;
}
_readTransactionInProgress = true;
_cReadFencePoints = cFencePoints;
_currentFencePoint = 0;
_requestFencePoint(_currentFencePoint);
}
/// Called when a new mavlink message for our vehicle is received
void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) {
mavlink_fence_point_t fencePoint;
mavlink_msg_fence_point_decode(&message, &fencePoint);
qCDebug(GeoFenceManagerLog) << "From vehicle fence_point: idx:lat:lng" << fencePoint.idx << fencePoint.lat << fencePoint.lng;
if (fencePoint.idx != _currentFencePoint) {
// FIXME: Protocol out of whack
_readTransactionInProgress = false;
emit inProgressChanged(inProgress());
qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
return;
}
if (fencePoint.idx == 0) {
_breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng);
qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _breachReturnPoint;
_requestFencePoint(++_currentFencePoint);
} else if (fencePoint.idx < _cReadFencePoints - 1) {
QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng);
_polygon.append(polyCoord);
qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord;
if (fencePoint.idx < _cReadFencePoints - 2) {
// Still more points to request
_requestFencePoint(++_currentFencePoint);
} else {
// We've finished collecting fence points
qCDebug(GeoFenceManagerLog) << "Fence point load complete";
_readTransactionInProgress = false;
emit loadComplete(_breachReturnPoint, _polygon);
emit inProgressChanged(inProgress());
}
}
}
}
void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex;
mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
QGeoCoordinate fenceCoord;
if (pointIndex == 0) {
fenceCoord = breachReturnPoint();
} else if (pointIndex == _cWriteFencePoints - 1) {
// Polygon close point
fenceCoord = _polygon[0];
} else {
// Polygon point
fenceCoord = _polygon[pointIndex - 1];
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
uint8_t totalPointCount = _polygon.count() + 1 + 1;
mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex, // Index of point to set
totalPointCount,
fenceCoord.latitude(),
fenceCoord.longitude());
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
bool APMGeoFenceManager::inProgress(void) const
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
void APMGeoFenceManager::_setCircleEnabled(bool circleEnabled)
{
if (circleEnabled != _circleEnabled) {
_circleEnabled = circleEnabled;
emit circleEnabledChanged(circleEnabled);
}
}
void APMGeoFenceManager::_setPolygonEnabled(bool polygonEnabled)
{
if (polygonEnabled != _polygonEnabled) {
_polygonEnabled = polygonEnabled;
emit polygonEnabledChanged(polygonEnabled);
}
}
void APMGeoFenceManager::_updateEnabledFlags(void)
{
bool fenceEnabled = false;
if (_fenceSupported) {
if (_fenceEnableFact) {
fenceEnabled = _fenceEnableFact->rawValue().toBool();
} else {
fenceEnabled = true;
}
bool newCircleEnabled = fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
_setCircleEnabled(newCircleEnabled);
bool newPolygonEnabled = _fenceSupported && fenceEnabled &&
((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) ||
_vehicle->fixedWing());
_setPolygonEnabled(newPolygonEnabled);
} else {
_setCircleEnabled(false);
_setPolygonEnabled(false);
}
}
void APMGeoFenceManager::_parametersReady(void)
{
if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true;
_fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) &&
_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam) &&
!qgcApp()->runningUnitTests();
if (_fenceSupported) {
QStringList paramNames;
QStringList paramLabels;
_polygonSupported = true;
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
_fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam);
connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
}
if (_vehicle->multiRotor()) {
_breachReturnSupported = false;
_fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
_circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
paramNames << QStringLiteral("FENCE_ENABLE") << QStringLiteral("FENCE_TYPE") << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_ALT_MAX")
<< QStringLiteral("FENCE_RADIUS") << QStringLiteral("FENCE_MARGIN");
paramLabels << QStringLiteral("Enabled:") << QStringLiteral("Type:") << QStringLiteral("Breach Action:") << QStringLiteral("Max Altitude:")
<< QStringLiteral("Radius:") << QStringLiteral("Margin:");
} if (_vehicle->fixedWing()) {
_breachReturnSupported = true;
paramNames << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_MINALT") << QStringLiteral("FENCE_MAXALT") << QStringLiteral("FENCE_RETALT")
<< QStringLiteral("FENCE_AUTOENABLE") << QStringLiteral("FENCE_RET_RALLY");
paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Min Altitude:") << QStringLiteral("Max Altitude:") << QStringLiteral("Return Altitude:")
<< QStringLiteral("Auto-Enable:") << QStringLiteral("Return to Rally:");
}
_params.clear();
_paramLabels.clear();
for (int i=0; i<paramNames.count(); i++) {
QString paramName = paramNames[i];
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
_params << QVariant::fromValue(paramFact);
_paramLabels << paramLabels[i];
}
}
emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels);
emit breachReturnSupportedChanged(_breachReturnSupported);
emit polygonSupportedChanged(_polygonSupported);
}
_updateEnabledFlags();
}
}
void APMGeoFenceManager::removeAll(void)
{
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll";
QmlObjectListModel emptyPolygon;
sendToVehicle(_breachReturnPoint, emptyPolygon);
emit removeAllComplete(false /* error */);
}
src/FirmwarePlugin/APM/APMGeoFenceManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMGeoFenceManager_H
#define APMGeoFenceManager_H
#include "GeoFenceManager.h"
#include "QGCMAVLink.h"
#include "FactSystem.h"
class QmlObjectListModel;
class APMGeoFenceManager : public GeoFenceManager
{
Q_OBJECT
public:
APMGeoFenceManager(Vehicle* vehicle);
~APMGeoFenceManager();
// Overrides from GeoFenceManager
bool inProgress (void) const final;
void loadFromVehicle (void) final;
void sendToVehicle (const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) final;
bool polygonSupported (void) const final { return _polygonSupported; }
bool polygonEnabled (void) const final { return _polygonEnabled; }
bool breachReturnSupported (void) const final { return _breachReturnSupported; }
bool circleEnabled (void) const { return _circleEnabled; }
Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
QVariantList params (void) const final { return _params; }
QStringList paramLabels (void) const final { return _paramLabels; }
void removeAll (void) final;
private slots:
void _mavlinkMessageReceived (const mavlink_message_t& message);
void _parametersReady (void);
private:
void _requestFencePoint (uint8_t pointIndex);
void _sendFencePoint (uint8_t pointIndex);
void _updateEnabledFlags(void);
void _setCircleEnabled (bool circleEnabled);
void _setPolygonEnabled (bool polygonEnabled);
private:
bool _fenceSupported;
bool _circleEnabled;
bool _polygonSupported;
bool _polygonEnabled;
bool _breachReturnSupported;
bool _firstParamLoadComplete;
QVariantList _params;
QStringList _paramLabels;
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
uint8_t _cReadFencePoints;
uint8_t _cWriteFencePoints;
uint8_t _currentFencePoint;
Fact* _fenceTypeFact;
Fact* _fenceEnableFact;
Fact* _circleRadiusFact;
static const char* _fenceTotalParam;
static const char* _fenceActionParam;
static const char* _fenceEnableParam;
};
#endif
src/FirmwarePlugin/APM/APMRallyPointManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMRallyPointManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL";
APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle)
: RallyPointManager(vehicle)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived);
}
APMRallyPointManager::~APMRallyPointManager()
{
}
void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported()) {
return;
}
if (_readTransactionInProgress) {
_sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress."));
return;
}
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count());
// FIXME: No validation of correct point received
_rgPoints = rgPoints;
for (uint8_t index=0; index<_rgPoints.count(); index++) {
_sendRallyPoint(index);
}
emit sendComplete(false /* error */);
}
void APMRallyPointManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
return;
}
_rgPoints.clear();
if (!rallyPointsSupported()) {
emit loadComplete(QList<QGeoCoordinate>());
return;
}
_cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt();
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints;
if (_cReadRallyPoints == 0) {
emit loadComplete(_rgPoints);
return;
}
_currentRallyPoint = 0;
_readTransactionInProgress = true;
_requestRallyPoint(_currentRallyPoint);
}
/// Called when a new mavlink message for out vehicle is received
void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) {
mavlink_rally_point_t rallyPoint;
mavlink_msg_rally_point_decode(&message, &rallyPoint);
qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt;
if (rallyPoint.idx != _currentRallyPoint) {
// FIXME: Protocol out of whack
_readTransactionInProgress = false;
emit inProgressChanged(inProgress());
qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint;
return;
}
QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt);
_rgPoints.append(point);
if (rallyPoint.idx < _cReadRallyPoints - 1) {
// Still more points to request
_requestRallyPoint(++_currentRallyPoint);
} else {
// We've finished collecting rally points
qCDebug(RallyPointManagerLog) << "Rally point load complete";
_readTransactionInProgress = false;
emit loadComplete(_rgPoints);
emit inProgressChanged(inProgress());
}
}
}
void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex;
mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
QGeoCoordinate point = _rgPoints[pointIndex];
mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
_vehicle->defaultComponentId(),
pointIndex,
_rgPoints.count(),
point.latitude() * 1e7,
point.longitude() * 1e7,
point.altitude(),
0, 0, 0); // break_alt, land_dir, flags
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
bool APMRallyPointManager::inProgress(void) const
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
bool APMRallyPointManager::rallyPointsSupported(void) const
{
return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam);
}
void APMRallyPointManager::removeAll(void)
{
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll";
QList<QGeoCoordinate> noRallyPoints;
sendToVehicle(noRallyPoints);
emit removeAllComplete(false /* error */);
}
src/FirmwarePlugin/APM/APMRallyPointManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMRallyPointManager_H
#define APMRallyPointManager_H
#include "RallyPointManager.h"
#include "QGCMAVLink.h"
class APMRallyPointManager : public RallyPointManager
{
Q_OBJECT
public:
APMRallyPointManager(Vehicle* vehicle);
~APMRallyPointManager();
// Overrides from RallyPointManager
bool inProgress (void) const final;
void loadFromVehicle (void) final;
void sendToVehicle (const QList<QGeoCoordinate>& rgPoints) final;
bool rallyPointsSupported (void) const final;
void removeAll (void);
QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); }
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
private:
void _requestRallyPoint(uint8_t pointIndex);
void _sendRallyPoint(uint8_t pointIndex);
private:
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
uint8_t _cReadRallyPoints;
uint8_t _currentRallyPoint;
static const char* _rallyTotalParam;
};
#endif
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ 537463c5
......@@ -278,12 +278,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
}
QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
{
Q_UNUSED(vehicle);
return QStringLiteral("FENCE_RADIUS");
}
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
if (vehicle->isOfflineEditingVehicle()) {
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @ 537463c5
......@@ -67,7 +67,6 @@ public:
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
bool multiRotorXConfig (Vehicle* vehicle) final;
QString geoFenceRadiusParam (Vehicle* vehicle) final;
QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Brake"); }
QString missionFlightMode (void) const override { return QString("Auto"); }
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 537463c5
......@@ -235,15 +235,6 @@ public:
/// @return true: X confiuration, false: Plus configuration
virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; }
/// Returns a newly created geofence manager for this vehicle.
virtual GeoFenceManager* newGeoFenceManager(Vehicle* vehicle) { return new GeoFenceManager(vehicle); }
/// Returns the parameter which holds the fence circle radius if supported.
virtual QString geoFenceRadiusParam(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
/// Returns a newly created rally point manager for this vehicle.
virtual RallyPointManager* newRallyPointManager(Vehicle* vehicle) { return new RallyPointManager(vehicle); }
/// Return the resource file which contains the set of params loaded for offline editing.
virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @ 537463c5
......@@ -17,7 +17,6 @@
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
#include "PX4ParameterMetaData.h"
#include "PX4GeoFenceManager.h"
class PX4FirmwarePlugin : public FirmwarePlugin
{
......@@ -62,7 +61,6 @@ public:
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override;
GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) override { return new PX4GeoFenceManager(vehicle); }
QString offlineEditingParamFile(Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/PX4/PX4.OfflineEditing.params"); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
......
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "PX4GeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
, _firstParamLoadComplete(false)
, _circleRadiusFact(NULL)
{
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady);
if (_vehicle->parameterManager()->parametersReady()) {
_parametersReady();
}
}
PX4GeoFenceManager::~PX4GeoFenceManager()
{
}
void PX4GeoFenceManager::_parametersReady(void)
{
if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true;
_circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST"));
emit circleRadiusFactChanged(_circleRadiusFact);
QStringList paramNames;
QStringList paramLabels;
paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST");
paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:");
_params.clear();
_paramLabels.clear();
for (int i=0; i<paramNames.count(); i++) {
QString paramName = paramNames[i];
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
_params << QVariant::fromValue(paramFact);
_paramLabels << paramLabels[i];
}
}
emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels);
}
}
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h deleted 100644 → 0
View file @ a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef PX4GeoFenceManager_H
#define PX4GeoFenceManager_H
#include "GeoFenceManager.h"
#include "QGCMAVLink.h"
#include "FactSystem.h"
class PX4GeoFenceManager : public GeoFenceManager
{
Q_OBJECT
public:
PX4GeoFenceManager(Vehicle* vehicle);
~PX4GeoFenceManager();
// Overrides from GeoFenceManager
bool circleEnabled (void) const { return true; }
Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
QVariantList params (void) const final { return _params; }
QStringList paramLabels (void) const final { return _paramLabels; }
private slots:
void _parametersReady(void);
private:
bool _firstParamLoadComplete;
Fact* _circleRadiusFact;
QVariantList _params;
QStringList _paramLabels;
};
#endif
src/FlightMap/qmldir
View file @ 537463c5
......@@ -29,6 +29,3 @@ MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml
PolygonEditor 1.0 PolygonEditor.qml
VehicleMapItem 1.0 VehicleMapItem.qml
# Editor controls
QGCMapPolygonControls 1.0 QGCMapPolygonControls.qml
src/MissionManager/GeoFenceController.cc
View file @ 537463c5
......@@ -39,11 +39,10 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q
: PlanElementController(masterController, parent)
, _geoFenceManager(_managerVehicle->geoFenceManager())
, _dirty(false)
, _mapPolygon(this)
, _itemsRequested(false)
{
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
managerVehicleChanged(_managerVehicle);
}
......@@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void)
{
emit breachReturnSupportedChanged(breachReturnSupported());
emit breachReturnPointChanged(breachReturnPoint());
emit circleEnabledChanged(circleEnabled());
emit circleRadiusFactChanged(circleRadiusFact());
emit polygonEnabledChanged(polygonEnabled());
emit polygonSupportedChanged(polygonSupported());
emit dirtyChanged(dirty());
emit paramsChanged(params());
emit paramLabelsChanged(paramLabels());
emit supportedChanged(supported());
}
void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_geoFenceManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle = NULL;
_geoFenceManager = NULL;
}
......@@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
}
_geoFenceManager = _managerVehicle->geoFenceManager();
connect(_geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged);
connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete);
connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete);
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
emit supportedChanged(supported());
_signalAll();
}
bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
{
Q_UNUSED(json);
Q_UNUSED(errorString);
#if 0
QString errorStr;
QString errorMessage = tr("GeoFence: %1");
......@@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
setDirty(false);
_signalAll();
#endif
return true;
}
void GeoFenceController::save(QJsonObject& json)
{
Q_UNUSED(json);
#if 0
json[JsonHelper::jsonVersionKey] = 1;
if (_breachReturnPoint.isValid()) {
......@@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json)
}
_mapPolygon.saveToJson(json);
#endif
}
void GeoFenceController::removeAll(void)
{
{
setBreachReturnPoint(QGeoCoordinate());
_mapPolygon.clear();
_polygons.clearAndDeleteContents();
_circles.clearAndDeleteContents();
}
void GeoFenceController::removeAllFromVehicle(void)
......@@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void)
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
} else {
qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle";
_geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
_mapPolygon.setDirty(false);
_geoFenceManager->sendToVehicle(_breachReturnPoint, _polygons, _circles);
setDirty(false);
}
}
......@@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty)
if (dirty != _dirty) {
_dirty = dirty;
if (!dirty) {
_mapPolygon.setDirty(dirty);
for (int i=0; i<_polygons.count(); i++) {
QGCFencePolygon* polygon = _polygons.value<QGCFencePolygon*>(i);
polygon->setDirty(false);
}
for (int i=0; i<_circles.count(); i++) {
QGCFenceCircle* circle = _circles.value<QGCFenceCircle*>(i);
circle->setDirty(false);
}
}
emit dirtyChanged(dirty);
}
......@@ -224,53 +231,26 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty)
}
}
bool GeoFenceController::breachReturnSupported(void) const
{
return _geoFenceManager->breachReturnSupported();
}
bool GeoFenceController::circleEnabled(void) const
{
return _geoFenceManager->circleEnabled();
}
Fact* GeoFenceController::circleRadiusFact(void) const
{
return _geoFenceManager->circleRadiusFact();
}
bool GeoFenceController::polygonSupported(void) const
{
return _geoFenceManager->polygonSupported();
}
bool GeoFenceController::polygonEnabled(void) const
{
return _geoFenceManager->polygonEnabled();
}
QVariantList GeoFenceController::params(void) const
{
return _geoFenceManager->params();
}
QStringList GeoFenceController::paramLabels(void) const
{
return _geoFenceManager->paramLabels();
}
void GeoFenceController::_setDirty(void)
{
setDirty(true);
}
void GeoFenceController::_setPolygonFromManager(const QList<QGeoCoordinate>& polygon)
void GeoFenceController::_setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles)
{
_mapPolygon.clear();
for (int i=0; i<polygon.count(); i++) {
_mapPolygon.appendVertex(polygon[i]);
_polygons.clearAndDeleteContents();
_circles.clearAndDeleteContents();
for (int i=0; i<polygons.count(); i++) {
_polygons.append(new QGCFencePolygon(polygons[i], this));
}
_mapPolygon.setDirty(false);
for (int i=0; i<circles.count(); i++) {
_circles.append(new QGCFenceCircle(circles[i], this));
}
setDirty(false);
}
void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnPoint)
......@@ -279,13 +259,13 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
emit breachReturnPointChanged(_breachReturnPoint);
}
void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
void GeoFenceController::_managerLoadComplete(void)
{
// Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) {
_setReturnPointFromManager(breachReturn);
_setPolygonFromManager(polygon);
_setReturnPointFromManager(_geoFenceManager->breachReturnPoint());
_setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles());
setDirty(false);
_signalAll();
emit loadComplete();
......@@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error)
bool GeoFenceController::containsItems(void) const
{
return _mapPolygon.count() > 2;
return _polygons.count() > 0 || _circles.count() > 0;
}
void GeoFenceController::_updateContainsItems(void)
......@@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void)
// Fake a _loadComplete with the current items
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true;
_managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon());
_managerLoadComplete();
return false;
}
}
}
void GeoFenceController::addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
{
QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
// Initial polygon is inset to take 3/4s of viewport with max width/height of 3000 meters
halfWidthMeters = qMin(halfWidthMeters * 0.75, 1500.0);
halfHeightMeters = qMin(halfHeightMeters * 0.75, 1500.0);
// Initial polygon has max width and height of 3000 meters
topLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0);
topRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0);
bottomLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180);
bottomRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180);
QGCFencePolygon* polygon = new QGCFencePolygon(true /* inclusion */, this);
polygon->appendVertex(topLeft);
polygon->appendVertex(topRight);
polygon->appendVertex(bottomRight);
polygon->appendVertex(bottomLeft);
_polygons.append(polygon);
clearAllInteractive();
polygon->setInteractive(true);
}
void GeoFenceController::addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
{
QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
// Initial radius is inset to take 3/4s of viewport and max of 1500 meters
double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
double radius = qMin(qMin(halfWidthMeters, halfHeightMeters) * 0.75, 1500.0);
QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
QGCFenceCircle* circle = new QGCFenceCircle(center, radius, true /* inclusion */, this);
_circles.append(circle);
clearAllInteractive();
circle->setInteractive(true);
}
void GeoFenceController::deletePolygon(int index)
{
if (index < 0 || index > _polygons.count() - 1) {
return;
}
QGCFencePolygon* polygon = qobject_cast<QGCFencePolygon*>(_polygons.removeAt(index));
polygon->deleteLater();
}
void GeoFenceController::deleteCircle(int index)
{
if (index < 0 || index > _circles.count() - 1) {
return;
}
QGCFenceCircle* circle = qobject_cast<QGCFenceCircle*>(_circles.removeAt(index));
circle->deleteLater();
}
void GeoFenceController::clearAllInteractive(void)
{
for (int i=0; i<_polygons.count(); i++) {
_polygons.value<QGCFencePolygon*>(i)->setInteractive(false);
}
for (int i=0; i<_circles.count(); i++) {
_circles.value<QGCFenceCircle*>(i)->setInteractive(false);
}
}
bool GeoFenceController::supported(void) const
{
return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
}
src/MissionManager/GeoFenceController.h
View file @ 537463c5
......@@ -12,7 +12,8 @@
#include "PlanElementController.h"
#include "GeoFenceManager.h"
#include "QGCMapPolygon.h"
#include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
#include "Vehicle.h"
#include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h"
......@@ -29,21 +30,32 @@ public:
GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL);
~GeoFenceController();
Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT)
Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
// The following properties are reflections of properties from GeoFenceManager
Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged)
Q_PROPERTY(Fact* circleRadiusFact READ circleRadiusFact NOTIFY circleRadiusFactChanged)
Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged)
Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged)
/// Add a new inclusion polygon to the fence
/// @param topLeft - Top left coordinate or map viewport
/// @param topLeft - Bottom right left coordinate or map viewport
Q_INVOKABLE void addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
Q_INVOKABLE void addPolygon (void) { emit addInitialFencePolygon(); }
Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); }
/// Add a new inclusion circle to the fence
/// @param topLeft - Top left coordinate or map viewport
/// @param topLeft - Bottom right left coordinate or map viewport
Q_INVOKABLE void addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
/// Deletes the specified polygon from the polygon list
/// @param index Index of poygon to delete
Q_INVOKABLE void deletePolygon(int index);
/// Deletes the specified circle from the circle list
/// @param index Index of circle to delete
Q_INVOKABLE void deleteCircle(int index);
/// Clears the interactive bit from all fence items
Q_INVOKABLE void clearAllInteractive(void);
bool supported (void) const final;
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
......@@ -58,15 +70,9 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
bool circleEnabled (void) const;
Fact* circleRadiusFact (void) const;
bool polygonSupported (void) const;
bool polygonEnabled (void) const;
bool breachReturnSupported (void) const;
QVariantList params (void) const;
QStringList paramLabels (void) const;
QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
QmlObjectListModel* polygons (void) { return &_polygons; }
QmlObjectListModel* circles (void) { return &_circles; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
......@@ -74,21 +80,14 @@ signals:
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void editorQmlChanged (QString editorQml);
void loadComplete (void);
void addInitialFencePolygon (void);
void circleEnabledChanged (bool circleEnabled);
void circleRadiusFactChanged (Fact* circleRadiusFact);
void polygonSupportedChanged (bool polygonSupported);
void polygonEnabledChanged (bool polygonEnabled);
void breachReturnSupportedChanged (bool breachReturnSupported);
void paramsChanged (QVariantList params);
void paramLabelsChanged (QStringList paramLabels);
private slots:
void _polygonDirtyChanged(bool dirty);
void _setDirty(void);
void _setPolygonFromManager(const QList<QGeoCoordinate>& polygon);
void _setFenceFromManager(const QList<QGCFencePolygon>& polygons,
const QList<QGCFenceCircle>& circles);
void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void _managerLoadComplete(void);
void _updateContainsItems(void);
void _managerSendComplete(bool error);
void _managerRemoveAllComplete(bool error);
......@@ -99,7 +98,8 @@ private:
GeoFenceManager* _geoFenceManager;
bool _dirty;
QGCMapPolygon _mapPolygon;
QmlObjectListModel _polygons;
QmlObjectListModel _circles;
QGeoCoordinate _breachReturnPoint;
bool _itemsRequested;
......
src/MissionManager/GeoFenceManager.cc
View file @ 537463c5
......@@ -10,14 +10,22 @@
#include "GeoFenceManager.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "ParameterManager.h"
#include "QGCMapPolygon.h"
#include "QGCMapCircle.h"
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog")
GeoFenceManager::GeoFenceManager(Vehicle* vehicle)
: QObject(vehicle)
, _vehicle(vehicle)
: _vehicle (vehicle)
, _planManager (vehicle, MAV_MISSION_TYPE_FENCE)
, _firstParamLoadComplete (false)
{
connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged);
connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error);
connect(&_planManager, &PlanManager::removeAllComplete, this, &GeoFenceManager::removeAllComplete);
connect(&_planManager, &PlanManager::sendComplete, this, &GeoFenceManager::_sendComplete);
connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &GeoFenceManager::_planManagerLoadComplete);
}
GeoFenceManager::~GeoFenceManager()
......@@ -25,30 +33,161 @@ GeoFenceManager::~GeoFenceManager()
}
void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
bool GeoFenceManager::inProgress(void) const
{
qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg;
emit error(errorCode, errorMsg);
return _planManager.inProgress();
}
void GeoFenceManager::loadFromVehicle(void)
{
// No geofence support in unknown vehicle
emit loadComplete(QGeoCoordinate(), QList<QGeoCoordinate>());
_planManager.loadFromVehicle();
}
void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
QmlObjectListModel& polygons,
QmlObjectListModel& circles)
{
// No geofence support in unknown vehicle
Q_UNUSED(breachReturn);
Q_UNUSED(polygon);
emit sendComplete(false /* error */);
}
QList<MissionItem*> fenceItems;
_sendPolygons.clear();
_sendCircles.clear();
for (int i=0; i<polygons.count(); i++) {
_sendPolygons.append(*polygons.value<QGCFencePolygon*>(i));
}
for (int i=0; i<circles.count(); i++) {
_sendCircles.append(*circles.value<QGCFenceCircle*>(i));
}
for (int i=0; i<_sendPolygons.count(); i++) {
const QGCFencePolygon& polygon = _sendPolygons[i];
for (int j=0; j<polygon.count(); j++) {
const QGeoCoordinate& vertex = polygon.path()[j].value<QGeoCoordinate>();
MissionItem* item = new MissionItem(0,
polygon.inclusion() ? MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION : MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
MAV_FRAME_GLOBAL,
polygon.count(), // vertex count
0, 0, 0, // param 2-4 unused
vertex.latitude(),
vertex.longitude(),
0, // param 7 unused
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
}
for (int i=0; i<_sendCircles.count(); i++) {
QGCFenceCircle& circle = _sendCircles[i];
MissionItem* item = new MissionItem(0,
circle.inclusion() ? MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION : MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
MAV_FRAME_GLOBAL,
circle.radius()->rawValue().toDouble(),
0, 0, 0, // param 2-4 unused
circle.center().latitude(),
circle.center().longitude(),
0, // param 7 unused
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
_planManager.writeMissionItems(fenceItems);
for (int i=0; i<fenceItems.count(); i++) {
fenceItems[i]->deleteLater();
}
}
void GeoFenceManager::removeAll(void)
{
// No geofence support in unknown vehicle
emit removeAllComplete(false /* error */);
_planManager.removeAll();
}
void GeoFenceManager::_sendComplete(bool error)
{
if (error) {
_polygons.clear();
_circles.clear();
} else {
_polygons = _sendPolygons;
_circles = _sendCircles;
}
_sendPolygons.clear();
_sendCircles.clear();
emit sendComplete(error);
}
void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
{
bool loadFailed = false;
Q_UNUSED(removeAllRequested);
_polygons.clear();
_circles.clear();
MAV_CMD expectedCommand = (MAV_CMD)0;
int expectedVertexCount = 0;
QGCFencePolygon nextPolygon(true /* inclusion */);
const QList<MissionItem*>& fenceItems = _planManager.missionItems();
for (int i=0; i<fenceItems.count(); i++) {
MissionItem* item = fenceItems[i];
MAV_CMD command = item->command();
if (command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
if (nextPolygon.count() == 0) {
// Starting a new polygon
expectedVertexCount = item->param1();
expectedCommand = command;
} else if (expectedVertexCount != item->param1()){
// In the middle of a polygon, but count suddenly changed
emit error(BadPolygonItemFormat, tr("GeoFence load: Vertex count change mid-polygon - actual:expected").arg(item->param1()).arg(expectedVertexCount));
break;
} if (expectedCommand != command) {
// Command changed before last polygon was completely loaded
emit error(BadPolygonItemFormat, tr("GeoFence load: Polygon type changed before last load complete - actual:expected").arg(command).arg(expectedCommand));
break;
}
nextPolygon.appendVertex(QGeoCoordinate(item->param5(), item->param6()));
if (nextPolygon.count() == expectedVertexCount) {
// Polygon is complete
nextPolygon.setInclusion(command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION);
_polygons.append(nextPolygon);
nextPolygon.clear();
}
} else if (command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || command == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
if (nextPolygon.count() != 0) {
// Incomplete polygon
emit error(IncompletePolygonLoad, tr("GeoFence load: Incomplete polygon loaded"));
break;
}
QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */);
_circles.append(circle);
} else {
emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command()));
break;
}
}
if (loadFailed) {
_polygons.clear();
_circles.clear();
}
emit loadComplete();
}
bool GeoFenceManager::supported(void) const
{
return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200);
}
src/MissionManager/GeoFenceManager.h
View file @ 537463c5
......@@ -15,9 +15,13 @@
#include "QGCLoggingCategory.h"
#include "FactSystem.h"
#include "PlanManager.h"
#include "QGCFencePolygon.h"
#include "QGCFenceCircle.h"
class Vehicle;
class QmlObjectListModel;
class PlanManager;
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog)
......@@ -31,80 +35,67 @@ public:
GeoFenceManager(Vehicle* vehicle);
~GeoFenceManager();
/// Returns true if GeoFence is supported by this vehicle
virtual bool supported(void) const;
/// Returns true if the manager is currently communicating with the vehicle
virtual bool inProgress(void) const { return false; }
virtual bool inProgress(void) const;
/// Load the current settings from the vehicle
/// Signals loadComplete when done
virtual void loadFromVehicle(void);
/// Send the current settings to the vehicle
/// Send the geofence settings to the vehicle
/// Signals sendComplete when done
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon);
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, ///< Breach return point
QmlObjectListModel& polygons, ///< List of QGCFencePolygons
QmlObjectListModel& circles); ///< List of QGCFenceCircles
/// Remove all fence related items from vehicle (does not affect parameters)
/// Signals removeAllComplete when done
virtual void removeAll(void);
/// Returns true if this vehicle support polygon fence
/// Signal: polygonSupportedChanged
virtual bool polygonSupported(void) const { return false; }
/// Returns true if polygon fence is currently enabled on this vehicle
/// Signal: polygonEnabledChanged
virtual bool polygonEnabled(void) const { return false; }
/// Returns true if breach return is supported by this vehicle
/// Signal: breachReturnSupportedChanged
virtual bool breachReturnSupported(void) const { return false; }
/// Returns a list of parameter facts that relate to geofence support for the vehicle
/// Signal: paramsChanged
virtual QVariantList params(void) const { return QVariantList(); }
virtual bool polygonEnabled(void) const { return true; }
/// Returns the user visible labels for the paremeters returned by params method
/// Signal: paramLabelsChanged
virtual QStringList paramLabels(void) const { return QStringList(); }
/// Returns true if circular fence is currently enabled on vehicle
/// Signal: circleEnabledChanged
virtual bool circleEnabled(void) const { return false; }
/// Returns the fact which controls the fence circle radius. NULL if not supported
/// Signal: circleRadiusFactChanged
virtual Fact* circleRadiusFact(void) const { return NULL; }
QList<QGeoCoordinate> polygon (void) const { return _polygon; }
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
const QList<QGCFencePolygon>& polygons(void) { return _polygons; }
const QList<QGCFenceCircle>& circles(void) { return _circles; }
const QGeoCoordinate& breachReturnPoint(void) const { return _breachReturnPoint; }
/// Error codes returned in error signal
typedef enum {
InternalError,
TooFewPoints, ///< Too few points for valid geofence
TooManyPoints, ///< Too many points for valid geofence
PolygonTooFewPoints, ///< Too few points for valid fence polygon
PolygonTooManyPoints, ///< Too many points for valid fence polygon
IncompletePolygonLoad, ///< Incomplete polygon loaded
UnsupportedCommand, ///< Usupported command in mission type
BadPolygonItemFormat, ///< Error re-creating polygons from mission items
InvalidCircleRadius,
} ErrorCode_t;
signals:
void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void loadComplete (void);
void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg);
void paramsChanged (QVariantList params);
void paramLabelsChanged (QStringList paramLabels);
void circleEnabledChanged (bool circleEnabled);
void circleRadiusFactChanged (Fact* circleRadiusFact);
void polygonSupportedChanged (bool polygonSupported);
void polygonEnabledChanged (bool polygonEnabled);
void breachReturnSupportedChanged (bool breachReturnSupported);
void removeAllComplete (bool error);
void sendComplete (bool error);
protected:
private slots:
void _sendComplete (bool error);
void _planManagerLoadComplete (bool removeAllRequested);
private:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Vehicle* _vehicle;
QList<QGeoCoordinate> _polygon;
PlanManager _planManager;
QList<QGCFencePolygon> _polygons;
QList<QGCFenceCircle> _circles;
QGeoCoordinate _breachReturnPoint;
bool _firstParamLoadComplete;
QList<QGCFencePolygon> _sendPolygons;
QList<QGCFenceCircle> _sendCircles;
};
#endif
src/MissionManager/MissionController.h
View file @ 537463c5
......@@ -110,6 +110,7 @@ public:
bool loadTextFile(QFile& file, QString& errorString);
// Overrides from PlanElementController
bool supported (void) const final { return true; };
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
......
src/MissionManager/MissionManager.cc
View file @ 537463c5
......@@ -22,110 +22,15 @@
QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle)
, _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()
: PlanManager(vehicle, MAV_MISSION_TYPE_MISSION)
{
}
void MissionManager::_writeMissionItemsWorker(void)
{
_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)
MissionManager::~MissionManager()
{
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)
{
if (inProgress()) {
......@@ -166,809 +71,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
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)
{
if (_vehicle->isOfflineEditingVehicle()) {
......@@ -1129,20 +231,3 @@ void MissionManager::generateResumeMission(int resumeIndex)
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 @@
#ifndef MissionManager_H
#define MissionManager_H
#include <QObject>
#include <QLoggingCategory>
#include <QThread>
#include <QMutex>
#include <QTimer>
#include "MissionItem.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"
class Vehicle;
#include "PlanManager.h"
Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog)
class MissionManager : public QObject
class MissionManager : public PlanManager
{
Q_OBJECT
......@@ -34,128 +23,14 @@ public:
MissionManager(Vehicle* vehicle);
~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.
/// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
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
/// from mission start to resumeIndex in the generate mission.
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
src/MissionManager/PlanElementController.h
View file @ 537463c5
......@@ -27,6 +27,7 @@ public:
PlanElementController(PlanMasterController* masterController, QObject* parent = NULL);
~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 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
......@@ -41,10 +42,11 @@ public:
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 containsItems (void) const = 0;
virtual bool syncInProgress (void) const = 0;
virtual bool dirty (void) const = 0;
virtual void setDirty (bool dirty) = 0;
virtual bool supported (void) const = 0;
virtual bool containsItems (void) const = 0;
virtual bool syncInProgress (void) const = 0;
virtual bool dirty (void) const = 0;
virtual void setDirty (bool dirty) = 0;
/// Sends the current plan element to the vehicle
/// Signals sendComplete when done
......@@ -58,6 +60,7 @@ public:
virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0;
signals:
void supportedChanged (bool supported);
void containsItemsChanged (bool containsItems);
void syncInProgressChanged (bool syncInProgress);
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)
if (_editMode && _loadGeoFence) {
_loadGeoFence = false;
_loadRallyPoints = true;
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
_geoFenceController.loadFromVehicle();
if (_geoFenceController.supported()) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
_geoFenceController.loadFromVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete GeoFence not supported skipping";
_geoFenceController.removeAll();
_loadGeoFenceComplete();
}
setDirty(false);
}
}
......@@ -187,8 +193,14 @@ void PlanMasterController::_loadGeoFenceComplete(void)
{
if (_editMode && _loadRallyPoints) {
_loadRallyPoints = false;
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
_rallyPointController.loadFromVehicle();
if (_rallyPointController.supported()) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
_rallyPointController.loadFromVehicle();
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete Rally Points not supported skipping";
_rallyPointController.removeAll();
_loadRallyPointsComplete();
}
setDirty(false);
}
}
......@@ -204,10 +216,15 @@ void PlanMasterController::_loadRallyPointsComplete(void)
void PlanMasterController::_sendMissionComplete(void)
{
if (_sendGeoFence) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle";
_sendGeoFence = false;
_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);
}
}
......@@ -215,16 +232,21 @@ void PlanMasterController::_sendMissionComplete(void)
void PlanMasterController::_sendGeoFenceComplete(void)
{
if (_sendRallyPoints) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
_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)
{
if (_syncInProgress) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete";
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Point send complete";
_syncInProgress = false;
emit syncInProgressChanged(false);
}
......@@ -361,8 +383,12 @@ void PlanMasterController::removeAllFromVehicle(void)
{
if (!offline()) {
_missionController.removeAllFromVehicle();
_geoFenceController.removeAllFromVehicle();
_rallyPointController.removeAllFromVehicle();
if (_geoFenceController.supported()) {
_geoFenceController.removeAllFromVehicle();
}
if (_rallyPointController.supported()) {
_rallyPointController.removeAllFromVehicle();
}
setDirty(false);
} else {
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 @@
const char* QGCMapPolygon::jsonPolygonKey = "polygon";
QGCMapPolygon::QGCMapPolygon(QObject* newCoordParent, QObject* parent)
: QObject(parent)
, _newCoordParent(newCoordParent)
, _dirty(false)
, _centerDrag(false)
, _ignoreCenterUpdates(false)
QGCMapPolygon::QGCMapPolygon(QObject* parent)
: QObject (parent)
, _dirty (false)
, _centerDrag (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::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged);
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)
{
// Bug workaround, see below
......@@ -132,7 +163,7 @@ void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path)
_polygonModel.clearAndDeleteContents();
foreach(const QGeoCoordinate& coord, path) {
_polygonPath.append(QVariant::fromValue(coord));
_polygonModel.append(new QGCQGeoCoordinate(coord, _newCoordParent));
_polygonModel.append(new QGCQGeoCoordinate(coord, this));
}
setDirty(true);
......@@ -145,7 +176,7 @@ void QGCMapPolygon::setPath(const QVariantList& path)
_polygonModel.clearAndDeleteContents();
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);
......@@ -179,7 +210,7 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString
}
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);
......@@ -225,7 +256,7 @@ void QGCMapPolygon::splitPolygonSegment(int vertexIndex)
void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate)
{
_polygonPath.append(QVariant::fromValue(coordinate));
_polygonModel.append(new QGCQGeoCoordinate(coordinate, _newCoordParent));
_polygonModel.append(new QGCQGeoCoordinate(coordinate, this));
emit pathChanged();
}
......@@ -315,3 +346,11 @@ void QGCMapPolygon::setCenterDrag(bool 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
Q_OBJECT
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)
Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT)
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)
const QGCMapPolygon& operator=(const QGCMapPolygon& other);
Q_PROPERTY(int count READ count NOTIFY countChanged)
Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT)
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 appendVertex(const QGeoCoordinate& coordinate);
......@@ -69,6 +73,7 @@ public:
void setDirty (bool dirty);
QGeoCoordinate center (void) const { return _center; }
bool centerDrag (void) const { return _centerDrag; }
bool interactive (void) const { return _interactive; }
QVariantList path (void) const { return _polygonPath; }
QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; }
......@@ -78,6 +83,7 @@ public:
void setPath (const QVariantList& path);
void setCenter (QGeoCoordinate newCenter);
void setCenterDrag (bool centerDrag);
void setInteractive (bool interactive);
static const char* jsonPolygonKey;
......@@ -88,6 +94,7 @@ signals:
void cleared (void);
void centerChanged (QGeoCoordinate center);
void centerDragChanged (bool centerDrag);
void interactiveChanged (bool interactive);
private slots:
void _polygonModelCountChanged(int count);
......@@ -95,17 +102,18 @@ private slots:
void _updateCenter(void);
private:
void _init(void);
QPolygonF _toPolygonF(void) const;
QGeoCoordinate _coordFromPointF(const QPointF& point) const;
QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const;
QObject* _newCoordParent;
QVariantList _polygonPath;
QmlObjectListModel _polygonModel;
bool _dirty;
QGeoCoordinate _center;
bool _centerDrag;
bool _ignoreCenterUpdates;
bool _interactive;
};
#endif
src/MissionManager/QGCMapPolygonTest.cc
View file @ 537463c5
......@@ -32,7 +32,7 @@ void QGCMapPolygonTest::init(void)
_rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int));
_rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
_mapPolygon = new QGCMapPolygon(this, this);
_mapPolygon = new QGCMapPolygon(this);
_pathModel = _mapPolygon->qmlPathModel();
QVERIFY(_pathModel);
......
src/MissionManager/QGCMapPolygonVisuals.qml
View file @ 537463c5
......@@ -24,16 +24,16 @@ Item {
property var mapControl ///< Map control to place item in
property var mapPolygon ///< QGCMapPolygon object
property bool interactive: true /// true: user can manipulate polygon
property bool interactive: mapPolygon.interactive
property color interiorColor: "transparent"
property real interiorOpacity: 1
property int borderWidth: 0
property color borderColor: "black"
property var _polygonComponent
property var _dragHandlesComponent
property var _splitHandlesComponent
property var _centerDragHandleComponent
property var _polygonComponent
property var _dragHandlesComponent
property var _splitHandlesComponent
property var _centerDragHandleComponent
property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap
property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2
......@@ -111,7 +111,7 @@ Item {
}
Component.onCompleted: {
mapPolygonVisuals.addVisuals()
addVisuals()
if (interactive) {
addHandles()
}
......
src/MissionManager/RallyPointController.cc
View file @ 537463c5
......@@ -58,6 +58,7 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_rallyPointManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle = NULL;
_rallyPointManager = NULL;
}
......@@ -74,7 +75,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete);
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)
......@@ -234,9 +237,9 @@ void RallyPointController::addPoint(QGeoCoordinate point)
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)
......
src/MissionManager/RallyPointController.h
View file @ 537463c5
......@@ -29,7 +29,6 @@ public:
RallyPointController(PlanMasterController* masterController, QObject* parent = NULL);
~RallyPointController();
Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged)
Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged)
......@@ -37,6 +36,7 @@ public:
Q_INVOKABLE void addPoint(QGeoCoordinate point);
Q_INVOKABLE void removePoint(QObject* rallyPoint);
bool supported (void) const final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
void loadFromVehicle (void) final;
......@@ -50,7 +50,6 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
bool rallyPointsSupported (void) const;
QmlObjectListModel* points (void) { return &_points; }
QString editorQml (void) const;
QObject* currentRallyPoint (void) const { return _currentRallyPoint; }
......@@ -58,7 +57,6 @@ public:
void setCurrentRallyPoint(QObject* rallyPoint);
signals:
void rallyPointsSupportedChanged(bool rallyPointsSupported);
void currentRallyPointChanged(QObject* rallyPoint);
void loadComplete(void);
......
src/MissionManager/RallyPointManager.cc
View file @ 537463c5
......@@ -49,3 +49,8 @@ void RallyPointManager::removeAll(void)
// No support in generic vehicle
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:
RallyPointManager(Vehicle* vehicle);
~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
virtual bool inProgress(void) const { return false; }
......@@ -44,8 +47,6 @@ public:
/// Signals removeAllCompleted when done
virtual void removeAll(void);
virtual bool rallyPointsSupported(void) const { return false; }
QList<QGeoCoordinate> points(void) const { return _rgPoints; }
virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); }
......
src/PlanView/GeoFenceEditor.qml
View file @ 537463c5
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
......@@ -10,8 +11,8 @@ import QGroundControl.FactControls 1.0
QGCFlickable {
id: root
width: availableWidth
height: Math.min(availableHeight, geoFenceEditorRect.height)
contentHeight: geoFenceEditorRect.height
height: availableHeight
contentHeight: editorColumn.height
clip: true
property real availableWidth
......@@ -23,99 +24,317 @@ QGCFlickable {
readonly property real _margin: 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 {
id: geoFenceEditorRect
width: parent.width
height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
radius: _radius
color: qgcPal.missionItemEditor
Rectangle {
id: geoFenceEditorRect
width: parent.width
height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
radius: _radius
color: qgcPal.missionItemEditor
QGCLabel {
id: geoFenceLabel
anchors.margins: _margin
anchors.left: parent.left
anchors.top: parent.top
text: qsTr("GeoFence")
color: "black"
}
QGCLabel {
id: geoFenceLabel
anchors.margins: _margin
anchors.left: parent.left
anchors.top: parent.top
text: qsTr("GeoFence")
color: "black"
}
Rectangle {
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
Rectangle {
id: geoFenceItems
anchors.margins: _margin
anchors.left: parent.left
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 {
id: geoLabel
Column {
id: fenceColumn
anchors.margins: _margin
anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
font.pointSize: ScreenTools.smallFontPointSize
text: qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.")
}
spacing: ScreenTools.defaultFontPixelHeight / 2
Repeater {
model: myGeoFenceController.params
QGCLabel {
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 {
width: fenceColumn.width
height: textField.height
Column {
anchors.left: parent.left
anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight / 2
visible: myGeoFenceController.supported
property bool showCombo: modelData.enumStrings.length > 0
Repeater {
model: myGeoFenceController.params
QGCLabel {
id: textFieldLabel
anchors.baseline: textField.baseline
text: myGeoFenceController.paramLabels[index]
Item {
width: fenceColumn.width
height: textField.height
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 {
id: textField
QGCButton {
anchors.left: parent.left
anchors.right: parent.right
width: _editFieldWidth
showUnits: true
fact: modelData
visible: !parent.showCombo
text: qsTr("Polygon Fence")
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.addInclusionPolygon(topLeftCoord, bottomRightCoord)
}
}
FactComboBox {
id: comboField
QGCButton {
anchors.left: parent.left
anchors.right: parent.right
width: _editFieldWidth
indexModel: false
fact: showCombo ? modelData : _nullFact
visible: parent.showCombo
text: qsTr("Circular Fence")
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 {
text: qsTr("Add fence polygon")
visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count === 0
onClicked: myGeoFenceController.addPolygon()
}
SectionHeader {
id: polygonSection
text: qsTr("Polygon Fences")
}
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 {
text: qsTr("Remove fence polygon")
visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count > 0
onClicked: myGeoFenceController.removePolygon()
on_InteractiveChanged: checked = _interactive
onClicked: {
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 {
property var _breachReturnPointComponent
property var _mouseAreaComponent
property var _circleComponent
property var _mapPolygon: myGeoFenceController.mapPolygon
property bool _interactive: interactive
property bool _circleSupported: myGeoFenceController.circleRadiusFact !== null
property bool _circleEnabled: myGeoFenceController.circleEnabled
property real _circleRadius: _circleSupported ? myGeoFenceController.circleRadiusFact.rawValue : 0
property bool _polygonSupported: myGeoFenceController.polygonSupported
property bool _polygonEnabled: myGeoFenceController.polygonEnabled
property var _polygons: myGeoFenceController.polygons
property var _circles: myGeoFenceController.circles
function addPolygon(inclusionPolygon) {
// Initial polygon is inset to take 2/3rds space
var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height)
rect.x += (rect.width * 0.25) / 2
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: {
_circleComponent = circleComponent.createObject(map)
map.addMapItem(_circleComponent)
_breachReturnPointComponent = breachReturnPointComponent.createObject(map)
map.addMapItem(_breachReturnPointComponent)
_mouseAreaComponent = mouseAreaComponent.createObject(map)
}
Component.onDestruction: {
_circleComponent.destroy()
_breachReturnPointComponent.destroy()
_mouseAreaComponent.destroy()
}
Connections {
target: myGeoFenceController
onAddInitialFencePolygon: mapPolygonVisuals.addInitialPolygon()
}
// Mouse area to capture breach return point coordinate
Component {
id: mouseAreaComponent
......@@ -69,28 +88,29 @@ Item {
}
}
QGCMapPolygonVisuals {
id: mapPolygonVisuals
mapControl: map
mapPolygon: _mapPolygon
interactive: _interactive
borderWidth: 2
borderColor: "orange"
visible: _polygonSupported && (planView || _polygonEnabled)
Instantiator {
model: _polygons
delegate : QGCMapPolygonVisuals {
mapControl: map
mapPolygon: object
borderWidth: object.inclusion ? 2 : 0
borderColor: "orange"
interiorColor: object.inclusion ? "transparent" : "orange"
interiorOpacity: object.inclusion ? 1: 0.2
}
}
// GeoFence circle
Component {
id: circleComponent
Instantiator {
model: _circles
MapCircle {
z: QGroundControl.zOrderMapItems
border.width: 2
border.color: "orange"
color: "transparent"
center: homePosition ? homePosition : QtPositioning.coordinate()
radius: _circleRadius
visible: _circleSupported && _circleRadius > 0 && (planView || _circleEnabled)
delegate : QGCMapCircleVisuals {
mapControl: map
mapCircle: object
borderWidth: object.inclusion ? 2 : 0
borderColor: "orange"
interiorColor: object.inclusion ? "transparent" : "orange"
interiorOpacity: object.inclusion ? 1: 0.2
}
}
......
src/PlanView/PlanView.qml
View file @ 537463c5
......@@ -335,7 +335,7 @@ QGCView {
}
break
case _layerRallyPoints:
if (_rallyPointController.rallyPointsSupported) {
if (_rallyPointController.supported) {
_rallyPointController.addPoint(coordinate)
}
break
......
src/PlanView/RallyPointEditorHeader.qml
View file @ 537463c5
......@@ -59,7 +59,7 @@ QGCFlickable {
anchors.right: parent.right
anchors.top: infoLabel.bottom
wrapMode: Text.WordWrap
text: controller.rallyPointsSupported ?
text: controller.supported ?
qsTr("Click in the map to add new rally points.") :
qsTr("This vehicle does not support Rally Points.")
}
......
src/PlanView/RallyPointMapVisuals.qml
View file @ 537463c5
......@@ -29,7 +29,7 @@ Item {
property bool _interactive: interactive
property var _rallyPointsComponent
property bool _rallyPointsSupported: myRallyPointController.rallyPointsSupported
property bool _rallyPointsSupported: myRallyPointController.supported
property var _rallyPoints: myRallyPointController.points
Component.onCompleted: {
......
src/QmlControls/QGroundControl.Controls.qmldir
View file @ 537463c5
......@@ -43,6 +43,7 @@ QGCGroupBox 1.0 QGCGroupBox.qml
QGCLabel 1.0 QGCLabel.qml
QGCListView 1.0 QGCListView.qml
QGCMapLabel 1.0 QGCMapLabel.qml
QGCMapCircleVisuals 1.0 QGCMapCircleVisuals.qml
QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml
QGCMouseArea 1.0 QGCMouseArea.qml
QGCMovableItem 1.0 QGCMovableItem.qml
......
src/Vehicle/Vehicle.cc
View file @ 537463c5
......@@ -116,7 +116,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _telemetryRNoise(0)
, _maxProtoVersion(0)
, _vehicleCapabilitiesKnown(false)
, _supportsMissionItemInt(false)
, _capabilityBits(0)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
......@@ -281,7 +281,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
, _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
, _vehicleCapabilitiesKnown(true)
, _supportsMissionItemInt(false)
, _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
......@@ -350,11 +350,11 @@ void Vehicle::_commonInit(void)
connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
// 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::loadComplete, this, &Vehicle::_geoFenceLoadComplete);
_rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
_rallyPointManager = new RallyPointManager(this);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete);
......@@ -416,6 +416,12 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
_firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
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)
......@@ -749,11 +755,10 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
void Vehicle::_setCapabilities(uint64_t capabilityBits)
{
if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
_supportsMissionItemInt = true;
}
_capabilityBits = capabilityBits;
_vehicleCapabilitiesKnown = true;
emit capabilitiesKnownChanged(true);
emit capabilityBitsChanged(_capabilityBits);
// This should potentially be turned into a user-facing warning
// if the general experience after deployment is that users want MAVLink 2
......@@ -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 %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)
......@@ -1908,8 +1919,13 @@ void Vehicle::_missionLoadComplete(void)
// After the initial mission request completes we ask for the geofence
if (!_geoFenceManagerInitialRequestSent) {
_geoFenceManagerInitialRequestSent = true;
qCDebug(VehicleLog) << "_missionLoadComplete requesting geoFence";
_geoFenceManager->loadFromVehicle();
if (_geoFenceManager->supported()) {
qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
_geoFenceManager->loadFromVehicle();
} else {
qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
_geoFenceLoadComplete();
}
}
}
......@@ -1918,15 +1934,23 @@ void Vehicle::_geoFenceLoadComplete(void)
// After geofence request completes we ask for the rally points
if (!_rallyPointManagerInitialRequestSent) {
_rallyPointManagerInitialRequestSent = true;
qCDebug(VehicleLog) << "_missionLoadComplete requesting rally points";
_rallyPointManager->loadFromVehicle();
if (_rallyPointManager->supported()) {
qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
_rallyPointManager->loadFromVehicle();
} else {
qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
_rallyPointLoadComplete();
}
}
}
void Vehicle::_rallyPointLoadComplete(void)
{
qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
_initialPlanRequestComplete = true;
if (!_initialPlanRequestComplete) {
_initialPlanRequestComplete = true;
emit initialPlanRequestCompleted();
}
}
void Vehicle::_parametersReady(bool parametersReady)
......
src/Vehicle/Vehicle.h
View file @ 537463c5
......@@ -7,12 +7,7 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#ifndef Vehicle_H
#define Vehicle_H
#pragma once
#include <QObject>
#include <QGeoCoordinate>
......@@ -687,8 +682,8 @@ public:
const QVariantList& toolBarIndicators ();
const QVariantList& cameraList (void) const;
bool capabilitiesKnown(void) const { return _vehicleCapabilitiesKnown; }
bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; }
bool capabilitiesKnown (void) const { return _vehicleCapabilitiesKnown; }
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
bool vehicleYawsToNextWaypointInMission(void) const;
......@@ -731,6 +726,8 @@ signals:
void firmwareTypeChanged(void);
void vehicleTypeChanged(void);
void capabilitiesKnownChanged(bool capabilitiesKnown);
void initialPlanRequestCompleted(void);
void capabilityBitsChanged(uint64_t capabilityBits);
void messagesReceivedChanged ();
void messagesSentChanged ();
......@@ -932,7 +929,7 @@ private:
int _telemetryRNoise;
unsigned _maxProtoVersion;
bool _vehicleCapabilitiesKnown;
bool _supportsMissionItemInt;
uint64_t _capabilityBits;
typedef struct {
int component;
......@@ -1073,4 +1070,3 @@ private:
static const char* _joystickEnabledSettingsKey;
};
#endif
src/comm/MockLink.cc
View file @ 537463c5
......@@ -892,7 +892,7 @@ void MockLink::_respondWithAutopilotVersion(void)
_vehicleComponentId,
_mavlinkChannel,
&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,
0, // middleware_sw_version,
0, // os_sw_version,
......
src/comm/MockLinkMissionItemHandler.cc
View file @ 537463c5
......@@ -45,37 +45,37 @@ void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
_handleMissionRequestList(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(msg);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
// Acks are received back for each MISSION_ITEM message
break;
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
// Sets the currently active mission item
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
// Delete all mission items
_missionItems.clear();
break;
default:
return false;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
_handleMissionRequestList(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(msg);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
// Acks are received back for each MISSION_ITEM message
break;
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
// Sets the currently active mission item
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
// Delete all mission items
_missionItems.clear();
break;
default:
return false;
}
return true;
......@@ -100,9 +100,24 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
Q_ASSERT(request.target_system == _mockLink->vehicleId());
int itemCount = _missionItems.count();
if (itemCount == 0 && _sendHomePositionOnEmptyList) {
itemCount = 1;
_requestType = (MAV_MISSION_TYPE)request.mission_type;
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;
......@@ -114,7 +129,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
itemCount, // Number of mission items
MAV_MISSION_TYPE_MISSION);
_requestType);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
......@@ -128,8 +143,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
mavlink_msg_mission_request_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count());
if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
} else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
......@@ -148,20 +162,32 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
}
if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR);
} else {
mavlink_message_t responseMsg;
mavlink_mission_item_t item;
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
item.command = MAV_CMD_NAV_WAYPOINT;
item.current = false;
item.autocontinue = true;
item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
} else {
item = _missionItems[request.seq];
mavlink_mission_item_t item;
mavlink_message_t responseMsg;
switch (request.mission_type) {
case MAV_MISSION_TYPE_MISSION:
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
item.command = MAV_CMD_NAV_WAYPOINT;
item.current = false;
item.autocontinue = true;
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(),
......@@ -177,7 +203,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z,
MAV_MISSION_TYPE_MISSION);
_requestType);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
......@@ -190,12 +216,23 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
mavlink_msg_mission_count_decode(&msg, &missionCount);
Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
_requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
_writeSequenceCount = missionCount.count;
Q_ASSERT(_writeSequenceCount >= 0);
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) {
_sendAck(MAV_MISSION_ACCEPTED);
......@@ -228,28 +265,25 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
}
if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
(_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
sequenceNumber ++;
}
if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
(_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
_sendAck(MAV_MISSION_ERROR);
} else {
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
memset(&missionRequest, 0, sizeof(missionRequest));
missionRequest.target_system = _mavlinkProtocol->getSystemId();
missionRequest.target_component = _mavlinkProtocol->getComponentId();
missionRequest.seq = sequenceNumber;
mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
_mockLink->mavlinkChannel(),
&message,
&missionRequest);
mavlink_message_t message;
mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
sequenceNumber,
_requestType);
_mockLink->respondWithMavlinkMessage(message);
// 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)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
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_message_t message;
mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
_mockLink->mavlinkChannel(),
&message,
&missionAck);
mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
ackType,
_requestType);
_mockLink->respondWithMavlinkMessage(message);
}
......@@ -290,8 +321,18 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
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++;
if (_writeSequenceIndex < _writeSequenceCount) {
if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
......
src/comm/MockLinkMissionItemHandler.h
View file @ 537463c5
......@@ -98,9 +98,13 @@ private:
int _writeSequenceCount; ///< Numbers of items about to be written
int _writeSequenceIndex; ///< Current index being reqested
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionItemList_t;
MAV_MISSION_TYPE _requestType;
MissionItemList_t _missionItems;
MissionItemList_t _fenceItems;
MissionItemList_t _rallyItems;
QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode;
bool _sendHomePositionOnEmptyList;
......
src/qgcunittest/UnitTest.cc
View file @ 537463c5
......@@ -391,6 +391,12 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable());
_vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
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)
......
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