Browse Source
- GeoFence support comes from plugin - Circle fence support - Better param supportQGC4.4
Don Gagne
9 years ago
24 changed files with 780 additions and 443 deletions
@ -0,0 +1,316 @@
@@ -0,0 +1,316 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 "ParameterLoader.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) |
||||
, _breachReturnSupported(vehicle->fixedWing()) |
||||
, _firstParamLoadComplete(false) |
||||
, _circleRadiusFact(NULL) |
||||
, _readTransactionInProgress(false) |
||||
, _writeTransactionInProgress(false) |
||||
, _fenceEnableFact(NULL) |
||||
, _fenceTypeFact(NULL) |
||||
{ |
||||
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived); |
||||
connect(_vehicle->getParameterLoader(), &ParameterLoader::parametersReady, this, &APMGeoFenceManager::_parametersReady); |
||||
} |
||||
|
||||
APMGeoFenceManager::~APMGeoFenceManager() |
||||
{ |
||||
|
||||
} |
||||
|
||||
void APMGeoFenceManager::sendToVehicle(void) |
||||
{ |
||||
if (_readTransactionInProgress) { |
||||
_sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); |
||||
return; |
||||
} |
||||
|
||||
if (!_geoFenceSupported()) { |
||||
return; |
||||
} |
||||
|
||||
// Validate
|
||||
if (polygonSupported()) { |
||||
if (_polygon.count() < 3) { |
||||
_sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points.")); |
||||
return; |
||||
} |
||||
if (_polygon.count() > std::numeric_limits<uint8_t>::max()) { |
||||
_sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); |
||||
return; |
||||
} |
||||
} |
||||
|
||||
// First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air.
|
||||
// Unfortunately the param to do this with differs between plane and copter.
|
||||
const char* enableParam = _vehicle->fixedWing() ? _fenceActionParam : _fenceEnableParam; |
||||
Fact* fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, enableParam); |
||||
QVariant savedEnableState = fenceEnableFact->rawValue(); |
||||
fenceEnableFact->setRawValue(0); |
||||
|
||||
// Total point count, +1 polygon close in last index, +1 for breach in index 0
|
||||
_cWriteFencePoints = _polygon.count() + 1 + 1 ; |
||||
_vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); |
||||
|
||||
// FIXME: No validation of correct fence received
|
||||
for (uint8_t index=0; index<_cWriteFencePoints; index++) { |
||||
_sendFencePoint(index); |
||||
} |
||||
|
||||
fenceEnableFact->setRawValue(savedEnableState); |
||||
} |
||||
|
||||
void APMGeoFenceManager::loadFromVehicle(void) |
||||
{ |
||||
_polygon.clear(); |
||||
|
||||
if (!_geoFenceSupported()) { |
||||
return; |
||||
} |
||||
|
||||
// Point 0: Breach return point (ArduPlane only)
|
||||
// Point [1,N]: Polygon points
|
||||
// Point N+1: Close polygon point (same as point 1)
|
||||
int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); |
||||
int minFencePoints = 6; |
||||
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints; |
||||
if (cFencePoints == 0) { |
||||
// No fence, no more work to do, fence data has already been cleared
|
||||
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 out 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
|
||||
qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; |
||||
return; |
||||
} |
||||
|
||||
if (fencePoint.idx == 0) { |
||||
setBreachReturnPoint(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
|
||||
_readTransactionInProgress = false; |
||||
emit polygonChanged(_polygon); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
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(mavlink->getSystemId(), |
||||
mavlink->getComponentId(), |
||||
&msg, |
||||
_vehicle->id(), |
||||
_vehicle->defaultComponentId(), |
||||
pointIndex); |
||||
_vehicle->sendMessageOnPriorityLink(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(mavlink->getSystemId(), |
||||
mavlink->getComponentId(), |
||||
&msg, |
||||
_vehicle->id(), |
||||
_vehicle->defaultComponentId(), |
||||
pointIndex, // Index of point to set
|
||||
totalPointCount, |
||||
fenceCoord.latitude(), |
||||
fenceCoord.longitude()); |
||||
_vehicle->sendMessageOnPriorityLink(msg); |
||||
} |
||||
|
||||
bool APMGeoFenceManager::inProgress(void) const |
||||
{ |
||||
return _readTransactionInProgress || _writeTransactionInProgress; |
||||
} |
||||
|
||||
bool APMGeoFenceManager::_geoFenceSupported(void) |
||||
{ |
||||
// FIXME: MockLink doesn't support geo fence yet
|
||||
if (qgcApp()->runningUnitTests()) { |
||||
return false; |
||||
} |
||||
|
||||
if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) || |
||||
!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) { |
||||
return false; |
||||
} else { |
||||
return true; |
||||
} |
||||
} |
||||
|
||||
void APMGeoFenceManager::_updateSupportedFlags(void) |
||||
{ |
||||
emit circleSupportedChanged(circleSupported()); |
||||
emit polygonSupportedChanged(polygonSupported()); |
||||
} |
||||
|
||||
void APMGeoFenceManager::_parametersReady(void) |
||||
{ |
||||
if (!_firstParamLoadComplete) { |
||||
_firstParamLoadComplete = true; |
||||
|
||||
_fenceSupported = _vehicle->parameterExists(FactSystem::defaultComponentId, QStringLiteral("FENCE_ACTION")); |
||||
|
||||
if (_fenceSupported) { |
||||
QStringList paramNames; |
||||
QStringList paramLabels; |
||||
|
||||
if (_vehicle->multiRotor()) { |
||||
_fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_ENABLE")); |
||||
_fenceTypeFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); |
||||
|
||||
connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); |
||||
connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); |
||||
|
||||
_circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS")); |
||||
connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged); |
||||
emit circleRadiusChanged(circleRadius()); |
||||
|
||||
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()) { |
||||
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->parameterExists(FactSystem::defaultComponentId, paramName)) { |
||||
Fact* paramFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, paramName); |
||||
_params << QVariant::fromValue(paramFact); |
||||
_paramLabels << paramLabels[i]; |
||||
} |
||||
} |
||||
emit paramsChanged(_params); |
||||
emit paramLabelsChanged(_paramLabels); |
||||
|
||||
emit fenceSupportedChanged(_fenceSupported); |
||||
emit circleSupportedChanged(circleSupported()); |
||||
emit polygonSupportedChanged(polygonSupported()); |
||||
} |
||||
|
||||
qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << |
||||
_fenceSupported << circleSupported() << polygonSupported() << _breachReturnSupported; |
||||
} |
||||
} |
||||
|
||||
float APMGeoFenceManager::circleRadius(void) const |
||||
{ |
||||
if (_circleRadiusFact) { |
||||
return _circleRadiusFact->rawValue().toFloat(); |
||||
} else { |
||||
return 0.0; |
||||
} |
||||
} |
||||
|
||||
void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value) |
||||
{ |
||||
emit circleRadiusChanged(value.toFloat()); |
||||
} |
||||
|
||||
bool APMGeoFenceManager::circleSupported(void) const |
||||
{ |
||||
if (_fenceSupported && _vehicle->multiRotor() && _fenceEnableFact && _fenceTypeFact) { |
||||
return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 2); |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
|
||||
bool APMGeoFenceManager::polygonSupported(void) const |
||||
{ |
||||
if (_fenceSupported) { |
||||
if (_vehicle->multiRotor()) { |
||||
if (_fenceEnableFact && _fenceTypeFact) { |
||||
return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 4); |
||||
} |
||||
} else if (_vehicle->fixedWing()) { |
||||
return true; |
||||
} |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
@ -0,0 +1,73 @@
@@ -0,0 +1,73 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 APMGeoFenceManager : public GeoFenceManager |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
APMGeoFenceManager(Vehicle* vehicle); |
||||
~APMGeoFenceManager(); |
||||
|
||||
// Overrides from GeoFenceManager
|
||||
bool inProgress (void) const final; |
||||
void loadFromVehicle (void) final; |
||||
void sendToVehicle (void) final; |
||||
bool fenceSupported (void) const final { return _fenceSupported; } |
||||
bool circleSupported (void) const final; |
||||
bool polygonSupported (void) const final; |
||||
bool breachReturnSupported (void) const final { return _breachReturnSupported; } |
||||
float circleRadius (void) const final; |
||||
QVariantList params (void) const final { return _params; } |
||||
QStringList paramLabels (void) const final { return _paramLabels; } |
||||
|
||||
private slots: |
||||
void _mavlinkMessageReceived(const mavlink_message_t& message); |
||||
void _updateSupportedFlags(void); |
||||
void _circleRadiusRawValueChanged(QVariant value); |
||||
void _parametersReady(void); |
||||
|
||||
private: |
||||
void _requestFencePoint(uint8_t pointIndex); |
||||
void _sendFencePoint(uint8_t pointIndex); |
||||
bool _geoFenceSupported(void); |
||||
|
||||
private: |
||||
bool _fenceSupported; |
||||
bool _breachReturnSupported; |
||||
bool _firstParamLoadComplete; |
||||
|
||||
QVariantList _params; |
||||
QStringList _paramLabels; |
||||
|
||||
Fact* _circleRadiusFact; |
||||
|
||||
bool _readTransactionInProgress; |
||||
bool _writeTransactionInProgress; |
||||
|
||||
uint8_t _cReadFencePoints; |
||||
uint8_t _cWriteFencePoints; |
||||
uint8_t _currentFencePoint; |
||||
|
||||
Fact* _fenceEnableFact; |
||||
Fact* _fenceTypeFact; |
||||
|
||||
static const char* _fenceTotalParam; |
||||
static const char* _fenceActionParam; |
||||
static const char* _fenceEnableParam; |
||||
}; |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue