diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 846634e5ee838..648c5b4b90ddd 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -7,6 +7,7 @@
 import os
 import shutil
 import time
+import numpy
 
 from pymavlink import mavutil
 from pymavlink import mavextra
@@ -1847,6 +1848,73 @@ def fly_motor_fail(self, fail_servo=0, fail_mul=0.0, holdtime=30):
 
         self.do_RTL()
 
+    def fly_motor_vibration(self):
+        """Test flight with motor vibration"""
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_rc_default()
+            self.set_parameter("INS_LOG_BAT_MASK", 3)
+            self.set_parameter("INS_LOG_BAT_OPT", 2)
+            self.set_parameter("LOG_BITMASK", 958)
+            self.set_parameter("LOG_DISARMED", 0)
+            self.set_parameter("SIM_VIB_MOT_MAX", 350)
+            self.set_parameter("SIM_GYR_RND", 100)
+            self.set_parameter("SIM_DRIFT_SPEED", 0)
+            self.set_parameter("SIM_DRIFT_TIME", 0)
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="LOITER")
+            self.change_alt(alt_min=15)
+
+            hover_time = 5
+            tstart = self.get_sim_time()
+            self.progress("Hovering for %u seconds" % hover_time)
+            while self.get_sim_time_cached() < tstart + hover_time:
+                attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+            tend = self.get_sim_time()
+
+            self.do_RTL()
+            psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
+            # ignore the first 20Hz
+            ignore_bins = 20
+            freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
+            if numpy.amax(psd["X"][ignore_bins:]) < -22 or freq < 200 or freq > 300:
+                raise NotAchievedException("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
+            else:
+                self.progress("Detected motor peak at %fHz" % freq)
+
+            # now add a notch and check that the peak is squashed
+            self.set_parameter("INS_NOTCH_ENABLE", 1)
+            self.set_parameter("INS_NOTCH_FREQ", freq)
+            self.set_parameter("INS_NOTCH_ATT", 50)
+            self.set_parameter("INS_NOTCH_BW", freq/2)
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="LOITER")
+            self.change_alt(alt_min=15)
+
+            tstart = self.get_sim_time()
+            self.progress("Hovering for %u seconds" % hover_time)
+            while self.get_sim_time_cached() < tstart + hover_time:
+                attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+            tend = self.get_sim_time()
+            self.do_RTL()
+            psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
+            freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
+            if numpy.amax(psd["X"][ignore_bins:]) < -22:
+                self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
+            else:
+                raise NotAchievedException("Detected motor peak at %f Hz" % (freq))
+        except Exception as e:
+            ex = e
+
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
     def fly_vision_position(self):
         """Disable GPS navigation, enable Vicon input."""
         # scribble down a location we can set origin to:
@@ -4403,12 +4471,15 @@ def tests(self):
              "Test rangefinder drivers",
              self.fly_rangefinder_drivers),
 
+            ("MotorVibration",
+             "Fly motor vibration test",
+             self.fly_motor_vibration),
+
             ("LogDownLoad",
              "Log download",
              lambda: self.log_download(
                  self.buildlogs_path("ArduCopter-log.bin"),
                  upload_logs=len(self.fail_list) > 0))
-
         ])
         return ret
 
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 38bfec28b1fee..e01aac1841519 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -11,6 +11,7 @@
 import pexpect
 import fnmatch
 import operator
+import numpy
 
 from MAVProxy.modules.lib import mp_util
 
@@ -3669,3 +3670,98 @@ def autotest(self):
         ret = self.run_tests(tests)
         self.post_tests_announcements()
         return ret
