zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Correct fence enabled handling

QGC4.4
Don Gagne 8 years ago
parent
88e4692db8
commit
18ea73bd1e
5 changed files with 44 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 41
      src/FirmwarePlugin/APM/APMGeoFenceManager.cc
  2. 2
      src/FirmwarePlugin/APM/APMGeoFenceManager.h
  3. 9
      src/MissionManager/GeoFenceController.cc
  4. 3
      src/MissionManager/GeoFenceController.h
  5. 2
      src/MissionManager/GeoFenceManager.h

41
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
Unescape View File

@ -14,9 +14,9 @@ @@ -14,9 +14,9 @@
#include "QGCApplication.h"
#include "ParameterManager.h"
const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
@ -73,13 +73,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const @@ -73,13 +73,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const
_breachReturnPoint = breachReturn;
_polygon = polygon;
// 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->parameterManager()->getParameter(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 = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
@ -89,8 +82,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const @@ -89,8 +82,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const
_sendFencePoint(index);
}
fenceEnableFact->setRawValue(savedEnableState);
emit loadComplete(_breachReturnPoint, _polygon);
}
@ -241,6 +232,28 @@ bool APMGeoFenceManager::_geoFenceSupported(void) @@ -241,6 +232,28 @@ bool APMGeoFenceManager::_geoFenceSupported(void)
}
}
bool APMGeoFenceManager::fenceEnabled(void) const
{
if (qgcApp()->runningUnitTests()) {
return false;
}
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
bool fenceEnabled = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam)->rawValue().toBool();
qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE available" << fenceEnabled;
return fenceEnabled;
}
qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE not available";
return true;
}
void APMGeoFenceManager::_fenceEnabledRawValueChanged(QVariant value)
{
qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE changed" << value.toBool();
emit fenceEnabledChanged(!qgcApp()->runningUnitTests() && value.toBool());
}
void APMGeoFenceManager::_updateSupportedFlags(void)
{
bool newCircleSupported = _fenceSupported && _vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
@ -269,6 +282,10 @@ void APMGeoFenceManager::_parametersReady(void) @@ -269,6 +282,10 @@ void APMGeoFenceManager::_parametersReady(void)
QStringList paramNames;
QStringList paramLabels;
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
connect(_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam), &Fact::rawValueChanged, this, &APMGeoFenceManager::_fenceEnabledRawValueChanged);
}
if (_vehicle->multiRotor()) {
_fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));

2
src/FirmwarePlugin/APM/APMGeoFenceManager.h
Unescape View File

@ -27,6 +27,7 @@ public: @@ -27,6 +27,7 @@ public:
void loadFromVehicle (void) final;
void sendToVehicle (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon) final;
bool fenceSupported (void) const final { return _fenceSupported; }
bool fenceEnabled (void) const final;
bool circleSupported (void) const final;
bool polygonSupported (void) const final;
bool breachReturnSupported (void) const final { return _breachReturnSupported; }
@ -40,6 +41,7 @@ private slots: @@ -40,6 +41,7 @@ private slots:
void _updateSupportedFlags(void);
void _circleRadiusRawValueChanged(QVariant value);
void _parametersReady(void);
void _fenceEnabledRawValueChanged(QVariant value);
private:
void _requestFencePoint(uint8_t pointIndex);

9
src/MissionManager/GeoFenceController.cc
Unescape View File

@ -63,6 +63,7 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn @@ -63,6 +63,7 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void)
{
emit fenceSupportedChanged(fenceSupported());
emit fenceEnabledChanged(fenceEnabled());
emit circleSupportedChanged(circleSupported());
emit polygonSupportedChanged(polygonSupported());
emit breachReturnSupportedChanged(breachReturnSupported());
@ -82,9 +83,10 @@ void GeoFenceController::_activeVehicleBeingRemoved(void) @@ -82,9 +83,10 @@ void GeoFenceController::_activeVehicleBeingRemoved(void)
void GeoFenceController::_activeVehicleSet(void)
{
GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager();
connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged);
connect(geoFenceManager, &GeoFenceManager::fenceEnabledChanged, this, &GeoFenceController::fenceEnabledChanged);
connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::_setDirty);
connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::_setDirty);
connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged);
connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::circleSupportedChanged);
connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
connect(geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged);
@ -375,6 +377,11 @@ bool GeoFenceController::fenceSupported(void) const @@ -375,6 +377,11 @@ bool GeoFenceController::fenceSupported(void) const
return _activeVehicle->geoFenceManager()->fenceSupported();
}
bool GeoFenceController::fenceEnabled(void) const
{
return _activeVehicle->geoFenceManager()->fenceEnabled();
}
bool GeoFenceController::circleSupported(void) const
{
return _activeVehicle->geoFenceManager()->circleSupported();

3
src/MissionManager/GeoFenceController.h
Unescape View File

@ -30,6 +30,7 @@ public: @@ -30,6 +30,7 @@ public:
~GeoFenceController();
Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged)
Q_PROPERTY(bool fenceEnabled READ fenceEnabled NOTIFY fenceEnabledChanged)
Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged)
Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
@ -55,6 +56,7 @@ public: @@ -55,6 +56,7 @@ public:
QString fileExtension(void) const final;
bool fenceSupported (void) const;
bool fenceEnabled (void) const;
bool circleSupported (void) const;
bool polygonSupported (void) const;
bool breachReturnSupported (void) const;
@ -69,6 +71,7 @@ public: @@ -69,6 +71,7 @@ public:
signals:
void fenceSupportedChanged (bool fenceSupported);
void fenceEnabledChanged (bool fenceEnabled);
void circleSupportedChanged (bool circleSupported);
void polygonSupportedChanged (bool polygonSupported);
void breachReturnSupportedChanged (bool breachReturnSupported);

2
src/MissionManager/GeoFenceManager.h
Unescape View File

@ -40,6 +40,7 @@ public: @@ -40,6 +40,7 @@ public:
// Support flags
virtual bool fenceSupported (void) const { return false; }
virtual bool fenceEnabled (void) const { return false; }
virtual bool circleSupported (void) const { return false; }
virtual bool polygonSupported (void) const { return false; }
virtual bool breachReturnSupported (void) const { return false; }
@ -64,6 +65,7 @@ public: @@ -64,6 +65,7 @@ public:
signals:
void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void fenceSupportedChanged (bool fenceSupported);
void fenceEnabledChanged (bool fenceEnabled);
void circleSupportedChanged (bool circleSupported);
void polygonSupportedChanged (bool polygonSupported);
void breachReturnSupportedChanged (bool fenceSupported);

Write Preview
Loading…
Cancel
Save