diff --git a/libraries/AP_Motors/AP_MotorsVectoredROV.cpp b/libraries/AP_Motors/AP_MotorsVectoredROV.cpp
index fc736a56c0..b0b7640d4b 100644
--- a/libraries/AP_Motors/AP_MotorsVectoredROV.cpp
+++ b/libraries/AP_Motors/AP_MotorsVectoredROV.cpp
@@ -18,8 +18,11 @@
  *
  */
 
+#include <AP_HAL/AP_HAL.h>
 #include "AP_MotorsVectoredROV.h"
 
+extern const AP_HAL::HAL& hal;
+
 // setup_motors - configures the motors for the BlueROV
 void AP_MotorsVectoredROV::setup_motors()
 {
@@ -84,3 +87,130 @@ void AP_MotorsVectoredROV::setup_motors()
     add_motor_raw_6dof(AP_MOTORS_MOT_5, MOT_5_ROLL_FACTOR, MOT_5_PITCH_FACTOR, MOT_5_YAW_FACTOR, MOT_5_THROTTLE_FACTOR, MOT_5_FORWARD_FACTOR, MOT_5_STRAFE_FACTOR,5);
     add_motor_raw_6dof(AP_MOTORS_MOT_6, MOT_6_ROLL_FACTOR, MOT_6_PITCH_FACTOR, MOT_6_YAW_FACTOR, MOT_6_THROTTLE_FACTOR, MOT_6_FORWARD_FACTOR, MOT_6_STRAFE_FACTOR,6);
 }
+
+
+// output_armed - sends commands to the motors
+// includes new scaling stability patch
+// TODO pull code that is common to output_armed_not_stabilizing into helper functions
+// ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/strafe commands
+void AP_MotorsVectoredROV::output_armed_stabilizing()
+{
+    int8_t i;
+    int16_t roll_pwm;                                               // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
+    int16_t pitch_pwm;                                              // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
+    int16_t yaw_pwm;                                                // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
+    int16_t throttle_radio_output;                                  // throttle pwm value, +/- 400
+    int16_t forward_pwm;                                            // forward pwm value, +/- 400
+    int16_t strafe_pwm;                                             // forward pwm value, +/- 400
+    int16_t out_min_pwm = 1100;      // minimum pwm value we can send to the motors
+    int16_t out_max_pwm = 1900;                      // maximum pwm value we can send to the motors
+//    int16_t out_mid_pwm = 1500;              // mid pwm value we can send to the motors
+                                      // the is the best throttle we can come up which provides good control without climbing
+
+    float rpy_scale = 1.0;                                          // this is used to scale the roll, pitch and yaw to fit within the motor limits
+
+    int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
+    int16_t linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
+    int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];    // final outputs sent to the motors
+
+    int16_t rpy_low = 0;    // lowest motor value
+    int16_t rpy_high = 0;   // highest motor value
+    int16_t yaw_allowed;    // amount of yaw we can fit in
+    int16_t thr_adj;        // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
+
+    // initialize limits flags
+    limit.roll_pitch = false;
+    limit.yaw = false;
+    limit.throttle_lower = false;
+    limit.throttle_upper = false;
+
+    // Ensure throttle is within bounds of 0 to 1000
+    int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
+    if (_throttle_control_input <= thr_in_min) {
+        _throttle_control_input = thr_in_min;
+        limit.throttle_lower = true;
+    }
+    if (_throttle_control_input >= _max_throttle) {
+        _throttle_control_input = _max_throttle;
+        limit.throttle_upper = true;
+    }
+
+    roll_pwm = calc_roll_pwm();
+    pitch_pwm = calc_pitch_pwm();
+    yaw_pwm = calc_yaw_pwm();
+    throttle_radio_output = (calc_throttle_radio_output()-_throttle_radio_min-(_throttle_radio_max-_throttle_radio_min)/2);
+    forward_pwm = get_forward()*0.4;
+    strafe_pwm = get_strafe()*0.4;
+
+    // calculate roll, pitch and yaw for each motor
+    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+
+        	rpy_out[i] = roll_pwm * _roll_factor[i] +
+        					pitch_pwm * _pitch_factor[i] +
+							yaw_pwm * _yaw_factor[i];
+
+        }
+    }
+
+    float forwardVerticalCouplingFactor = 1.0; // 0.0-1.5
+    int16_t forward_coupling_limit = max(forwardVerticalCouplingFactor*(400-fabs(throttle_radio_output)),0);
+    int8_t forward_coupling_direction[] = {-1,0,1,1,0,-1};
+
+    // calculate linear command for each motor
+    // linear factors should be 0.0 or 1.0 for now
+    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+
+        	float forward_pwm_limited;
+
+        	// The following statements decouple forward/vertical hydrodynamic coupling on
+        	// vectored ROVs. This is done by limiting the maximum output of the "rear" vectored
+        	// thruster (where "rear" depends on direction of travel).
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define sign(x) ((x>0)-(x<0))
+        	if ( sign(forward_pwm) == sign(forward_coupling_direction[i]) && forward_coupling_direction[i] != 0 ) {
+        		forward_pwm_limited = constrain(forward_pwm,-forward_coupling_limit,forward_coupling_limit);
+        	}
+#undef constrain
+
+        	linear_out[i] = throttle_radio_output * _throttle_factor[i] +
+        					forward_pwm_limited * _forward_factor[i] +
+							strafe_pwm * _strafe_factor[i];
+
+        }
+    }
+
+    // Calculate final pwm output for each motor
+    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+
+        	motor_out[i] = 1500 + _motor_reverse[i]*(rpy_out[i] + linear_out[i]);
+
+        }
+    }
+
+//    // apply thrust curve and voltage scaling
+//    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+//        if (motor_enabled[i]) {
+//            motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
+//        }
+//    }
+
+    // clip motor output if required (shouldn't be)
+    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
+        }
+    }
+
+    // send output to each motor
+    hal.rcout->cork();
+    for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+        if( motor_enabled[i] ) {
+            rc_write(i, motor_out[i]);
+        }
+    }
+    hal.rcout->push();
+}
+
diff --git a/libraries/AP_Motors/AP_MotorsVectoredROV.h b/libraries/AP_Motors/AP_MotorsVectoredROV.h
index ebfcaac675..512904dd35 100644
--- a/libraries/AP_Motors/AP_MotorsVectoredROV.h
+++ b/libraries/AP_Motors/AP_MotorsVectoredROV.h
@@ -24,6 +24,8 @@ public:
     // This takes care of all of the enabling and bitmask stuff seen in the Tricopter motors class
     virtual void        setup_motors();
 
+    void output_armed_stabilizing() override;
+
 protected:
 
 };