+
+    def mavfft_fttd(self, sensor_type, sensor_instance, since, until):
+        '''display fft for raw ACC data in current logfile'''
+
+        '''object to store data about a single FFT plot'''
+        class MessageData(object):
+            def __init__(self, ffth):
+                self.seqno = -1
+                self.fftnum = ffth.N
+                self.sensor_type = ffth.type
+                self.instance = ffth.instance
+                self.sample_rate_hz = ffth.smp_rate
+                self.multiplier = ffth.mul
+                self.sample_us = ffth.SampleUS
+                self.data = {}
+                self.data["X"] = []
+                self.data["Y"] = []
+                self.data["Z"] = []
+                self.holes = False
+                self.freq = None
+
+            def add_fftd(self, fftd):
+                self.seqno += 1
+                self.data["X"].extend(fftd.x)
+                self.data["Y"].extend(fftd.y)
+                self.data["Z"].extend(fftd.z)
+
+        mlog = self.dfreader_for_current_onboard_log()
+
+        # see https://holometer.fnal.gov/GH_FFT.pdf for a description of the techniques used here
+        messages = []
+        messagedata = None
+        while True:
+            m = mlog.recv_match()
+            if m is None:
+                break
+            msg_type = m.get_type()
+            if msg_type == "ISBH":
+                if messagedata is not None:
+                    if messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance and messagedata.sample_us > since and messagedata.sample_us < until:
+                        messages.append(messagedata)
+                messagedata = MessageData(m)
+                continue
+
+            if msg_type == "ISBD":
+                if messagedata is not None and messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance:
+                    messagedata.add_fftd(m)
+
+        fft_len = len(messages[0].data["X"])
+        sum_fft = {
+                "X": numpy.zeros(fft_len/2+1),
+                "Y": numpy.zeros(fft_len/2+1),
+                "Z": numpy.zeros(fft_len/2+1),
+            }
+        sample_rate = 0
+        counts = 0
+        window = numpy.hanning(fft_len)
+        freqmap = numpy.fft.rfftfreq(fft_len, 1.0 / messages[0].sample_rate_hz)
+
+        # calculate NEBW constant
+        S2 = numpy.inner(window, window)
+
+        for message in messages:
+            for axis in [ "X","Y","Z" ]:
+                # normalize data and convert to dps in order to produce more meaningful magnitudes
+                if message.sensor_type == 1:
+                    d = numpy.array(numpy.degrees(message.data[axis])) / float(message.multiplier)
+                else:
+                    d = numpy.array(message.data[axis]) / float(message.multiplier)
+
+                # apply window to the input
+                d *= window
+                # perform RFFT
+                d_fft = numpy.fft.rfft(d)
+                # convert to squared complex magnitude
+                d_fft = numpy.square(abs(d_fft))
+                # remove DC component
+                d_fft[0] = 0
+                d_fft[-1] = 0
+                # accumulate the sums
+                sum_fft[axis] += d_fft
+
+            sample_rate = message.sample_rate_hz
+            counts += 1
+
+        numpy.seterr(divide = 'ignore')
+        psd = {}
+        for axis in [ "X","Y","Z" ]:
+            # normalize output to averaged PSD
+            psd[axis] = 2 * (sum_fft[axis] / counts) / (sample_rate * S2)
+            psd[axis] = 10 * numpy.log10 (psd[axis])
+
+        psd["F"] = freqmap
+
+        return psd
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index a293c1db5a9c3..cae816c48f5cb 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -177,6 +177,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
         dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
         _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
+        sample_us = _imu._gyro_last_sample_us[instance];
     }
 
 #if AP_MODULE_SUPPORTED
@@ -353,6 +354,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
 
         dt = 1.0f / _imu._accel_raw_sample_rates[instance];
         _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
+        sample_us = _imu._accel_last_sample_us[instance];
     }
 
 #if AP_MODULE_SUPPORTED
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
index a936fbe517a8b..c36ca6f5e0100 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
@@ -66,24 +66,44 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 
     if (sitl->motors_on) {
         // add extra noise when the motors are on
-        accel_noise += instance==0?sitl->accel_noise:sitl->accel2_noise;
+        accel_noise += instance == 0 ? sitl->accel_noise : sitl->accel2_noise;
     }
 
     // add accel bias and noise
-    Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
+    Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get();
     float xAccel = sitl->state.xAccel + accel_bias.x;
     float yAccel = sitl->state.yAccel + accel_bias.y;
     float zAccel = sitl->state.zAccel + accel_bias.z;
     const Vector3f &vibe_freq = sitl->vibe_freq;
