diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
index 08cb1057be..3edb1c40ac 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
@@ -378,3 +378,33 @@ void AC_AttitudeControl_Sub::parameter_sanity_check()
         _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT);
     }
 }
+
+// This function ensures that the ROV reaches the target orientation with the desired yaw rate
+void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float target_yaw_rate)
+{
+    // Convert from centidegrees on public interface to radians
+    const float euler_yaw_angle = wrap_PI(radians(euler_yaw_angle_cd * 0.01f));
+
+    const float current_yaw = AP::ahrs().get_yaw();
+
+    // Compute angle error
+    const float yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
+    
+    int direction = 0;
+    if (yaw_error < 0){
+        direction = -1;
+    } else {
+        direction = 1;
+    }
+
+    target_yaw_rate *= direction;
+
+
+    if (fabsf(yaw_error) > MAX_YAW_ERROR) {
+        // rotate the rov with desired yaw rate towards the target yaw
+        input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, target_yaw_rate);
+    } else {
+        // holds the rov's angles
+        input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, true);
+    }
+}
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h
index ee98bca306..a776526bcb 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h
@@ -23,6 +23,8 @@
 #define AC_ATC_SUB_RATE_YAW_IMAX       0.222f
 #define AC_ATC_SUB_RATE_YAW_FILT_HZ    5.0f
 
+#define MAX_YAW_ERROR                  radians(5)
+
 class AC_AttitudeControl_Sub : public AC_AttitudeControl {
 public:
     AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors);
@@ -60,6 +62,9 @@ public:
     // sanity check parameters.  should be called once before take-off
     void parameter_sanity_check() override;
 
+    // This function ensures that the ROV reaches the target orientation with the desired yaw rate
+    void input_euler_angle_roll_pitch_slew_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float slew_yaw);
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];