diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp
index 44f283bdb7..f9763c8573 100644
--- a/ArduCopter/autoyaw.cpp
+++ b/ArduCopter/autoyaw.cpp
@@ -26,31 +26,31 @@ void Mode::AutoYaw::set_mode_to_default(bool rtl)
 
 // default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
 // set rtl parameter to true if this is during an RTL
-autopilot_yaw_mode Mode::AutoYaw::default_mode(bool rtl) const
+Mode::AutoYaw::Mode Mode::AutoYaw::default_mode(bool rtl) const
 {
     switch (copter.g.wp_yaw_behavior) {
 
     case WP_YAW_BEHAVIOR_NONE:
-        return AUTO_YAW_HOLD;
+        return Mode::HOLD;
 
     case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
         if (rtl) {
-            return AUTO_YAW_HOLD;
+            return Mode::HOLD;
         } else {
-            return AUTO_YAW_LOOK_AT_NEXT_WP;
+            return Mode::LOOK_AT_NEXT_WP;
         }
 
     case WP_YAW_BEHAVIOR_LOOK_AHEAD:
-        return AUTO_YAW_LOOK_AHEAD;
+        return Mode::LOOK_AHEAD;
 
     case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
     default:
-        return AUTO_YAW_LOOK_AT_NEXT_WP;
+        return Mode::LOOK_AT_NEXT_WP;
     }
 }
 
 // set_mode - sets the yaw mode for auto
-void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
+void Mode::AutoYaw::set_mode(Mode yaw_mode)
 {
     // return immediately if no change
     if (_mode == yaw_mode) {
@@ -61,40 +61,41 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
     // perform initialisation
     switch (_mode) {
 
-    case AUTO_YAW_HOLD:
+    case Mode::HOLD:
         break;
 
-    case AUTO_YAW_LOOK_AT_NEXT_WP:
+    case Mode::LOOK_AT_NEXT_WP:
         // wpnav will initialise heading when wpnav's set_destination method is called
         break;
 
-    case AUTO_YAW_ROI:
+    case Mode::ROI:
         // look ahead until we know otherwise
         break;
 
-    case AUTO_YAW_FIXED:
+    case Mode::FIXED:
         // keep heading pointing in the direction held in fixed_yaw
         // caller should set the fixed_yaw
         break;
 
-    case AUTO_YAW_LOOK_AHEAD:
+    case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
         _look_ahead_yaw = copter.ahrs.yaw_sensor;
         break;
 
-    case AUTO_YAW_RESETTOARMEDYAW:
+    case Mode::RESETTOARMEDYAW:
         // initial_armed_bearing will be set during arming so no init required
         break;
 
-    case AUTO_YAW_ANGLE_RATE:
+    case Mode::ANGLE_RATE:
         break;
 
-    case AUTO_YAW_RATE:
+    case Mode::RATE:
         // initialise target yaw rate to zero
         _yaw_rate_cds = 0.0f;
         break;
 
-    case AUTO_YAW_CIRCLE:
+    case Mode::CIRCLE:
+    case Mode::PILOT_RATE:
         // no initialisation required
         break;
     }
@@ -130,7 +131,7 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
     }
 
     // set yaw mode
-    set_mode(AUTO_YAW_FIXED);
+    set_mode(Mode::FIXED);
 }
 
 // set_fixed_yaw - sets the yaw look at heading for auto mode
@@ -142,7 +143,7 @@ void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
     _yaw_rate_cds = yaw_rate_ds * 100.0;
 
     // set yaw mode
-    set_mode(AUTO_YAW_ANGLE_RATE);
+    set_mode(Mode::ANGLE_RATE);
 }
 
 // set_roi - sets the yaw to look at roi for auto mode
@@ -163,7 +164,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
         // check if mount type requires us to rotate the quad
         if (!copter.camera_mount.has_pan_control()) {
             if (roi_location.get_vector_from_origin_NEU(roi)) {
-                auto_yaw.set_mode(AUTO_YAW_ROI);
+                auto_yaw.set_mode(Mode::ROI);
             }
         }
         // send the command to the camera mount
