diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index 67646caed9c0b..97cce1313437b 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -4,9 +4,102 @@
  *  Attitude Rate controllers and timing
  ****************************************************************/
 
-// update rate controllers and output to roll, pitch and yaw actuators
-//  called at 400hz by default
-void Copter::run_rate_controller()
+/*
+  thread for rate control
+*/
+void Copter::rate_controller_thread()
+{
+    HAL_BinarySemaphore rate_sem;
+    ins.set_rate_loop_sem(&rate_sem);
+
+    uint32_t last_run_us = AP_HAL::micros();
+    float dt_avg = 0.0;
+    uint32_t now_ms = AP_HAL::millis();
+    uint32_t last_report_ms = now_ms;
+    uint32_t last_notch_sample_ms = now_ms;
+    bool was_using_rate_thread = false;
+
+    while (true) {
+        // allow changing option at runtime
+        if (!flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD) ||
+            ap.motor_test) {
+            using_rate_thread = false;
+            if (was_using_rate_thread) {
+                // if we were using the rate thread, we need to
+                // setup the notch filter sample rate
+                attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+                was_using_rate_thread = false;
+            }
+            hal.scheduler->delay_microseconds(500);
+            last_run_us = AP_HAL::micros();
+            continue;
+        }
+
+        using_rate_thread = true;
+
+        // wait for an IMU sample
+        rate_sem.wait_blocking();
+        if (ap.motor_test) {
+            continue;
+        }
+
+        const uint32_t now_us = AP_HAL::micros();
+        const uint32_t dt_us = now_us - last_run_us;
+        const float dt = dt_us * 1.0e-6;
+        last_run_us = now_us;
+
+        if (is_zero(dt_avg)) {
+            dt_avg = dt;
+        } else {
+            dt_avg = 0.99f * dt_avg + 0.01f * dt;
+        }
+
+        /*
+          run the rate controller. We pass in the long term average dt
+          not the per loop dt, as most of the timing jitter is from
+          the timing of the FIFO reads on the SPI bus, which does not
+          reflect the actual time between IMU samples, which is steady
+        */
+        attitude_control->rate_controller_run_dt(dt_avg);
+
+        /*
+          immediately output the new motor values
+         */
+        motors_output();
+
+        /*
+          update the center frequencies of notch filters
+         */
+        update_dynamic_notch_at_specified_rate();
+
+#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
+        // ensure we give at least some CPU to other threads
+        // don't sleep on SITL where small sleeps are not possible
+        hal.scheduler->delay_microseconds(100);
+#endif
+
+        now_ms = AP_HAL::millis();
+
+        if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
+            // update the PID notch sample rate at 1Hz if if we are
+            // enabled at runtime
+            last_notch_sample_ms = now_ms;
+            attitude_control->set_notch_sample_rate(1.0 / dt_avg);
+        }
+        
+        if (now_ms - last_report_ms >= 200) {
+            last_report_ms = now_ms;
+            gcs().send_named_float("LRATE", 1.0/dt_avg);
+        }
+
+        was_using_rate_thread = true;
+    }
+}
+
+/*
+  update rate controller when run from main thread (normal operation)
+*/
+void Copter::run_rate_controller_main()
 {
     // set attitude and position controller loop time
     const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
@@ -14,8 +107,10 @@ void Copter::run_rate_controller()
     attitude_control->set_dt(last_loop_time_s);
     pos_control->set_dt(last_loop_time_s);
 
-    // run low level rate controllers that only require IMU data
-    attitude_control->rate_controller_run(); 
+    if (!using_rate_thread) {
+        // only run the rate controller if we are not using the rate thread
+        attitude_control->rate_controller_run();
+    }
 }
 
 /*************************************************************
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index dda496b3e0921..03cf8355f2152 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -113,7 +113,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     // update INS immediately to get current gyro data populated
     FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
     // run low level rate controllers that only require IMU data
-    FAST_TASK(run_rate_controller),
+    FAST_TASK(run_rate_controller_main),
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     FAST_TASK(run_custom_controller),
 #endif
@@ -121,7 +121,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     FAST_TASK(heli_update_autorotation),
 #endif //HELI_FRAME
     // send outputs to the motors library immediately
-    FAST_TASK(motors_output),
+    FAST_TASK(motors_output_main),
      // run EKF state estimator (expensive)
     FAST_TASK(read_AHRS),
 #if FRAME_CONFIG == HELI_FRAME
@@ -674,11 +674,22 @@ void Copter::one_hz_loop()
     AP_Notify::flags.flying = !ap.land_complete;
 
     // slowly update the PID notches with the average loop rate
-    attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+    if (!using_rate_thread) {
+        attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+    }
     pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 #endif
+
+    // see if we should have a separate rate thread
+    if (!started_rate_thread && flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD)) {
+        if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
+                                         "rate_controller",
+                                         2048, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
+            started_rate_thread = true;
+        }
+    }
 }
 
 void Copter::init_simple_bearing()
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 3c922cb14073f..c542014859ca2 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -641,8 +641,14 @@ class Copter : public AP_Vehicle {
         DISABLE_THRUST_LOSS_CHECK     = (1<<0),   // 1
         DISABLE_YAW_IMBALANCE_WARNING = (1<<1),   // 2
         RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2),  // 4
+        USE_RATE_LOOP_THREAD           = (1<<3),  // 8
     };
 
+    // return true if a flight option is set
+    bool flight_option_is_set(FlightOptions option) const {
+        return (g2.flight_options & uint32_t(FlightOptions::USE_RATE_LOOP_THREAD)) != 0;
+    }
+
     static constexpr int8_t _failsafe_priorities[] = {
                                                       (int8_t)FailsafeAction::TERMINATE,
                                                       (int8_t)FailsafeAction::LAND,
@@ -724,7 +730,8 @@ class Copter : public AP_Vehicle {
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
-    void run_rate_controller();
+    void rate_controller_thread();
+    void run_rate_controller_main();
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     void run_custom_controller() { custom_control.update(); }
@@ -899,6 +906,7 @@ class Copter : public AP_Vehicle {
     void arm_motors_check();
     void auto_disarm_check();
     void motors_output();
+    void motors_output_main();
     void lost_vehicle_check();
 
     // navigation.cpp
@@ -1059,6 +1067,8 @@ class Copter : public AP_Vehicle {
     Mode *mode_from_mode_num(const Mode::Number mode);
     void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
 
+    bool started_rate_thread;
+
 public:
     void failsafe_check();      // failsafe.cpp
 };
diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index 35ede3b0516cb..657a08ae8041f 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -1008,7 +1008,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: FLIGHT_OPTIONS
     // @DisplayName: Flight mode options
     // @Description: Flight mode specific options
-    // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss
+    // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss, 3:Use a separate rate loop thread
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0),
 
diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp
index 2ce5d89420cc8..84a27d04e5025 100644
--- a/ArduCopter/crash_check.cpp
+++ b/ArduCopter/crash_check.cpp
@@ -102,7 +102,7 @@ void Copter::thrust_loss_check()
     static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed
 
     // no-op if suppressed by flight options param
-    if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) {
+    if (flight_option_is_set(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) {
         return;
     }
 
@@ -171,7 +171,7 @@ void Copter::thrust_loss_check()
         // the motors library disables this when it is no longer needed to achieve the commanded output
 
 #if AP_GRIPPER_ENABLED
-        if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) {
+        if (flight_option_is_set(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) {
             copter.g2.gripper.release();
         }
 #endif
@@ -182,7 +182,7 @@ void Copter::thrust_loss_check()
 void Copter::yaw_imbalance_check()
 {
     // no-op if suppressed by flight options param
-    if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) {
+    if (flight_option_is_set(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) {
         return;
     }
 
diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp
index 5c5deae8995cd..b85103be2b63c 100644
--- a/ArduCopter/motor_test.cpp
+++ b/ArduCopter/motor_test.cpp
@@ -158,6 +158,12 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
             ap.motor_test = true;
 
             EXPECT_DELAY_MS(3000);
+
+            // wait for rate thread to stop running due to motor test
+            while (using_rate_thread) {
+                hal.scheduler->delay(1);
+            }
+
             // enable and arm motors
             if (!motors->armed()) {
                 motors->output_min();  // output lowest possible value to motors
diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp
index 34a3ff201b6ce..08595ba6528e2 100644
--- a/ArduCopter/motors.cpp
+++ b/ArduCopter/motors.cpp
@@ -183,6 +183,14 @@ void Copter::motors_output()
     SRV_Channels::push();
 }
 
+// motors_output from main thread
+void Copter::motors_output_main()
+{
+    if (!using_rate_thread) {
+        motors_output();
+    }
+}
+
 // check for pilot stick input to trigger lost vehicle alarm
 void Copter::lost_vehicle_check()
 {
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
index f75230980414d..ba03d380365b8 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -199,6 +199,9 @@ class AC_AttitudeControl {
     // Run angular velocity controller and send outputs to the motors
     virtual void rate_controller_run() = 0;
 
+    // optional variant to allow running with different dt
+    virtual void rate_controller_run_dt(float dt) { rate_controller_run(); }
+
     // Convert a 321-intrinsic euler angle derivative to an angular velocity vector
     void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);
 
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
index 0d519bf87d8f2..5da7bd02af71c 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
@@ -51,7 +51,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 
 	// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
 	// should be called at 100hz or more
-	virtual void rate_controller_run() override;
+    virtual void rate_controller_run() override;
 
     // Update Alt_Hold angle maximum
     void update_althold_lean_angle_max(float throttle_in) override;
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
index 947975a0a1686..6a936ad156feb 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
@@ -412,15 +412,15 @@ void AC_AttitudeControl_Multi::update_throttle_gain_boost()
 }
 
 // update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
-void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
+void AC_AttitudeControl_Multi::update_throttle_rpy_mix(float dt)
 {
     // slew _throttle_rpy_mix to _throttle_rpy_mix_desired
     if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
         // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
-        _throttle_rpy_mix += MIN(2.0f * _dt, _throttle_rpy_mix_desired - _throttle_rpy_mix);
+        _throttle_rpy_mix += MIN(2.0f * dt, _throttle_rpy_mix_desired - _throttle_rpy_mix);
     } else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
         // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
-        _throttle_rpy_mix -= MIN(0.5f * _dt, _throttle_rpy_mix - _throttle_rpy_mix_desired);
+        _throttle_rpy_mix -= MIN(0.5f * dt, _throttle_rpy_mix - _throttle_rpy_mix_desired);
 
         // if the mix is still higher than that being used, reset immediately
         const float throttle_hover = _motors.get_throttle_hover();
@@ -439,25 +439,25 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
     _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
 }
 
-void AC_AttitudeControl_Multi::rate_controller_run()
+void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt)
 {
     // boost angle_p/pd each cycle on high throttle slew
     update_throttle_gain_boost();
 
     // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
-    update_throttle_rpy_mix();
+    update_throttle_rpy_mix(dt);
 
     _ang_vel_body += _sysid_ang_vel_body;
 
     Vector3f gyro_latest = _ahrs.get_gyro_latest();
 
-    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x,  _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
+    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x,  dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
     _motors.set_roll_ff(get_rate_roll_pid().get_ff());
 
-    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y,  _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
+    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y,  dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
     _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
 
-    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z,  _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
+    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z,  dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
     _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
 
     _sysid_ang_vel_body.zero();
@@ -469,6 +469,11 @@ void AC_AttitudeControl_Multi::rate_controller_run()
     control_monitor_update();
 }
 
+void AC_AttitudeControl_Multi::rate_controller_run()
+{
+    rate_controller_run_dt(_dt);
+}
+
 // sanity check parameters.  should be called once before takeoff
 void AC_AttitudeControl_Multi::parameter_sanity_check()
 {
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
index c9966cbefd91a..a9692acf6c187 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
@@ -76,6 +76,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl {
     bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); }
 
     // run lowest level body-frame rate controller and send outputs to the motors
+    void rate_controller_run_dt(float dt) override;
     void rate_controller_run() override;
 
     // sanity check parameters.  should be called once before take-off
@@ -93,7 +94,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl {
     void update_throttle_gain_boost();
 
     // update_throttle_rpy_mix - updates thr_low_comp value towards the target
-    void update_throttle_rpy_mix();
+    void update_throttle_rpy_mix(float dt);
 
     // get maximum value throttle can be raised to based on throttle vs attitude prioritisation
     float get_throttle_avg_max(float throttle_in);
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 1817c3e6813eb..501e45ec18fc6 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1391,7 +1391,9 @@ void RCOutput::trigger_groups(void)
     osalSysUnlock();
 #if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED
     // trigger a PWM send
-    if (!in_soft_serial() && hal.scheduler->in_main_thread() && rcout_thread_ctx) {
+    if (!in_soft_serial() &&
+        (hal.scheduler->in_main_thread() ||
+         chThdGetPriorityX() > APM_MAIN_PRIORITY_BOOST) && rcout_thread_ctx) {
         chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND);
     }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index 47052d79d7123..663fef96c4577 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -22,10 +22,10 @@
 
 #define CHIBIOS_SCHEDULER_MAX_TIMER_PROCS 8
 
-#define APM_MONITOR_PRIORITY    183
+#define APM_MONITOR_PRIORITY    184
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      181
-#define APM_RCOUT_PRIORITY      181
+#define APM_RCOUT_PRIORITY      183
 #define APM_LED_PRIORITY         60
 #define APM_UART_PRIORITY        60
 #define APM_NET_PRIORITY         60
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 3df340b673a48..9d82538bd76af 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -796,6 +796,15 @@ class AP_InertialSensor : AP_AccelCal_Client
     bool raw_logging_option_set(RAW_LOGGING_OPTION option) const {
         return (raw_logging_options.get() & int32_t(option)) != 0;
     }
+
+    /*
+      binary semaphore for rate loop to use to start a rate loop when
+      we hav finished filtering the primary IMU
+     */
+    HAL_BinarySemaphore *rate_loop_sem;
+
+public:
+    void set_rate_loop_sem(HAL_BinarySemaphore *sem) { rate_loop_sem = sem; }
 };
 
 namespace AP {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 85c9ed8f34e35..fc9b49ce5ae4e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -212,6 +212,9 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
     save_gyro_window(instance, gyro, filter_phase++);
 
     Vector3f gyro_filtered = gyro;
+#if AP_AHRS_ENABLED
+    const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
+#endif
 
     // apply the harmonic notch filters
     for (auto &notch : _imu.harmonic_notches) {
@@ -224,7 +227,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
         // currently active IMU we reset the inactive notch filters so
         // that if we switch IMUs we're not left with old data
         if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
-            instance != AP::ahrs().get_primary_gyro_index()) {
+            instance != primary_gyro) {
             inactive = true;
         }
 #endif
@@ -253,6 +256,16 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
     } else {
         _imu._gyro_filtered[instance] = gyro_filtered;
     }