-    if (vibe_freq.is_zero()) {
+    bool vibe_motor = !is_zero(sitl->vibe_motor);
+    if (vibe_freq.is_zero() && !vibe_motor) {
         xAccel += accel_noise * rand_float();
         yAccel += accel_noise * rand_float();
         zAccel += accel_noise * rand_float();
     } else {
-        float t = AP_HAL::micros() * 1.0e-6f;
-        xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
-        yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
-        zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
+        if (vibe_motor) {
+            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+                float& phase = accel_motor_phase[instance][i];
+                float motor_freq = sitl->state.rpm[i] / 60.0f;
+                float phase_incr = motor_freq * 2 * M_PI / accel_sample_hz[instance];
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+                xAccel += sinf(phase) * accel_noise;
+                yAccel += sinf(phase) * accel_noise;
+                zAccel += sinf(phase) * accel_noise;
+            }
+        }
+        if (!vibe_freq.is_zero()) {
+            float t = AP_HAL::micros() * 1.0e-6f;
+            xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
+            yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
+            zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
+        }
     }
     
     // correct for the acceleration due to the IMU position offset and angular acceleration
@@ -116,7 +136,7 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 
     _rotate_and_correct_accel(accel_instance[instance], accel);
 
-    uint8_t nsamples = enable_fast_sampling(accel_instance[instance])?4:1;
+    uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1;
     for (uint8_t i=0; i<nsamples; i++) {
         _notify_new_accel_raw_sample(accel_instance[instance], accel);
     }
@@ -142,29 +162,50 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
     float r = radians(sitl->state.yawRate) + gyro_drift();
 
     const Vector3f &vibe_freq = sitl->vibe_freq;
-    if (vibe_freq.is_zero()) {
+    bool vibe_motor = !is_zero(sitl->vibe_motor);
+    if (vibe_freq.is_zero() && !vibe_motor) {
         p += gyro_noise * rand_float();
         q += gyro_noise * rand_float();
         r += gyro_noise * rand_float();
     } else {
-        float t = AP_HAL::micros() * 1.0e-6f;
-        p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
-        q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
-        r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
+        if (vibe_motor) {
+            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+                float motor_freq = sitl->state.rpm[i] / 60.0f;
+                float phase_incr = motor_freq * 2 * M_PI / gyro_sample_hz[instance];
+                float& phase = gyro_motor_phase[instance][i];
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+
+                p += sinf(phase) * gyro_noise;
+                q += sinf(phase) * gyro_noise;
+                r += sinf(phase) * gyro_noise;
+            }
+        }
+        if (!vibe_freq.is_zero()) {
+            float t = AP_HAL::micros() * 1.0e-6f;
+            p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
+            q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
+            r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
+        }
     }
 
     Vector3f gyro = Vector3f(p, q, r);
 
     // add in gyro scaling
     Vector3f scale = sitl->gyro_scale;
-    gyro.x *= (1 + scale.x*0.01f);
-    gyro.y *= (1 + scale.y*0.01f);
-    gyro.z *= (1 + scale.z*0.01f);
+    gyro.x *= (1 + scale.x * 0.01f);
+    gyro.y *= (1 + scale.y * 0.01f);
+    gyro.z *= (1 + scale.z * 0.01f);
 
     _rotate_and_correct_gyro(gyro_instance[instance], gyro);
     
-    uint8_t nsamples = enable_fast_sampling(gyro_instance[instance])?8:1;
-    for (uint8_t i=0; i<nsamples; i++) {
+    uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1;
+    for (uint8_t i = 0; i < nsamples; i++) {
         _notify_new_gyro_raw_sample(gyro_instance[instance], gyro);
     }
 }
@@ -183,16 +224,26 @@ void AP_InertialSensor_SITL::timer_update(void)
         if (now >= next_accel_sample[i]) {
             if (((1U<<i) & sitl->accel_fail_mask) == 0) {
                 generate_accel(i);
-                while (now >= next_accel_sample[i]) {
-                    next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
+                if (next_accel_sample[i] == 0) {
+                    next_accel_sample[i] = now + 1000000UL / accel_sample_hz[i];
+                }
+                else {
+                    while (now >= next_accel_sample[i]) {
+                        next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
+                    }
                 }
             }
         }
         if (now >= next_gyro_sample[i]) {
             if (((1U<<i) & sitl->gyro_fail_mask) == 0) {
                 generate_gyro(i);
-                while (now >= next_gyro_sample[i]) {
-                    next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
+                if (next_gyro_sample[i] == 0) {
+                    next_gyro_sample[i] = now + 1000000UL / gyro_sample_hz[i];
+                }
+                else {
+                    while (now >= next_gyro_sample[i]) {
+                        next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
+                    }
                 }
             }
         }
@@ -211,7 +262,6 @@ float AP_InertialSensor_SITL::gyro_drift(void)
         return minutes * ToRad(sitl->drift_speed);
     }
     return (period - minutes) * ToRad(sitl->drift_speed);
-
 }
 
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
index 802bb16931b56..723d505e4d56b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
@@ -36,5 +36,7 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend
     uint8_t accel_instance[INS_SITL_INSTANCES];
     uint64_t next_gyro_sample[INS_SITL_INSTANCES];
     uint64_t next_accel_sample[INS_SITL_INSTANCES];
