From 33768cd79c3455e2759afbdc4faf7dc06487bf6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 6 Mar 2019 10:47:35 -0800 Subject: [PATCH] Sub: Fix overshoot from joystick input MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This approach waits for a zero derivative point and to set the new actual position Such method is necessary since the inertia of the ROV underwater is bigger than aerial vehicles resulting in a big overshoot Fix #9797 Signed-off-by: Patrick José Pereira --- ArduSub/control_althold.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 4034e9d423..628d56957d 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -95,11 +95,17 @@ void Sub::althold_run() } } + // 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_alt_hold_controllers(motors.get_throttle_hover()); + engageStopZ = true; + lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z()); } else { // hold z if (ap.at_bottom) { @@ -110,6 +116,16 @@ void Sub::althold_run() float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } + + // 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()))) { + engageStopZ = false; + pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); + } + pos_control.update_z_controller(); }