Implement APM circle geofence from params

Both PX4 and APM supports circular geofences around the home point.
QGC with PX4 displays this by checking GF_MAX_HOR_DIST.
This commit adds equivalent functionality for APM.
Params used on APM are FENCE_ENABLE,FENCE_TYPE,FENCE_RADIUS.
Tested with ArduCopter 4.3, ArduPlane master, and PX4 1.13.

src/MissionManager/GeoFenceController.cc

@@ -39,6 +39,9 @@ const char* GeoFenceController::_jsonCirclesKey =           "circles";
 const char* GeoFenceController::_breachReturnAltitudeFactName = "Altitude";
 
 const char* GeoFenceController::_px4ParamCircularFence =    "GF_MAX_HOR_DIST";
+const char* GeoFenceController::_apmParamCircularFenceRadius =    "FENCE_RADIUS";
+const char* GeoFenceController::_apmParamCircularFenceEnabled =    "FENCE_ENABLE";
+const char* GeoFenceController::_apmParamCircularFenceType =    "FENCE_TYPE";
 
 GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent)
     : PlanElementController         (masterController, parent)
@@ -495,30 +498,102 @@ bool GeoFenceController::supported(void) const
     return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
 }
 
-// Hack for PX4
+/* Returns the radius of the "paramCircularFence"
+ * which is called the "Geofence Failsafe" in PX4 and the "Circular Geofence" on Ardupilot
+ * this code should ideally live in the firmware plugin since it is specific to apm and px4 firmwares */
 double GeoFenceController::paramCircularFence(void)
 {
-    if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) {
+    if(_managerVehicle->isOfflineEditingVehicle()){
         return 0;
     }
 
-    return _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence)->rawValue().toDouble();
+    if(_managerVehicle->px4Firmware()){
+        if(!_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)){
+            return 0;
+        }
+
+        return _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence)->rawValue().toDouble();
+    }
+
+    if(_managerVehicle->apmFirmware())
+    {
+        if (!_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceRadius) ||
+            !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceEnabled) ||
+            !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceType)){
+            return 0;
+        }
+
+        bool apm_fence_enabled = _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceEnabled)->rawValue().toBool();
+        bool apm_fence_type_circle = (1 << 1) & _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceType)->rawValue().toUInt();
+
+        if(!apm_fence_enabled || !apm_fence_type_circle)
+            return 0;
+
+        return _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceRadius)->rawValue().toDouble();
+    }
+
+    return 0;
 }
 
 void GeoFenceController::_parametersReady(void)
 {
+    /* When parameters are ready we setup notifications of param changes
+     * so that if a param changes we can emit paramCircularFenceChanged
+     * and trigger an update to the UI */
+
+    // First disconnect from any existing facts
     if (_px4ParamCircularFenceFact) {
         _px4ParamCircularFenceFact->disconnect(this);
         _px4ParamCircularFenceFact = nullptr;
     }
+    if (_apmParamCircularFenceRadiusFact) {
+        _apmParamCircularFenceRadiusFact->disconnect(this);
+        _apmParamCircularFenceRadiusFact = nullptr;
+    }
+    if (_apmParamCircularFenceEnabledFact) {
+        _apmParamCircularFenceEnabledFact->disconnect(this);
+        _apmParamCircularFenceEnabledFact = nullptr;
+    }
+    if (_apmParamCircularFenceTypeFact) {
+        _apmParamCircularFenceTypeFact->disconnect(this);
+        _apmParamCircularFenceTypeFact = nullptr;
+    }
 
-    if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) {
+    // then connect to needed paremters
+    // While checking they exist to avoid errors
+    ParameterManager* _paramManager = _managerVehicle->parameterManager();
+
+    if(_managerVehicle->isOfflineEditingVehicle()){
         emit paramCircularFenceChanged();
         return;
     }
 
-    _px4ParamCircularFenceFact = _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence);
-    connect(_px4ParamCircularFenceFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+    if(_managerVehicle->px4Firmware()){
+        if(!_paramManager->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)){
+            emit paramCircularFenceChanged();
+            return;
+        }
+
+        _px4ParamCircularFenceFact = _paramManager->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence);
+        connect(_px4ParamCircularFenceFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+    }
+    else if(_managerVehicle->apmFirmware())
+    {
+        if (!_paramManager->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceRadius) ||
+            !_paramManager->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceEnabled) ||
+            !_paramManager->parameterExists(FactSystem::defaultComponentId, _apmParamCircularFenceType)){
+            emit paramCircularFenceChanged();
+            return;
+        }
+
+        _apmParamCircularFenceRadiusFact = _paramManager->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceRadius);
+        _apmParamCircularFenceEnabledFact = _paramManager->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceEnabled);
+        _apmParamCircularFenceTypeFact = _paramManager->getParameter(FactSystem::defaultComponentId, _apmParamCircularFenceType);
+        connect(_apmParamCircularFenceRadiusFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+        connect(_apmParamCircularFenceEnabledFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+        connect(_apmParamCircularFenceTypeFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged);
+    }
+
     emit paramCircularFenceChanged();
 }
 

src/MissionManager/GeoFenceController.h

@@ -35,8 +35,8 @@ public:
     Q_PROPERTY(QGeoCoordinate       breachReturnPoint       READ breachReturnPoint      WRITE setBreachReturnPoint  NOTIFY breachReturnPointChanged)
     Q_PROPERTY(Fact*                breachReturnAltitude    READ breachReturnAltitude                               CONSTANT)
 
-    // Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST
-    Q_PROPERTY(double               paramCircularFence  READ paramCircularFence                             NOTIFY paramCircularFenceChanged)
+    // Radius of the "paramCircularFence" which is called the "Geofence Failsafe" in PX4 and the "Circular Geofence" on ArduPilot
+    Q_PROPERTY(double               paramCircularFence      READ paramCircularFence                                 NOTIFY paramCircularFenceChanged)
 
     /// Add a new inclusion polygon to the fence
     ///     @param topLeft: Top left coordinate or map viewport
@@ -114,11 +114,18 @@ private:
     Fact                _breachReturnAltitudeFact;
     double              _breachReturnDefaultAltitude =  qQNaN();
     bool                _itemsRequested =               false;
-    Fact*               _px4ParamCircularFenceFact =    nullptr;
+
+    Fact*               _px4ParamCircularFenceFact =        nullptr;
+    Fact*               _apmParamCircularFenceRadiusFact =  nullptr;
+    Fact*               _apmParamCircularFenceEnabledFact = nullptr;
+    Fact*               _apmParamCircularFenceTypeFact =    nullptr;
 
     static QMap<QString, FactMetaData*> _metaDataMap;
 
     static const char* _px4ParamCircularFence;
+    static const char* _apmParamCircularFenceRadius;
+    static const char* _apmParamCircularFenceEnabled;
+    static const char* _apmParamCircularFenceType;
 
     static const int _jsonCurrentVersion = 2;