Merge pull request #6228 from DonLakeFlyer/ParamCircleFence

Display PX4 circular fence from GF_MAX_HOR_DIST

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -237,7 +237,7 @@ FlightMap {
         myGeoFenceController:   _geoFenceController
         interactive:            false
         planView:               false
-        homePosition:           _activeVehicle && _activeVehicle.homePosition.isValid ? _activeVehicle.homePosition : undefined
+        homePosition:           _activeVehicle && _activeVehicle.homePosition.isValid ? _activeVehicle.homePosition :  QtPositioning.coordinate()
     }
 
     // Rally points on map

src/MissionManager/GeoFenceController.cc

@@ -32,14 +32,16 @@
 
 QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog")
 
-const char* GeoFenceController::_jsonFileTypeValue =    "GeoFence";
-const char* GeoFenceController::_jsonBreachReturnKey =  "breachReturn";
+const char* GeoFenceController::_jsonFileTypeValue =        "GeoFence";
+const char* GeoFenceController::_jsonBreachReturnKey =      "breachReturn";
+const char* GeoFenceController::_px4ParamCircularFence =    "GF_MAX_HOR_DIST";
 
 GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent)
-    : PlanElementController(masterController, parent)
-    , _geoFenceManager(_managerVehicle->geoFenceManager())
-    , _dirty(false)
-    , _itemsRequested(false)
+    : PlanElementController     (masterController, parent)
+    , _geoFenceManager          (_managerVehicle->geoFenceManager())
+    , _dirty                    (false)
+    , _itemsRequested           (false)
+    , _px4ParamCircularFenceFact(NULL)
 {
     connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
     connect(&_circles,  &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
@@ -86,6 +88,7 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
     if (_managerVehicle) {
         _geoFenceManager->disconnect(this);
         _managerVehicle->disconnect(this);
+        _managerVehicle->parameterManager()->disconnect(this);
         _managerVehicle = NULL;
         _geoFenceManager = NULL;
     }
@@ -102,7 +105,10 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
     connect(_geoFenceManager, &GeoFenceManager::removeAllComplete,              this, &GeoFenceController::_managerRemoveAllComplete);
     connect(_geoFenceManager, &GeoFenceManager::inProgressChanged,              this, &GeoFenceController::syncInProgressChanged);
 
-    connect(_managerVehicle,  &Vehicle::capabilityBitsChanged,                  this, &RallyPointController::supportedChanged);
+    connect(_managerVehicle,  &Vehicle::capabilityBitsChanged,                  this, &GeoFenceController::supportedChanged);
+
+    connect(_managerVehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &GeoFenceController::_parametersReady);
+    _parametersReady();
 
     emit supportedChanged(supported());
     _signalAll();
@@ -413,3 +419,30 @@ bool GeoFenceController::supported(void) const
 {
     return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
 }
+
+// Hack for PX4
+double GeoFenceController::paramCircularFence(void)
+{
+    if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) {
+        return 0;
+    }
+
+    return _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence)->rawValue().toDouble();
+}
+
+void GeoFenceController::_parametersReady(void)
+{
+    if (_px4ParamCircularFenceFact) {
+        _px4ParamCircularFenceFact->disconnect(this);
+        _px4ParamCircularFenceFact = NULL;
+    }
+
+    if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) {
+        emit paramCircularFenceChanged();
+        return;
+    }
+
+    _px4ParamCircularFenceFact = _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence);
+    connect(_px4ParamCircularFenceFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+    emit paramCircularFenceChanged();
+}

src/MissionManager/GeoFenceController.h

@@ -34,6 +34,9 @@ public:
     Q_PROPERTY(QmlObjectListModel*  circles             READ circles                                        CONSTANT)
     Q_PROPERTY(QGeoCoordinate       breachReturnPoint   READ breachReturnPoint  WRITE setBreachReturnPoint  NOTIFY breachReturnPointChanged)
 
+    // Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST
+    Q_PROPERTY(double               paramCircularFence  READ paramCircularFence                             NOTIFY paramCircularFenceChanged)
+
     /// 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
@@ -55,6 +58,9 @@ public:
     /// Clears the interactive bit from all fence items
     Q_INVOKABLE void clearAllInteractive(void);
 
+    double paramCircularFence(void);
+
+    // Overrides from PlanElementController
     bool supported                  (void) const final;
     void start                      (bool editMode) final;
     void save                       (QJsonObject& json) final;
@@ -80,17 +86,18 @@ signals:
     void breachReturnPointChanged       (QGeoCoordinate breachReturnPoint);
     void editorQmlChanged               (QString editorQml);
     void loadComplete                   (void);
+    void paramCircularFenceChanged      (void);
 
 private slots:
-    void _polygonDirtyChanged(bool dirty);
-    void _setDirty(void);
-    void _setFenceFromManager(const QList<QGCFencePolygon>& polygons,
-                              const QList<QGCFenceCircle>&  circles);
-    void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
-    void _managerLoadComplete(void);
-    void _updateContainsItems(void);
-    void _managerSendComplete(bool error);
-    void _managerRemoveAllComplete(bool error);
+    void _polygonDirtyChanged       (bool dirty);
+    void _setDirty                  (void);
+    void _setFenceFromManager       (const QList<QGCFencePolygon>& polygons, const QList<QGCFenceCircle>&  circles);
+    void _setReturnPointFromManager (QGeoCoordinate breachReturnPoint);
+    void _managerLoadComplete       (void);
+    void _updateContainsItems       (void);
+    void _managerSendComplete       (bool error);
+    void _managerRemoveAllComplete  (bool error);
+    void _parametersReady           (void);
 
 private:
     void _init(void);