+    float gyro_motor_phase[INS_SITL_INSTANCES][12];
+    float accel_motor_phase[INS_SITL_INSTANCES][12];
 };
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_RPM/RPM_SITL.cpp b/libraries/AP_RPM/RPM_SITL.cpp
index 18965094cd824..7800962021e6b 100644
--- a/libraries/AP_RPM/RPM_SITL.cpp
+++ b/libraries/AP_RPM/RPM_SITL.cpp
@@ -36,9 +36,9 @@ void AP_RPM_SITL::update(void)
         return;
     }
     if (instance == 0) {
-        state.rate_rpm = sitl->state.rpm1;
+        state.rate_rpm = sitl->state.rpm[0];
     } else {
-        state.rate_rpm = sitl->state.rpm2;
+        state.rate_rpm = sitl->state.rpm[1];
     }
     state.rate_rpm *= ap_rpm._scaling[state.instance];
     state.signal_quality = 0.5f;
diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 874f2d2ab009a..669f19c17fcac 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -55,6 +55,7 @@ Aircraft::Aircraft(const char *frame_str) :
     rate_hz(1200.0f),
     autotest_dir(nullptr),
     frame(frame_str),
+    num_motors(1),
 #if defined(__CYGWIN__) || defined(__CYGWIN64__)
     min_sleep_time(20000)
 #else
@@ -341,8 +342,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.airspeed = airspeed_pitot;
     fdm.battery_voltage = battery_voltage;
     fdm.battery_current = battery_current;
-    fdm.rpm1 = rpm1;
-    fdm.rpm2 = rpm2;
+    fdm.num_motors = num_motors;
+    memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
     fdm.rcin_chan_count = rcin_chan_count;
     fdm.range = range;
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 80a73719ebda8..768b3aaa79320 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -155,8 +155,8 @@ class Aircraft {
     float airspeed_pitot;                // m/s, apparent airspeed, as seen by fwd pitot tube
     float battery_voltage = -1.0f;
     float battery_current = 0.0f;
-    float rpm1 = 0;
-    float rpm2 = 0;
+    uint8_t num_motors = 1;
+    float rpm[12];
     uint8_t rcin_chan_count = 0;
     float rcin[8];
     float range = -1.0f;                 // rangefinder detection in m
diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.cpp b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
index 9520a90c67342..1a9a376871d23 100644
--- a/libraries/SITL/SIM_EFI_MegaSquirt.cpp
+++ b/libraries/SITL/SIM_EFI_MegaSquirt.cpp
@@ -46,7 +46,7 @@ void EFI_MegaSquirt::update()
     if (!connected) {
         return;
     }
-    float rpm = sitl->state.rpm1;
+    float rpm = sitl->state.rpm[0];
 
     table7.rpm = rpm;
     table7.fuelload = 20;
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index 5881ffaa1b4d1..c16c8c7658a72 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -86,6 +86,7 @@ FlightAxis::FlightAxis(const char *frame_str) :
     Aircraft(frame_str)
 {
     use_time_sync = false;
+    num_motors = 2;
     rate_hz = 250 / target_speedup;
     heli_demix = strstr(frame_str, "helidemix") != nullptr;
     rev4_servos = strstr(frame_str, "rev4") != nullptr;
@@ -473,8 +474,8 @@ void FlightAxis::update(const struct sitl_input &input)
 
     battery_voltage = state.m_batteryVoltage_VOLTS;
     battery_current = state.m_batteryCurrentDraw_AMPS;
-    rpm1 = state.m_heliMainRotorRPM;
-    rpm2 = state.m_propRPM;
+    rpm[0] = state.m_heliMainRotorRPM;
+    rpm[1] = state.m_propRPM;
 
     /*
       the interlink interface supports 8 input channels
diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index d2da87ed677c2..5838c560e2e8a 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -206,13 +206,13 @@ static Frame supported_frames[] =
     Frame("firefly",   6, firefly_motors)
 };
 
-void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
+void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
 {
     /*
        scaling from total motor power to Newtons. Allows the copter
        to hover against gravity when each motor is at hover_throttle
     */
-    thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * hover_throttle);
+    thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * _hover_throttle);
 
     terminal_velocity = _terminal_velocity;
     terminal_rotation_rate = _terminal_rotation_rate;