@@ -178,7 +179,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
 #else
         // if we have no camera mount aim the quad at the location
         if (roi_location.get_vector_from_origin_NEU(roi)) {
-            auto_yaw.set_mode(AUTO_YAW_ROI);
+            auto_yaw.set_mode(Mode::ROI);
         }
 #endif  // HAL_MOUNT_ENABLED
     }
@@ -187,20 +188,37 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
 // set auto yaw rate in centi-degrees per second
 void Mode::AutoYaw::set_rate(float turn_rate_cds)
 {
-    set_mode(AUTO_YAW_RATE);
+    set_mode(Mode::RATE);
     _yaw_rate_cds = turn_rate_cds;
 }
 
+// return true if fixed yaw target has been reached
+bool Mode::AutoYaw::reached_fixed_yaw_target()
+{
+    if (mode() != Mode::FIXED) {
+        // should not happen, not in the right mode
+        return true;
+    }
+
+    if (!is_zero(_fixed_yaw_offset_cd)) {
+        // still slewing yaw target
+        return false;
+    }
+
+    // Within 2 deg of target
+    return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200);
+}
+
 // yaw - returns target heading depending upon auto_yaw.mode()
 float Mode::AutoYaw::yaw()
 {
     switch (_mode) {
 
-    case AUTO_YAW_ROI:
+    case Mode::ROI:
         // point towards a location held in roi
         return roi_yaw();
 
-    case AUTO_YAW_FIXED: {
+    case Mode::FIXED: {
         // keep heading pointing in the direction held in fixed_yaw
         // with no pilot input allowed
         const uint32_t now_ms = millis();
@@ -212,15 +230,15 @@ float Mode::AutoYaw::yaw()
         return _yaw_angle_cd;
     }
 
-    case AUTO_YAW_LOOK_AHEAD:
+    case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
         return look_ahead_yaw();
 
-    case AUTO_YAW_RESETTOARMEDYAW:
+    case Mode::RESETTOARMEDYAW:
         // changes yaw to be same as when quad was armed
         return copter.initial_armed_bearing;
 
-    case AUTO_YAW_CIRCLE:
+    case Mode::CIRCLE:
 #if MODE_CIRCLE_ENABLED
         if (copter.circle_nav->is_active()) {
             return copter.circle_nav->get_yaw();
@@ -229,7 +247,7 @@ float Mode::AutoYaw::yaw()
         // return the current attitude target
         return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
 
-    case AUTO_YAW_ANGLE_RATE:{
+    case Mode::ANGLE_RATE:{
         const uint32_t now_ms = millis();
         float dt = (now_ms - _last_update_ms) * 0.001;
         _last_update_ms = now_ms;
@@ -237,7 +255,7 @@ float Mode::AutoYaw::yaw()
         return _yaw_angle_cd;
     }
 
-    case AUTO_YAW_LOOK_AT_NEXT_WP:
+    case Mode::LOOK_AT_NEXT_WP:
     default:
         // point towards next waypoint.
         // we don't use wp_bearing because we don't want the copter to turn too much during flight
@@ -251,22 +269,64 @@ float Mode::AutoYaw::rate_cds() const
 {
     switch (_mode) {
 
-    case AUTO_YAW_HOLD:
-    case AUTO_YAW_ROI:
-    case AUTO_YAW_FIXED:
-    case AUTO_YAW_LOOK_AHEAD:
-    case AUTO_YAW_RESETTOARMEDYAW:
-    case AUTO_YAW_CIRCLE:
+    case Mode::HOLD:
+    case Mode::ROI:
+    case Mode::FIXED:
+    case Mode::LOOK_AHEAD:
+    case Mode::RESETTOARMEDYAW:
+    case Mode::CIRCLE:
         return 0.0f;
 
-    case AUTO_YAW_ANGLE_RATE:
-    case AUTO_YAW_RATE:
+    case Mode::ANGLE_RATE:
+    case Mode::RATE:
         return _yaw_rate_cds;
 
-    case AUTO_YAW_LOOK_AT_NEXT_WP:
+    case Mode::LOOK_AT_NEXT_WP:
         return copter.pos_control->get_yaw_rate_cds();
+
+    case Mode::PILOT_RATE:
+        return _pilot_yaw_rate_cds;
     }
 
     // return zero turn rate (this should never happen)
     return 0.0f;
 }
+
+AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
+{
+    // process pilot's yaw input
+    _pilot_yaw_rate_cds = 0.0;
+    if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
+        // get pilot's desired yaw rate
+        _pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
+        if (!is_zero(_pilot_yaw_rate_cds)) {
+            auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
+        }
+    } else if (auto_yaw.mode() == AutoYaw::Mode::PILOT_RATE) {
+        // RC failsafe, or disabled make sure not in pilot control
+        auto_yaw.set_mode(AutoYaw::Mode::HOLD);
+    }
+
+    AC_AttitudeControl::HeadingCommand heading;
+    heading.yaw_angle_cd = yaw();
+    heading.yaw_rate_cds = auto_yaw.rate_cds();
+
+    switch (auto_yaw.mode()) {
+        case Mode::HOLD:
+        case Mode::RATE:
+        case Mode::PILOT_RATE:
+            heading.heading_mode = AC_AttitudeControl::HeadingMode::Rate_Only;
+            break;
+        case Mode::LOOK_AT_NEXT_WP:
+        case Mode::ROI:
+        case Mode::FIXED:
+        case Mode::LOOK_AHEAD:
+        case Mode::RESETTOARMEDYAW:
+        case Mode::ANGLE_RATE:
+        case Mode::CIRCLE:
+            heading.heading_mode = AC_AttitudeControl::HeadingMode::Angle_And_Rate;
+            break;
+    }
+
+    return heading;
+}
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 37684bc398..7fa78252a7 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -10,19 +10,6 @@
 #define ENABLE ENABLED
 #define DISABLE DISABLED
 
-// Autopilot Yaw Mode enumeration
-enum autopilot_yaw_mode {
-    AUTO_YAW_HOLD =             0,  // pilot controls the heading
-    AUTO_YAW_LOOK_AT_NEXT_WP =  1,  // point towards next waypoint (no pilot input accepted)
-    AUTO_YAW_ROI =              2,  // point towards a location held in roi (no pilot input accepted)
-    AUTO_YAW_FIXED =            3,  // point towards a particular angle (no pilot input accepted)
-    AUTO_YAW_LOOK_AHEAD =       4,  // point in the direction the copter is moving
-    AUTO_YAW_RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
-    AUTO_YAW_ANGLE_RATE =       6,  // turn at a specified rate from a starting angle
-    AUTO_YAW_RATE =             7,  // turn at a specified rate (held in auto_yaw_rate)
-    AUTO_YAW_CIRCLE =           8,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
-};
-
 // Frame types
 #define UNDEFINED_FRAME 0
 #define MULTICOPTER_FRAME 1
diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index a2362c440d..1879219889 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -636,7 +636,6 @@ void Mode::land_run_vertical_control(bool pause_descent)
 void Mode::land_run_horizontal_control()
 {
     Vector2f vel_correction;
-    float target_yaw_rate = 0;
 
     // relax loiter target if we might be landed
     if (copter.ap.land_complete_maybe) {
@@ -671,12 +670,6 @@ void Mode::land_run_horizontal_control()
                 copter.ap.land_repo_active = true;
             }
         }
-
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
     }
 
     // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
@@ -730,13 +723,8 @@ void Mode::land_run_horizontal_control()
     }
 
     // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
-    }
+    attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading());
+
 }
 
 // run normal or precision landing (if enabled)
