From deccd051a06b61e2aa1688a2212574ff0f5cb88c Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 9 Dec 2019 19:02:30 -0300 Subject: [PATCH] Sub: Improve attitude control in depth hold This adds a specific case for yaw input only, and adds a missing set to 'last_input_ms' which made the controls feel "sticky" --- ArduSub/control_althold.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index fe08ea011e..158f907d79 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -55,14 +55,20 @@ void Sub::handle_attitude() // If we don't have a mavlink attitude target, we use the pilot's input instead get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max()); target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) { + if (abs(target_roll) > 50 || abs(target_pitch) > 50) { last_roll = ahrs.roll_sensor; last_pitch = ahrs.pitch_sensor; last_yaw = ahrs.yaw_sensor; last_input_ms = tnow; attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + } else if (abs(target_yaw) > 50) { + // if only yaw is being controlled, don't update pitch and roll + attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw); + last_yaw = ahrs.yaw_sensor; + last_input_ms = tnow; } else if (tnow < last_input_ms + 250) { // just brake for a few mooments so we don't bounce + last_yaw = ahrs.yaw_sensor; attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0); } else { // Lock attitude