diff --git a/ArduSub/config.h b/ArduSub/config.h index c90a6a7c47..8b7f1608bf 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -77,7 +77,7 @@ #endif #ifndef SURFACE_DEPTH_DEFAULT - # define SURFACE_DEPTH_DEFAULT -0.10 // pressure sensor reading 10cm depth means craft is considered surfaced + # define SURFACE_DEPTH_DEFAULT -10.0 // pressure sensor reading 10cm depth means craft is considered surfaced #endif diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index dc382877e1..42945b8ca0 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -129,7 +129,7 @@ void Sub::althold_run() pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } else 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(g.surface_depth*100); // 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. } } else { pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 2f91c2ad63..23b88022b5 100644 --- a/ArduSub/surface_bottom_detector.cpp +++ b/ArduSub/surface_bottom_detector.cpp @@ -21,7 +21,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 > g.surface_depth); // If we are above surface depth, we are surfaced + set_surfaced(current_depth > g.surface_depth/100.0); // 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