@@ -763,7 +751,6 @@ void Mode::land_run_normal_or_precland(bool pause_descent)
 // The passed in location is expected to be NED and in m
 void Mode::precland_retry_position(const Vector3f &retry_pos)
 {
-    float target_yaw_rate = 0;
     if (!copter.failsafe.radio) {
         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
@@ -790,11 +777,6 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
                 copter.ap.land_repo_active = true;
             }
         }
-
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
     }
 
     Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
@@ -806,16 +788,9 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
     pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
-    const Vector3f thrust_vector{pos_control->get_thrust_vector()};
-
     // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
-    }
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
+
 }
 
 // Run precland statemachine. This function should be called from any mode that wants to do precision landing.
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index d3fc0524f4..89978992da 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -232,17 +232,27 @@ public:
 
     public:
 
-        // yaw(): main product of AutoYaw; the heading:
-        float yaw();
+        // Autopilot Yaw Mode enumeration
+        enum class Mode {
+            HOLD =             0,  // hold zero yaw rate
+            LOOK_AT_NEXT_WP =  1,  // point towards next waypoint (no pilot input accepted)
+            ROI =              2,  // point towards a location held in roi (no pilot input accepted)
+            FIXED =            3,  // point towards a particular angle (no pilot input accepted)
+            LOOK_AHEAD =       4,  // point in the direction the copter is moving
+            RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
+            ANGLE_RATE =       6,  // turn at a specified rate from a starting angle
+            RATE =             7,  // turn at a specified rate (held in auto_yaw_rate)
+            CIRCLE =           8,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
+            PILOT_RATE =       9,  // target rate from pilot stick
+        };
 
         // mode(): current method of determining desired yaw:
-        autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; }
+        Mode mode() const { return _mode; }
         void set_mode_to_default(bool rtl);
-        void set_mode(autopilot_yaw_mode new_mode);
-        autopilot_yaw_mode default_mode(bool rtl) const;
+        void set_mode(Mode new_mode);
+        Mode default_mode(bool rtl) const;
+
 
-        // rate_cds(): desired yaw rate in centidegrees/second:
-        float rate_cds() const;
         void set_rate(float new_rate_cds);
 
         // set_roi(...): set a "look at" location:
@@ -255,17 +265,25 @@ public:
 
         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
 
-        bool fixed_yaw_slew_finished() { return is_zero(_fixed_yaw_offset_cd); }
+        bool reached_fixed_yaw_target();
+
+        AC_AttitudeControl::HeadingCommand get_heading();
 
     private:
 
+        // yaw(): main product of AutoYaw; the heading:
+        float yaw();
+
+        // rate_cds(): desired yaw rate in centidegrees/second:
+        float rate_cds() const;
+
         float look_ahead_yaw();
         float roi_yaw() const;
 
         // auto flight mode's yaw mode
-        uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
+        Mode _mode = Mode::LOOK_AT_NEXT_WP;
 
-        // Yaw will point at this location if mode is set to AUTO_YAW_ROI
+        // Yaw will point at this location if mode is set to Mode::ROI
         Vector3f roi;
 
         // yaw used for YAW_FIXED yaw_mode
@@ -283,6 +301,7 @@ public:
         // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
         float _yaw_angle_cd;
         float _yaw_rate_cds;
+        float _pilot_yaw_rate_cds;
     };
     static AutoYaw auto_yaw;
 
@@ -763,7 +782,6 @@ protected:
 private:
 
     // Circle
-    bool pilot_yaw_override = false; // true if pilot is overriding yaw
     bool speed_changing = false;     // true when the roll stick is being held to facilitate stopping at 0 rate
 };
 
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 5e2da22d44..d6e9092f44 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -34,8 +34,8 @@ bool ModeAuto::init(bool ignore_checks)
 
         // stop ROI from carrying over from previous runs of the mission
         // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
-        if (auto_yaw.mode() == AUTO_YAW_ROI) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
+        if (auto_yaw.mode() == AutoYaw::Mode::ROI) {
+            auto_yaw.set_mode(AutoYaw::Mode::HOLD);
         }
 
         // initialise waypoint and spline controller
@@ -277,7 +277,7 @@ bool ModeAuto::loiter_start()
     wp_nav->set_wp_destination(stopping_point);
 
     // hold yaw at current heading
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
     return true;
 }
@@ -337,7 +337,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
     // clear i term when we're taking off
     pos_control->init_z_controller();
@@ -373,7 +373,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
         auto_yaw.set_mode_to_default(false);
     }
 
@@ -403,7 +403,7 @@ void ModeAuto::land_start()
     }
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
 #if LANDING_GEAR_ENABLED == ENABLED
     // optionally deploy landing gear
@@ -460,12 +460,12 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
         const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center().xy());
         // initialise yaw
         // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-        if (auto_yaw.mode() != AUTO_YAW_ROI) {
+        if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
             if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
                 auto_yaw.set_mode_to_default(false);
             } else {
                 // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
-                auto_yaw.set_mode(AUTO_YAW_HOLD);
+                auto_yaw.set_mode(AutoYaw::Mode::HOLD);
             }
         }
 