@@ -102,9 +109,11 @@ private:
     QmlObjectListModel  _circles;
     QGeoCoordinate      _breachReturnPoint;
     bool                _itemsRequested;
+    Fact*               _px4ParamCircularFenceFact;
 
     static const char* _jsonFileTypeValue;
     static const char* _jsonBreachReturnKey;
+    static const char* _px4ParamCircularFence;
 };
 
 #endif

src/MissionManager/PlanElementController.h

@@ -70,8 +70,8 @@ signals:
 
 protected:
     PlanMasterController*   _masterController;
-    Vehicle*                _controllerVehicle;
-    Vehicle*                _managerVehicle;
+    Vehicle*                _controllerVehicle; ///< Offline controller vehicle
+    Vehicle*                _managerVehicle;    ///< Either active vehicle or _controllerVehicle if none
     bool                    _editMode;
 };
 

src/MissionManager/PlanMasterController.h

@@ -103,8 +103,8 @@ private:
     void _showPlanFromManagerVehicle(void);
 
     MultiVehicleManager*    _multiVehicleMgr;
-    Vehicle*                _controllerVehicle;
-    Vehicle*                _managerVehicle;
+    Vehicle*                _controllerVehicle; ///< Offline controller vehicle
+    Vehicle*                _managerVehicle;    ///< Either active vehicle or _controllerVehicle if none
     bool                    _editMode;
     bool                    _offline;
     MissionController       _missionController;

src/PlanView/GeoFenceMapVisuals.qml

@@ -30,8 +30,16 @@ Item {
 
     property var    _breachReturnPointComponent
     property var    _mouseAreaComponent
+    property var    _paramCircleFenceComponent
     property var    _polygons:                  myGeoFenceController.polygons
     property var    _circles:                   myGeoFenceController.circles
+    property color  _borderColor:               "orange"
+    property int    _borderWidthInclusion:      2
+    property int    _borderWidthExclusion:      0
+    property color  _interiorColorExclusion:    "orange"
+    property color  _interiorColorInclusion:    "transparent"
+    property real   _interiorOpacityExclusion:  0.2
+    property real   _interiorOpacityInclusion:  1
 
     function addPolygon(inclusionPolygon) {
         // Initial polygon is inset to take 2/3rds space
@@ -69,11 +77,14 @@ Item {
     Component.onCompleted: {
         _breachReturnPointComponent = breachReturnPointComponent.createObject(map)
         map.addMapItem(_breachReturnPointComponent)
+        _paramCircleFenceComponent = paramCircleFenceComponent.createObject(map)
+        map.addMapItem(_paramCircleFenceComponent)
         _mouseAreaComponent = mouseAreaComponent.createObject(map)
     }
 
     Component.onDestruction: {
         _breachReturnPointComponent.destroy()
+        _paramCircleFenceComponent.destory()
         _mouseAreaComponent.destroy()
     }
 
@@ -94,10 +105,10 @@ Item {
         delegate : QGCMapPolygonVisuals {
             mapControl:         map
             mapPolygon:         object
-            borderWidth:        object.inclusion ? 2 : 0
-            borderColor:        "orange"
-            interiorColor:      object.inclusion ? "transparent" : "orange"
-            interiorOpacity:    object.inclusion ? 1: 0.2
+            borderWidth:        object.inclusion ? _borderWidthInclusion : _borderWidthExclusion
+            borderColor:        _borderColor
+            interiorColor:      object.inclusion ? _interiorColorInclusion : _interiorColorExclusion
+            interiorOpacity:    object.inclusion ? _interiorOpacityInclusion : _interiorOpacityExclusion
         }
     }
 
@@ -107,10 +118,29 @@ Item {
         delegate : QGCMapCircleVisuals {
             mapControl:         map
             mapCircle:          object
-            borderWidth:        object.inclusion ? 2 : 0
-            borderColor:        "orange"
-            interiorColor:      object.inclusion ? "transparent" : "orange"
-            interiorOpacity:    object.inclusion ? 1: 0.2
+            borderWidth:        object.inclusion ? _borderWidthInclusion : _borderWidthExclusion
+            borderColor:        _borderColor
+            interiorColor:      object.inclusion ? _interiorColorInclusion : _interiorColorExclusion
+            interiorOpacity:    object.inclusion ? _interiorOpacityInclusion : _interiorOpacityExclusion
+        }
+    }
+
+    // Circular geofence specified from parameter
+    Component {
+        id: paramCircleFenceComponent
+
+        MapCircle {
+            color:          _interiorColorInclusion
+            opacity:        _interiorOpacityInclusion
+            border.color:   _borderColor
+            border.width:   _borderWidthInclusion
+            center:         homePosition
+            radius:         _radius
+            visible:        homePosition.isValid && _radius > 0
+
+            property real _radius: myGeoFenceController.paramCircularFence
+
+            on_RadiusChanged: console.log("_radius", _radius, homePosition.isValid, homePosition)
         }
     }