@@ -228,6 +228,8 @@ Frame *Frame::find_frame(const char *name)
         if (strncasecmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) {
             return &supported_frames[i];
         }
+
+
     }
     return nullptr;
 }
@@ -236,7 +238,8 @@ Frame *Frame::find_frame(const char *name)
 void Frame::calculate_forces(const Aircraft &aircraft,
                              const struct sitl_input &input,
                              Vector3f &rot_accel,
-                             Vector3f &body_accel)
+                             Vector3f &body_accel,
+                             float* rpm)
 {
     Vector3f thrust; // newtons
 
@@ -245,6 +248,10 @@ void Frame::calculate_forces(const Aircraft &aircraft,
         motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
         rot_accel += mraccel;
         thrust += mthrust;
+        // simulate motor rpm
+        if (!is_zero(AP::sitl()->vibe_motor)) {
+            rpm[i] = sqrtf(mthrust.length() / thrust_scale) * AP::sitl()->vibe_motor * 60.0f;
+        }
     }
 
     body_accel = thrust/aircraft.gross_mass();
diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h
index f231df48e385e..96db2e2b9430b 100644
--- a/libraries/SITL/SIM_Frame.h
+++ b/libraries/SITL/SIM_Frame.h
@@ -49,7 +49,7 @@ class Frame {
     // calculate rotational and linear accelerations
     void calculate_forces(const Aircraft &aircraft,
                           const struct sitl_input &input,
-                          Vector3f &rot_accel, Vector3f &body_accel);
+                          Vector3f &rot_accel, Vector3f &body_accel, float* rpm);
     
     float terminal_velocity;
     float terminal_rotation_rate;
diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp
index d9c27f7e09779..2a11d7cc1fc18 100644
--- a/libraries/SITL/SIM_Helicopter.cpp
+++ b/libraries/SITL/SIM_Helicopter.cpp
@@ -152,7 +152,7 @@ void Helicopter::update(const struct sitl_input &input)
     Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
 
     // simulate rotor speed
-    rpm1 = thrust * 1300;
+    rpm[0] = thrust * 1300;
 
     // scale thrust to newtons
     thrust *= thrust_scale;
diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp
index 94e8eb9274272..6528580a57b9c 100644
--- a/libraries/SITL/SIM_JSBSim.cpp
+++ b/libraries/SITL/SIM_JSBSim.cpp
@@ -62,6 +62,7 @@ JSBSim::JSBSim(const char *frame_str) :
     }
     control_port = 5505 + instance*10;
     fdm_port = 5504 + instance*10;
+    num_motors = 2;
 
     printf("JSBSim backend started: control_port=%u fdm_port=%u\n",
            control_port, fdm_port);
@@ -443,8 +444,8 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
     // update magnetic field
     update_mag_field_bf();
     
-    rpm1 = fdm.rpm[0];
-    rpm2 = fdm.rpm[1];
+    rpm[0] = fdm.rpm[0];
+    rpm[1] = fdm.rpm[1];
     
     time_now_us = fdm.cur_time;
 }
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index e568d9405e5bd..d45085fe495f7 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -42,13 +42,15 @@ MultiCopter::MultiCopter(const char *frame_str) :
         frame->init(gross_mass(), 0.51, 15, 4*radians(360));
     }
     frame_height = 0.1;
+    num_motors = frame->num_motors;
     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
 }
 
 // calculate rotational and linear accelerations
 void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
-    frame->calculate_forces(*this, input, rot_accel, body_accel);
+    frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
+
     add_shove_forces(rot_accel, body_accel);
     add_twist_forces(rot_accel);
 }
@@ -78,3 +80,4 @@ void MultiCopter::update(const struct sitl_input &input)
     // update magnetic field
     update_mag_field_bf();
 }