@@ -483,8 +483,8 @@ void ModeAuto::circle_start()
     // initialise circle controller
     copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
 
-    if (auto_yaw.mode() != AUTO_YAW_ROI) {
-        auto_yaw.set_mode(AUTO_YAW_CIRCLE);
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
+        auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
     }
 
     // set submode to circle
@@ -552,7 +552,7 @@ void ModeAuto::payload_place_start()
     }
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
     // set submode
     set_submode(SubMode::NAV_PAYLOAD_PLACE);
@@ -958,16 +958,6 @@ void ModeAuto::takeoff_run()
 //      called by auto_run at 100hz or more
 void ModeAuto::wp_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
@@ -984,14 +974,8 @@ void ModeAuto::wp_run()
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // auto_land_run - lands in auto mode
@@ -1024,16 +1008,6 @@ void ModeAuto::rtl_run()
 //      called by auto_run at 100hz or more
 void ModeAuto::circle_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // call circle controller
     copter.failsafe_terrain_set_status(copter.circle_nav->update());
 
@@ -1041,13 +1015,8 @@ void ModeAuto::circle_run()
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), auto_yaw.yaw());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 #if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
@@ -1070,12 +1039,6 @@ void ModeAuto::loiter_run()
         return;
     }
 
-    // accept pilot input of yaw
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-    }
-
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
@@ -1083,7 +1046,9 @@ void ModeAuto::loiter_run()
     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     pos_control->update_z_controller();
-    attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
+
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // auto_loiter_run - loiter to altitude in AUTO flight mode
@@ -1355,7 +1320,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
         auto_yaw.set_mode_to_default(false);
     }
 
@@ -1577,7 +1542,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+    if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
         auto_yaw.set_mode_to_default(false);
     }
 
@@ -2107,10 +2072,10 @@ bool ModeAuto::verify_within_distance()
 bool ModeAuto::verify_yaw()
 {
     // make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
-    auto_yaw.set_mode(AUTO_YAW_FIXED);
+    auto_yaw.set_mode(AutoYaw::Mode::FIXED);
 
-    // check if we are within 2 degrees of the target heading
-    return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
+    // check if we have reached the target heading
+    return auto_yaw.reached_fixed_yaw_target();
 }
 
 // verify_nav_wp - check if we have reached the next way point
diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp
index 7ec53c2674..1835fcd6c7 100644
--- a/ArduCopter/mode_circle.cpp
+++ b/ArduCopter/mode_circle.cpp
@@ -9,7 +9,6 @@
 // circle_init - initialise circle controller flight mode
 bool ModeCircle::init(bool ignore_checks)
 {
-    pilot_yaw_override = false;
     speed_changing = false;
 
     // set speed and acceleration limits
@@ -32,12 +31,6 @@ void ModeCircle::run()
     pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 
-    // get pilot's desired yaw rate (or zero if in radio failsafe)
-    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-    if (!is_zero(target_yaw_rate)) {
-        pilot_yaw_override = true;
-    }
-
     // Check for any change in params and update in real time
     copter.circle_nav->check_param_change();
 
@@ -106,14 +99,10 @@ void ModeCircle::run()
     copter.surface_tracking.update_surface_offset();
 
     copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
-
-    // call attitude controller
-    if (pilot_yaw_override) {
-        attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), copter.circle_nav->get_yaw());
-    }
     pos_control->update_z_controller();
+
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 uint32_t ModeCircle::wp_distance() const
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 2bbe33fd8c..2077f0c660 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -140,7 +140,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     guided_mode = SubMode::TakeOff;
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
     // clear i term when we're taking off
     pos_control->init_z_controller();
@@ -178,16 +178,6 @@ void ModeGuided::wp_control_start()
 // run guided mode's waypoint navigation controller
 void ModeGuided::wp_control_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         // do not spool down tradheli when on the ground with motor interlock enabled
@@ -204,17 +194,8 @@ void ModeGuided::wp_control_run()
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // initialise position controller
@@ -332,7 +313,7 @@ void ModeGuided::angle_control_start()
     guided_angle_state.use_yaw_rate = false;
 
     // pilot always controls yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 }
 
 // set_destination - sets guided mode's target destination
