diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp
index 88a59e4517..af5e2a3e8b 100644
--- a/ArduSub/control_stabilize.cpp
+++ b/ArduSub/control_stabilize.cpp
@@ -42,6 +42,7 @@ void Sub::stabilize_run()
 
 void Sub::handle_attitude()
 {
+    uint32_t tnow = AP_HAL::millis();
     float desired_roll_rate, desired_pitch_rate, desired_yaw_rate;
     // initialize vertical speeds and acceleration
     pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@@ -53,32 +54,50 @@ void Sub::handle_attitude()
     float yaw_input =  channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim());
     desired_yaw_rate = get_pilot_desired_yaw_rate(yaw_input);
 
-    switch (g.control_frame) {
-      case MAV_FRAME_BODY_FRD:
-      {
-        if (abs(desired_roll_rate) > 50 || abs(desired_pitch_rate) > 50 || abs(desired_yaw_rate) > 50) {
-            attitude_control.input_rate_bf_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
-            Vector3f attitude_target = attitude_control.get_att_target_euler_cd();
-            last_roll = attitude_target.x;
-            last_pitch = attitude_target.y;
-            last_pilot_heading = attitude_target.z;
-        } else {
-            attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+    switch (g.control_frame)
+    {
+        case MAV_FRAME_BODY_FRD:
+        {
+            if (abs(desired_roll_rate) > 50 || abs(desired_pitch_rate) > 50 || abs(desired_yaw_rate) > 50)
+            {
+                attitude_control.input_rate_bf_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
+                Quaternion attitude_target = attitude_control.get_attitude_target_quat();
+                last_roll = degrees(attitude_target.get_euler_roll()) * 100;
+                last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
+                last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
+            }
+            else
+            {
+                attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+            }
         }
-      }
-      break;
-      default:
-      {
-        if (!is_zero(desired_roll_rate) || !is_zero(desired_pitch_rate) || !is_zero(desired_yaw_rate)) {
-            attitude_control.input_euler_rate_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
-            Vector3f attitude_target = attitude_control.get_att_target_euler_cd();
-            last_roll = attitude_target.x;
-            last_pitch = attitude_target.y;
-            last_pilot_heading = attitude_target.z;
-            last_input_ms = AP_HAL::millis();
-            return;
+        break;
+        default:
+        {
+            if (!is_zero(desired_yaw_rate))
+            { // call attitude controller with rate yaw determined by pilot input
+                attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, desired_yaw_rate);
+                last_pilot_heading = ahrs.yaw_sensor;
+                last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
+            }
+            else
+            { // hold current heading
+
+                // this check is required to prevent bounce back after very fast yaw maneuvers
+                // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
+                if (tnow < last_pilot_yaw_input_ms + 250)
+                {                         // give 250ms to slow down, then set target heading
+                    desired_yaw_rate = 0; // Stop rotation on yaw axis
+
+                    // call attitude controller with target yaw rate = 0 to decelerate onqq yaw axis
+                    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, desired_yaw_rate);
+                    last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
+                }
+                else
+                { // call attitude controller holding absolute absolute bearing
+                    attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+                }
+            }
         }
-        attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
-      }
     }
 }