+
diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp
index 36b532c57a738..494b4cb8f50a1 100644
--- a/libraries/SITL/SIM_Plane.cpp
+++ b/libraries/SITL/SIM_Plane.cpp
@@ -34,6 +34,7 @@ Plane::Plane(const char *frame_str) :
     */
     thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
     frame_height = 0.1f;
+    num_motors = 1;
 
     ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
     
@@ -342,7 +343,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     }
     
     // simulate engine RPM
-    rpm1 = thrust * 7000;
+    rpm[0] = thrust * 7000;
     
     // scale thrust to newtons
     thrust *= thrust_scale;
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index 3c8aee544dcf9..ee91247bb04ad 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -73,6 +73,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
         printf("Failed to find frame '%s'\n", frame_type);
         exit(1);
     }
+    num_motors = 1 + frame->num_motors;
 
     if (strstr(frame_str, "cl84")) {
         // setup retract servos at front
@@ -107,7 +108,7 @@ void QuadPlane::update(const struct sitl_input &input)
     Vector3f quad_rot_accel;
     Vector3f quad_accel_body;
 
-    frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body);
+    frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]);
 
     // estimate voltage and current
     frame->current_and_voltage(input, battery_voltage, battery_current);
diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp
index 6a81989a9c92c..658934c4c0fc5 100644
--- a/libraries/SITL/SIM_Sailboat.cpp
+++ b/libraries/SITL/SIM_Sailboat.cpp
@@ -182,7 +182,7 @@ void Sailboat::update(const struct sitl_input &input)
     const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(AP::ahrs().yaw));
 
     // set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL
-    rpm1 = wind_apparent_speed;
+    rpm[0] = wind_apparent_speed;
     airspeed_pitot = wind_apparent_speed;
 
     // calculate angle-of-attack from wind to mainsail
diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp
index d0f1e57633b92..2901198d2ca4b 100644
--- a/libraries/SITL/SIM_Scrimmage.cpp
+++ b/libraries/SITL/SIM_Scrimmage.cpp
@@ -190,8 +190,8 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
     battery_voltage = 0;
     battery_current = 0;
-    rpm1 = 0;
-    rpm2 = 0;
+    rpm[0] = 0;
+    rpm[1] = 0;
 
     airspeed = pkt.airspeed;
     airspeed_pitot = pkt.airspeed;
diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp
index d4c58c158761a..87cc6be0374ed 100644
--- a/libraries/SITL/SIM_XPlane.cpp
+++ b/libraries/SITL/SIM_XPlane.cpp
@@ -42,6 +42,7 @@ XPlane::XPlane(const char *frame_str) :
     }
 
     heli_frame = (strstr(frame_str, "-heli") != nullptr);
+    num_motors = 2;
 
     socket_in.bind("0.0.0.0", bind_port);
     printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
@@ -258,11 +259,11 @@ bool XPlane::receive_data(void)
         }
 
         case EngineRPM:
-            rpm1 = data[1];
+            rpm[0] = data[1];
             break;
 
         case PropRPM:
-            rpm2 = data[1];
+            rpm[1] = data[1];
             break;
             
         case Joystick2:
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index ff21c9b43c4a7..08c989de06245 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -204,6 +204,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
 
     AP_GROUPINFO("MAG_SCALING",    60, SITL,  mag_scaling, 1),
 
+    // max motor vibration frequency
+    AP_GROUPINFO("VIB_MOT_MAX", 61, SITL,  vibe_motor, 0.0f),
+
     AP_GROUPEND
 
 };
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 5b444769bac1c..a468e8a93e920 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -45,8 +45,8 @@ struct sitl_fdm {
     double airspeed; // m/s
     double battery_voltage; // Volts
     double battery_current; // Amps
-    double rpm1;            // main prop RPM
-    double rpm2;            // secondary RPM
+    uint8_t num_motors;
+    float rpm[12];         // RPM of all motors
     uint8_t rcin_chan_count;
     float  rcin[8];         // RC input 0..1
     double range;           // rangefinder value
@@ -248,6 +248,9 @@ class SITL {
     // vibration frequencies in Hz on each axis
     AP_Vector3f vibe_freq;
 
+    // hover frequency to use as baseline for adding motor noise for the gyros and accels
+    AP_Float vibe_motor;
+
     // gyro and accel fail masks
     AP_Int8 gyro_fail_mask;
     AP_Int8 accel_fail_mask;