@@ -682,17 +663,6 @@ void ModeGuided::takeoff_run()
 // called from guided_run
 void ModeGuided::pos_control_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         // do not spool down tradheli when on the ground with motor interlock enabled
@@ -717,8 +687,8 @@ void ModeGuided::pos_control_run()
 
     // stop rotating if no updates received within timeout_ms
     if (millis() - update_time_ms > get_timeout_ms()) {
-        if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
-            auto_yaw.set_rate(0.0f);
+        if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
+            auto_yaw.set_mode(AutoYaw::Mode::HOLD);
         }
     }
 
@@ -732,33 +702,14 @@ void ModeGuided::pos_control_run()
     pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // velaccel_control_run - runs the guided velocity controller
 // called from guided_run
 void ModeGuided::accel_control_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         // do not spool down tradheli when on the ground with motor interlock enabled
@@ -774,8 +725,8 @@ void ModeGuided::accel_control_run()
     if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
-        if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
-            auto_yaw.set_rate(0.0f);
+        if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
+            auto_yaw.set_mode(AutoYaw::Mode::HOLD);
         }
         pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
         pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
@@ -796,33 +747,14 @@ void ModeGuided::accel_control_run()
     pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // velaccel_control_run - runs the guided velocity and acceleration controller
 // called from guided_run
 void ModeGuided::velaccel_control_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         // do not spool down tradheli when on the ground with motor interlock enabled
@@ -838,8 +770,8 @@ void ModeGuided::velaccel_control_run()
     if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
-        if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
-            auto_yaw.set_rate(0.0f);
+        if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
+            auto_yaw.set_mode(AutoYaw::Mode::HOLD);
         }
     }
 
@@ -871,17 +803,8 @@ void ModeGuided::velaccel_control_run()
     pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // pause_control_run - runs the guided mode pause controller
@@ -911,24 +834,13 @@ void ModeGuided::pause_control_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // posvelaccel_control_run - runs the guided position, velocity and acceleration controller
 // called from guided_run
 void ModeGuided::posvelaccel_control_run()
 {
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         // do not spool down tradheli when on the ground with motor interlock enabled
@@ -944,8 +856,8 @@ void ModeGuided::posvelaccel_control_run()
     if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
-        if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
-            auto_yaw.set_rate(0.0f);
+        if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
+            auto_yaw.set_mode(AutoYaw::Mode::HOLD);
         }
     }
 
@@ -983,17 +895,8 @@ void ModeGuided::posvelaccel_control_run()
     pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // angle_control_run - runs the guided angle controller
diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp
index 8c518e5cec..b81b97af80 100644
--- a/ArduCopter/mode_land.cpp
+++ b/ArduCopter/mode_land.cpp
@@ -34,7 +34,7 @@ bool ModeLand::init(bool ignore_checks)
     copter.ap.prec_land_active = false;
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
 #if LANDING_GEAR_ENABLED == ENABLED
     // optionally deploy landing gear
@@ -98,7 +98,6 @@ void ModeLand::gps_run()
 void ModeLand::nogps_run()
 {
     float target_roll = 0.0f, target_pitch = 0.0f;
-    float target_yaw_rate = 0;
 
     // process pilot inputs
     if (!copter.failsafe.radio) {
@@ -116,11 +115,6 @@ void ModeLand::nogps_run()
             get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
         }
 
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
     }
 
     // disarm when the landing detector says we've landed
@@ -144,7 +138,7 @@ void ModeLand::nogps_run()
     }
 
     // call attitude controller
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
+    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, auto_yaw.get_heading().yaw_rate_cds);
 }
 
 // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index 4d320370c9..08234a76bb 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -138,7 +138,7 @@ void ModeRTL::climb_start()
     }
 
     // hold current yaw during initial climb
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 }
 
 // rtl_return_start - initialise return to home
@@ -166,16 +166,6 @@ void ModeRTL::climb_return_run()
         return;
     }
 
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
@@ -186,14 +176,8 @@ void ModeRTL::climb_return_run()
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // check if we've completed this stage of RTL
     _state_complete = wp_nav->reached_wp_destination();
@@ -207,10 +191,10 @@ void ModeRTL::loiterathome_start()
     _loiter_start_time = millis();
 
     // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
