Sub: Enable altitude fences for sub

ArduSub/APM_Config.h

@@ -16,7 +16,7 @@
 //#define LOGGING_ENABLED       DISABLED            // disable dataflash logging to save 11K of flash space
 //#define MOUNT                 DISABLED            // disable the camera gimbal to save 8K of flash space
 #define AUTOTUNE_ENABLED      DISABLED            // disable the auto tune functionality to save 7k of flash
-#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
+//#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
 //#define CAMERA                DISABLED            // disable camera trigger to save 1k of flash
 //#define CONFIG_SONAR          DISABLED            // disable sonar to save 1k of flash
 #define POSHOLD_ENABLED       DISABLED            // disable PosHold flight mode to save 4.5k of flash

ArduSub/Attitude.cpp

@@ -289,25 +289,19 @@ void Sub::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
 // updates position controller's maximum altitude using fence and EKF limits
 void Sub::update_poscon_alt_max()
 {
-    float alt_limit_cm = 0.0f;  // interpreted as no limit if left as zero
+	float min_alt_cm = 0.0f;  // interpreted as no limit if left as zero
+	float max_alt_cm = 0.0f;
 
 #if AC_FENCE == ENABLED
     // set fence altitude limit in position controller
     if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
-        alt_limit_cm = pv_alt_above_origin(fence.get_safe_alt()*100.0f);
+        min_alt_cm = fence.get_safe_depth()*100.0f;
+        max_alt_cm = fence.get_safe_alt()*100.0f;
     }
 #endif
-
-    // get alt limit from EKF (limited during optical flow flight)
-    float ekf_limit_cm = 0.0f;
-    if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) {
-        if ((alt_limit_cm <= 0.0f) || (ekf_limit_cm < alt_limit_cm)) {
-            alt_limit_cm = ekf_limit_cm;
-        }
-    }
-
     // pass limit to pos controller
-    pos_control.set_alt_max(alt_limit_cm);
+    pos_control.set_alt_min(min_alt_cm);
+    pos_control.set_alt_max(max_alt_cm);
 }
 
 // rotate vector from vehicle's perspective to North-East frame

ArduSub/fence.cpp

@@ -8,7 +8,7 @@
 
 // fence_check - ask fence library to check for breaches and initiate the response
 // called at 1hz
-void Copter::fence_check()
+void Sub::fence_check()
 {
     uint8_t new_breaches; // the type of fence that has been breached
     uint8_t orig_breaches = fence.get_breaches();
@@ -58,7 +58,7 @@ void Copter::fence_check()
 }
 
 // fence_send_mavlink_status - send fence status to ground station
-void Copter::fence_send_mavlink_status(mavlink_channel_t chan)
+void Sub::fence_send_mavlink_status(mavlink_channel_t chan)
 {   
     if (fence.enabled()) {
         // traslate fence library breach types to mavlink breach types