AP_AdvancedFailsafe: added AFS_MAX_RANGE option

this allows a maximum range since first arm to be set in
AFS_MAX_RANGE. This value (in km) will trigger the configured
termination type if the GPS location shows that it has been breached.

This feature, in combination with the @READONLY apj parameter feature,
is intended to be used to meet regulatory restrictions on a vehicles
maximum range

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp

@@ -149,6 +149,13 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
     // @Units: s
     AP_GROUPINFO("RC_FAIL_TIME",   19, AP_AdvancedFailsafe, _rc_fail_time_seconds,    0),
 
+    // @Param: MAX_RANGE
+    // @DisplayName: Max allowed range
+    // @Description: This is the maximum range of the vehicle in kilometers from first arming. If the vehicle goes beyond this range then the TERM_ACTION is performed. A value of zero disables this feature.
+    // @User: Advanced
+    // @Units: km
+    AP_GROUPINFO("MAX_RANGE",   20, AP_AdvancedFailsafe, _max_range_km,    0),
+    
     AP_GROUPEND
 };
 
@@ -171,6 +178,9 @@ AP_AdvancedFailsafe::check(uint32_t last_heartbeat_ms, bool geofence_breached, u
         }
     }
 
+    // update max range check
+    max_range_update();
+
     enum control_mode mode = afs_mode();
     
     // check if RC termination is enabled
@@ -404,3 +414,42 @@ bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reaso
     }
     return false;
 }
+
+/*
+  update check of maximum range
+ */
+void AP_AdvancedFailsafe::max_range_update(void)
+{
+    if (_max_range_km <= 0) {
+        return;
+    }
+
+    if (!_have_first_location) {
+        if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
+            // wait for 3D fix
+            return;
+        }
+        if (!hal.util->get_soft_armed()) {
+            // wait for arming
+            return;
+        }
+        _first_location = AP::gps().location();
+        _have_first_location = true;
+    }
+
+    if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
+        // don't trigger when dead-reckoning
+        return;
+    }
+    
+    // check distance from first location
+    float distance_km = _first_location.get_distance(AP::gps().location()) * 0.001;
+    if (distance_km > _max_range_km) {
+        uint32_t now = AP_HAL::millis();
+        if (now - _term_range_notice_ms > 5000) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to range %.1fkm", distance_km);
+            _term_range_notice_ms = now;
+        }
+        _terminate.set_and_notify(1);
+    }
+}

libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h

@@ -118,6 +118,7 @@ protected:
     AP_Int8  _enable_RC_fs;
     AP_Int8  _rc_term_manual_only;
     AP_Int8  _enable_dual_loss;
+    AP_Int16  _max_range_km;
 
     bool _heartbeat_pin_value;
 
@@ -139,5 +140,13 @@ protected:
     // have the failsafe values been setup?
     bool _failsafe_setup:1;
 
+    Location _first_location;
+    bool _have_first_location;
+    uint32_t _term_range_notice_ms;
+
     bool check_altlimit(void);
+
+private:
+    // update maximum range check
+    void max_range_update();
 };