-    if (auto_yaw.default_mode(true) != AUTO_YAW_HOLD) {
-        auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
+    if (auto_yaw.default_mode(true) != AutoYaw::Mode::HOLD) {
+        auto_yaw.set_mode(AutoYaw::Mode::RESETTOARMEDYAW);
     } else {
-        auto_yaw.set_mode(AUTO_YAW_HOLD);
+        auto_yaw.set_mode(AutoYaw::Mode::HOLD);
     }
 }
 
@@ -224,16 +208,6 @@ void ModeRTL::loiterathome_run()
         return;
     }
 
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
@@ -244,18 +218,12 @@ void ModeRTL::loiterathome_run()
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // check if we've completed this stage of RTL
     if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
-        if (auto_yaw.mode() == AUTO_YAW_RESETTOARMEDYAW) {
+        if (auto_yaw.mode() == AutoYaw::Mode::RESETTOARMEDYAW) {
             // check if heading is within 2 degrees of heading when vehicle was armed
             if (abs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
                 _state_complete = true;
@@ -277,7 +245,7 @@ void ModeRTL::descent_start()
     pos_control->init_z_controller_stopping_point();
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
 #if LANDING_GEAR_ENABLED == ENABLED
     // optionally deploy landing gear
@@ -295,7 +263,6 @@ void ModeRTL::descent_start()
 void ModeRTL::descent_run()
 {
     Vector2f vel_correction;
-    float target_yaw_rate = 0.0f;
 
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
@@ -328,11 +295,6 @@ void ModeRTL::descent_run()
                 copter.ap.land_repo_active = true;
             }
         }
-
-        if (g.land_repositioning || use_pilot_yaw()) {
-            // get pilot's desired yaw rate
-            target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        }
     }
 
     // set motors to full range
@@ -348,7 +310,7 @@ void ModeRTL::descent_run()
     pos_control->update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // check if we've reached within 20cm of final altitude
     _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
@@ -375,7 +337,7 @@ void ModeRTL::land_start()
     }
 
     // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AutoYaw::Mode::HOLD);
 
 #if LANDING_GEAR_ENABLED == ENABLED
     // optionally deploy landing gear
diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp
index d2953dce57..2014dae95e 100644
--- a/ArduCopter/mode_smart_rtl.cpp
+++ b/ArduCopter/mode_smart_rtl.cpp
@@ -70,7 +70,7 @@ void ModeSmartRTL::wait_cleanup_run()
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     wp_nav->update_wpnav();
     pos_control->update_z_controller();
-    attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // check if return path is computed and if yes, begin journey home
     if (g2.smart_rtl.request_thorough_cleanup()) {
@@ -81,15 +81,6 @@ void ModeSmartRTL::wait_cleanup_run()
 
 void ModeSmartRTL::path_follow_run()
 {
-    float target_yaw_rate = 0.0f;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // if we are close to current target point, switch the next point to be our target.
     if (wp_nav->reached_wp_destination()) {
         Vector3f dest_NED;
@@ -140,14 +131,8 @@ void ModeSmartRTL::path_follow_run()
     wp_nav->update_wpnav();
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 void ModeSmartRTL::pre_land_position_run()
@@ -169,7 +154,7 @@ void ModeSmartRTL::pre_land_position_run()
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     wp_nav->update_wpnav();
     pos_control->update_z_controller();
-    attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 }
 
 // save current position for use by the smart_rtl flight mode
diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp
index 46d3c81d37..0d7d3e7ce6 100644
--- a/ArduCopter/takeoff.cpp
+++ b/ArduCopter/takeoff.cpp
@@ -139,16 +139,6 @@ void Mode::auto_takeoff_run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
-        if (!is_zero(target_yaw_rate)) {
-            auto_yaw.set_mode(AUTO_YAW_HOLD);
-        }
-    }
-
     // aircraft stays in landed state until rotor speed run up has finished
     if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
         // motors have not completed spool up yet so relax navigation and position controllers
@@ -208,17 +198,8 @@ void Mode::auto_takeoff_run()
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
-    // call attitude controller
-    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
-        // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
-    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
-        // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
-    } else {
-        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
-    }
+    // call attitude controller with auto yaw
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // handle takeoff completion
     bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90);