Sub: Make surface_depth a parameter

ArduSub/Parameters.cpp

@@ -29,6 +29,14 @@
 #define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&sub.v, {group_info : class::var_info} }
 
 const AP_Param::Info Sub::var_info[] = {
+
+	// @Param: SURFACE_DEPTH
+	// @DisplayName: Depth reading at surface
+	// @Description: The depth the external pressure sensor will read when the vehicle is considered at the surface (in meters)
+	// @Range: -0.05 -1.0
+	// @User: Standard
+	GSCALAR(surface_depth, "SURFACE_DEPTH", SURFACE_DEPTH_DEFAULT),
+
     // @Param: SYSID_SW_MREV
     // @DisplayName: Eeprom format version number
     // @Description: This value is incremented when changes are made to the eeprom format

ArduSub/Parameters.h

@@ -359,6 +359,9 @@ public:
         k_param_DataFlash = 253, // 253 - Logging Group
 
         // 254,255: reserved
+
+		//Sub-specific parameters
+		k_param_surface_depth = 256,
     };
 
     AP_Int16        format_version;
@@ -512,6 +515,8 @@ public:
     AP_Float                autotune_aggressiveness;
     AP_Float                autotune_min_d;
 
+    AP_Float				surface_depth;
+
     // Note: keep initializers here in the same order as they are declared
     // above.
     Parameters() :

ArduSub/config.h

@@ -76,6 +76,9 @@
  # define FRAME_CONFIG_STRING "UNKNOWN"
 #endif
 
+#ifndef SURFACE_DEPTH_DEFAULT
+ # define SURFACE_DEPTH_DEFAULT -0.10 // pressure sensor reading 10cm depth means craft is considered surfaced
+#endif
 
 
 /////////////////////////////////////////////////////////////////////////////////

ArduSub/control_althold.cpp

@@ -127,7 +127,7 @@ void Sub::althold_run()
         } 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(SURFACE_DEPTH); // set alt target to the same depth that triggers the surface detector.
+        		pos_control.set_alt_target(g.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);

ArduSub/defines.h

@@ -13,7 +13,6 @@
 #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
 

ArduSub/surface_bottom_detector.cpp

@@ -22,7 +22,7 @@ void Sub::update_surface_and_bottom_detector()
 	if (ap.depth_sensor_present) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
 		current_depth = barometer.get_altitude();
 
-		set_surfaced(current_depth > SURFACE_DEPTH); // If we are above surface depth, we are surfaced
+		set_surfaced(current_depth > g.surface_depth); // If we are above surface depth, we are surfaced
 
 		if(motors.limit.throttle_lower && vel_stationary) {
 			// bottom criteria met - increment the counter and check if we've triggered