diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 8c72bfad68..4defe6f0c1 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -393,7 +393,6 @@ private:
     int32_t baro_alt;            // barometer altitude in cm above home
     float baro_climbrate;        // barometer climbrate in cm/s
     LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
-    LowPassFilterVector3f depth_accel_ef_filter; // accelerations for land and crash detector tests
 
     // 3D Location vectors
     // Current location of the Sub (altitude is relative to home)
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 7aceecddcd..8a3047d779 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -124,13 +124,18 @@ void Sub::althold_run()
         if(ap.at_bottom) {
         	pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
             pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
-        } else if(ap.at_surface) { // We could use the alt max parameter for this
-        	if(motors.limit.throttle_upper) // ToDo use a better condition
+        } else if(ap.at_surface) {
+        	if(pos_control.get_vel_target_z() > 0.0) {
         		pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
-            pos_control.set_alt_target(inertial_nav.get_altitude() - 10.0f); // set target to 10 cm below surface
+        		pos_control.set_alt_target(SURFACE_DEPTH); // set alt target to the same depth that triggers the surface detector.
+        	}
+        	if(target_climb_rate < 0) { // Dive if the pilot wants to
+            	pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        	}
         } else {
         	pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         }
+
         pos_control.update_z_controller();
         break;
     }
diff --git a/ArduSub/defines.h b/ArduSub/defines.h
index 0d3ce6ba74..078420894a 100644
--- a/ArduSub/defines.h
+++ b/ArduSub/defines.h
@@ -13,6 +13,10 @@
 #define ENABLE ENABLED
 #define DISABLE DISABLED
 
+#define SURFACE_DEPTH -0.05 // 5cm, depends on where the external depth sensor is mounted
+#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
+#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
+
 // Autopilot Yaw Mode enumeration
 enum autopilot_yaw_mode {
     AUTO_YAW_HOLD =             0,  // pilot controls the heading