From cd9cc1419bc721b1a310b6358896d6a91eabddd9 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Fri, 4 Sep 2020 00:31:33 -0300
Subject: [PATCH] Sub: Improve althold to handle small inputs with
 payload/buyoancy better

---
 ArduSub/control_althold.cpp | 41 +++++++++++++------------------------
 1 file changed, 14 insertions(+), 27 deletions(-)

diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 2e49e3c046..a420cfe314 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -37,7 +37,7 @@ void Sub::althold_run()
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
-        attitude_control.set_throttle_out(0,true,g.throttle_filt);
+        attitude_control.set_throttle_out(0.5,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
         pos_control.relax_z_controller(motors.get_throttle_hover());
         last_pilot_heading = ahrs.yaw_sensor;
@@ -101,33 +101,20 @@ void Sub::althold_run()
 }
 
 void Sub::control_depth() {
-    // Hold actual position until zero derivative is detected
-    static bool engageStopZ = true;
-    // Get last user velocity direction to check for zero derivative points
-    static bool lastVelocityZWasNegative = false;
-    if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
-        // output pilot's throttle
-        attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
-        // reset z targets to current values
-        pos_control.relax_z_controller(channel_throttle->norm_input());
-        engageStopZ = true;
-        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z_up_cms());
-    } else { // hold z
-
-        if (ap.at_bottom) {
-            pos_control.init_z_controller();
-            pos_control.set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 10.0); // set target to 10 cm above bottom
+    float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+    target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up);
+
+    // desired_climb_rate returns 0 when within the deadzone.
+    //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
+    if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
+        if (ap.at_surface) {
+            pos_control.set_pos_target_z_cm(MIN(pos_control.get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
+        } else if (ap.at_bottom) {
+            pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom
         }
+    }
 
-        // Detects a zero derivative
-        // When detected, move the altitude set point to the actual position
-        // This will avoid any problem related to joystick delays
-        // or smaller input signals
-        if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z_up_cms()))) {
-            engageStopZ = false;
-            pos_control.init_z_controller();
-        }
+    pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
+    pos_control.update_z_controller();
 
-        pos_control.update_z_controller();
-    }
 }