diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index b3e1e487a6..38997bf425 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -1460,6 +1460,9 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
             // param3 : throttle (range depends upon param2)
             // param4 : timeout (in seconds)
+            if (sub.handle_do_motor_test(packet)) {
+                result = MAV_RESULT_ACCEPTED;
+            }
             break;
 
 #if GRIPPER_ENABLED == ENABLED
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 70a51b4966..9b74f750aa 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -745,6 +745,10 @@ private:
     void surface_run();
 
     void convert_old_parameters(void);
+    bool handle_do_motor_test(mavlink_command_long_t command);
+    bool init_motor_test();
+    bool verify_motor_test();
+
 
     bool control_check_barometer();
 
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 2a51442f1b..946e9d61ff 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -139,11 +139,111 @@ void Sub::motors_output()
 {
     // check if we are performing the motor test
     if (ap.motor_test) {
-        return; // Placeholder
+        verify_motor_test();
+    } else {
+        motors.set_interlock(true);
+        motors.output();
     }
-    motors.set_interlock(true);
-    motors.output();
 }
+static uint32_t last_do_motor_test_fail_ms = 0;
+static uint32_t last_do_motor_test_ms = 0;
+// Initialize new style motor test
+// Perform checks to see if it is ok to begin the motor test
+// Returns true if motor test has begun
+bool Sub::init_motor_test()
+{
+    uint32_t tnow = AP_HAL::millis();
+
+    // Ten second cooldown period required with no do_set_motor requests required
+    // after failure.
+    if (tnow < last_do_motor_test_fail_ms + 10000 && last_do_motor_test_fail_ms > 0) {
+        gcs_send_text(MAV_SEVERITY_CRITICAL, "10 second cool down required");
+    }
+
+    // check if safety switch has been pushed
+    if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        gcs_send_text(MAV_SEVERITY_CRITICAL,"Disarm hardware safety switch before testing motors.");
+        return false;
+    }
+
+    // Make sure we are on the ground
+    if (!motors.armed()) {
+        gcs_send_text(MAV_SEVERITY_WARNING, "Arm motors before testing motors.");
+        return false;
+    }
+
+    enable_motor_output(); // set all motor outputs to zero
+    ap.motor_test = true;
+
+    return true;
+}
+
+// Verify new style motor test
+// The motor test will fail if the interval between received
+// MAV_CMD_DO_SET_MOTOR requests exceeds a timeout period
+// Returns true if it is ok to proceed with new style motor test
+bool Sub::verify_motor_test()
+{
+    bool pass = true;
+
+    // Require at least 2 Hz incoming do_set_motor requests
+    if (AP_HAL::millis() > last_do_motor_test_ms + 500) {
+        gcs_send_text(MAV_SEVERITY_WARNING, "Motor test timed out!");
+        pass = false;
+    }
+
+    if (!pass) {
+        ap.motor_test = false;
+        motors.armed(false); // disarm motors
+        last_do_motor_test_fail_ms = AP_HAL::millis();
+        return false;
+    }
+
+    return true;
+}
+
+bool Sub::handle_do_motor_test(mavlink_command_long_t command) {
+    last_do_motor_test_ms = AP_HAL::millis();
+
+    // If we are not already testing motors, initialize test
+    if(!ap.motor_test) {
+        if (!init_motor_test()) {
+            gcs_send_text(MAV_SEVERITY_WARNING, "motor test initialization failed!");
+            return false; // init fail
+        }
+    }
+
+    float motor_number = command.param1;
+    float throttle_type = command.param2;
+    float throttle = command.param3;
+    float timeout_s = command.param4;
+    float test_type = command.param5;
+
+    if (!is_equal(test_type, (float)MOTOR_TEST_ORDER_BOARD) &&
+        !is_equal(test_type, (float)MOTOR_TEST_ORDER_DEFAULT)) {
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "bad test type %0.2f", test_type);
+        return false; // test type not supported here
+    }
+
+    if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PILOT)) {
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "bad throtle type %0.2f", throttle_type);
+
+        return false; // throttle type not supported here
+    }
+
+    if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PWM)) {
+        return motors.set_output(motor_number, throttle); // true if motor output is set
+    }
+
+    if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PERCENT)) {
+        throttle = constrain_float(throttle, 0.0f, 100.0f);
+        throttle = channel_throttle->get_radio_min() + throttle / 100.0f * (channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
+        return motors.set_output(motor_number, throttle); // true if motor output is set
+    }
+
+    return false;
+}
+
 
 // translate wpnav roll/pitch outputs to lateral/forward
 void Sub::translate_wpnav_rp(float &lateral_out, float &forward_out)