+
+#if AP_AHRS_ENABLED
+    if (_imu.rate_loop_sem != nullptr && instance == primary_gyro) {
+        /*
+          tell the rate thread we have a new sample
+        */
+        _imu._gyro[instance] = gyro_filtered;
+        _imu.rate_loop_sem->signal();
+    }
+#endif
 }
 
 void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 04f80777fcf7f..e6ec28412345d 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -569,7 +569,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update,                  400, 50, 205),
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update_parameters,         1, 50, 210),
 #endif
-    SCHED_TASK(update_dynamic_notch_at_specified_rate,      LOOP_RATE,                    200, 215),
+    SCHED_TASK(update_dynamic_notches,                      LOOP_RATE,               200, 215),
 #if AP_VIDEOTX_ENABLED
     SCHED_TASK_CLASS(AP_VideoTX,   &vehicle.vtx,            update,                    2, 100, 220),
 #endif
@@ -857,6 +857,16 @@ void AP_Vehicle::update_dynamic_notch_at_specified_rate()
     }
 }
 
+/*
+  update dynamic notches if not using rate thread
+ */
+void AP_Vehicle::update_dynamic_notches()
+{
+    if (!using_rate_thread) {
+        update_dynamic_notch_at_specified_rate();
+    }
+}
+
 void AP_Vehicle::notify_no_such_mode(uint8_t mode_number)
 {
     GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No such mode %u", mode_number);
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 6476dffea19ec..c2d3424f77315 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -490,8 +490,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     // update the harmonic notch
     void update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch);
 
-    // run notch update at either loop rate or 200Hz
-    void update_dynamic_notch_at_specified_rate();
+    // scheduler task
+    void update_dynamic_notches(void);
 #endif
 
     // decimation for 1Hz update
@@ -521,6 +521,14 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
 
     // Bitmask of modes to disable from gcs
     AP_Int32 flight_mode_GCS_block;
+
+protected:
+    /*
+      run notch update at either loop rate or 200Hz. Called by
+      AP_Vehicle scheduler if using_rate_thread is not true
+    */
+    void update_dynamic_notch_at_specified_rate();
+    bool using_rate_thread;
 };
 
 namespace AP {