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.
*