Copter: run copter attitude control with separate rate thread

run motors output at rate thread loop rate
allow rate thread to be enabled/disabled at runtime for in-flight impact testing
setup the right PID notch sample rate when using the rate thread the PID notches
 run at a very different sample rate
call update_dynamic_notch_at_specified_rate() in rate thread
log RTDT messages to track rate loop performance
set dt each cycle of the rate loop thread
run rate controller on samples as soon as they are ready
detect overload conditions in both the rate loop and main loop
decimate the rate thread if the CPU appears overloaded
decimate the gyro window inside the IMU
add in gyro drift to attitude rate thread
add fixed-rate thread option
configure rate loop based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
better rate loop thread decimation management
ensure fix rate attitude is enabled on arming
add rate loop timing debug
update backend filters rather than all the backends
provide more options around attitude rates
only log attitude and IMU from main loop
force trigger_groups() and reduce attitude thread priority
migrate fast rate enablement to FSTRATE_ENABLE
remove rate thread logging configuration and choose sensible logging rates
conditionally compile rate thread pieces
allow fast rate decimation to be user throttled
if target rate changes immediately jump to target rate
recover quickly from rate changes
ensure fixed rate always prints the rate on arming and is always up to date
add support for fixed rate attitude that does not change when disarmed
only push to subsystems at main loop rate
add logging and motor timing debug
correctly round gyro decimation rates
set dshot rate when changing attitude rate
fallback to higher dshot rates at lower loop rates
re-factor rate loop rate updates
log rates in systemid mode
reset target modifiers at loop rate
don't compile in support on tradheli
move rate thread into its own compilation unit
add rate loop config abstraction that allows code to be elided on non-copter builds
dynamically enable/disable rate thread correctly
add design comment for the rate thread

Co-authored-by: Andrew Tridgell <andrew@tridgell.net>

ArduCopter/Attitude.cpp

@@ -4,18 +4,21 @@
  *  Attitude Rate controllers and timing
  ****************************************************************/
 
-// update rate controllers and output to roll, pitch and yaw actuators
+/*
-//  called at 400hz by default
+  update rate controller when run from main thread (normal operation)
-void Copter::run_rate_controller()
+*/
+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();
-    motors->set_dt(last_loop_time_s);
-    attitude_control->set_dt(last_loop_time_s);
     pos_control->set_dt(last_loop_time_s);
+    attitude_control->set_dt(last_loop_time_s);
 
-    // run low level rate controllers that only require IMU data
+    if (!using_rate_thread) {
-    attitude_control->rate_controller_run();
+        motors->set_dt(last_loop_time_s);
+        // only run the rate controller if we are not using the rate thread
+        attitude_control->rate_controller_run();
+    }
     // reset sysid and other temporary inputs
     attitude_control->rate_controller_target_reset();
 }

ArduCopter/Copter.cpp

@@ -75,6 +75,7 @@
  */
 
 #include "Copter.h"
+#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
 
 #define FORCE_VERSION_H_INCLUDE
 #include "version.h"
@@ -113,7 +114,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
     FAST_TASK(run_custom_controller),
 #endif
@@ -121,7 +122,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
@@ -259,6 +260,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if HAL_BUTTON_ENABLED
     SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
 #endif
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    SCHED_TASK(update_dynamic_notch_at_specified_rate_main,                       LOOP_RATE, 200, 215),
+#endif
 };
 
 void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -618,12 +622,15 @@ void Copter::update_batt_compass(void)
 // should be run at loop rate
 void Copter::loop_rate_logging()
 {
-    if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
+   if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();
-        Log_Write_PIDS(); // only logs if PIDS bitmask is set
+        if (!using_rate_thread) {
+            Log_Write_Rate();
+            Log_Write_PIDS(); // only logs if PIDS bitmask is set
+        }
     }
 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
-    if (should_log(MASK_LOG_FTN_FAST)) {
+    if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
         AP::ins().write_notch_log_messages();
     }
 #endif
@@ -641,10 +648,15 @@ void Copter::ten_hz_logging_loop()
     // log attitude controller data if we're not already logging at the higher rate
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();
+        if (!using_rate_thread) {
+            Log_Write_Rate();
+        }
     }
     if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
     // log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set
-        Log_Write_PIDS();
+        if (!using_rate_thread) {
+            Log_Write_PIDS();
+        }
     }
     // log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
     if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
@@ -789,11 +801,24 @@ 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
     custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 #endif
+
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    // see if we should have a separate rate thread
+    if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
+        if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
+                                         "rate",
+                                         1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
+            started_rate_thread = true;
+        }
+    }
+#endif
 }
 
 void Copter::init_simple_bearing()

ArduCopter/Copter.h

@@ -631,6 +631,17 @@ private:
         RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2),  // 4
         REQUIRE_POSITION_FOR_ARMING =   (1<<3),   // 8
     };
+
+    // type of fast rate attitude controller in operation
+    enum class FastRateType : uint8_t {
+        FAST_RATE_DISABLED            = 0,
+        FAST_RATE_DYNAMIC             = 1,
+        FAST_RATE_FIXED_ARMED         = 2,
+        FAST_RATE_FIXED               = 3,
+    };
+
+    FastRateType get_fast_rate_type() const { return FastRateType(g2.att_enable.get()); }
+
     // returns true if option is enabled for this vehicle
     bool option_is_enabled(FlightOption option) const {
         return (g2.flight_options & uint32_t(option)) != 0;
@@ -728,7 +739,18 @@ private:
     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 run_rate_controller_main();
+
+    // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    uint8_t calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz);
+    void rate_controller_thread();
+    void rate_controller_filter_update();
+    void rate_controller_log_update();
+    uint8_t rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high);
+    void enable_fast_rate_loop(uint8_t rate_decimation);
+    void disable_fast_rate_loop();
+    void update_dynamic_notch_at_specified_rate_main();
+    // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED
     void run_custom_controller() { custom_control.update(); }
@@ -878,6 +900,7 @@ private:
     // Log.cpp
     void Log_Write_Control_Tuning();
     void Log_Write_Attitude();
+    void Log_Write_Rate();
     void Log_Write_EKF_POS();
     void Log_Write_PIDS();
     void Log_Write_Data(LogDataID id, int32_t value);
@@ -921,7 +944,8 @@ private:
     // motors.cpp
     void arm_motors_check();
     void auto_disarm_check();
-    void motors_output();
+    void motors_output(bool full_push = true);
+    void motors_output_main();
     void lost_vehicle_check();
 
     // navigation.cpp
@@ -1086,6 +1110,9 @@ private:
     Mode *mode_from_mode_num(const Mode::Number mode);
     void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
 
+    bool started_rate_thread;
+    bool using_rate_thread;
+
 public:
     void failsafe_check();      // failsafe.cpp
 };

ArduCopter/Log.cpp

@@ -76,6 +76,10 @@ void Copter::Log_Write_Control_Tuning()
 void Copter::Log_Write_Attitude()
 {
     attitude_control->Write_ANG();
+}
+
+void Copter::Log_Write_Rate()
+{
     attitude_control->Write_Rate(*pos_control);
 }
 

ArduCopter/Parameters.cpp

@@ -1,6 +1,7 @@
 #include "Copter.h"
 
 #include <AP_Gripper/AP_Gripper.h>
+#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
 
 /*
    This program is free software: you can redistribute it and/or modify
@@ -1232,6 +1233,22 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT),
 
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    // @Param: FSTRATE_ENABLE
+    // @DisplayName: Enable the fast Rate thread
+    // @Description: Enable the fast Rate thread
+    // @User: Advanced
+    // @Values: 0:Disabled,1:Enabled-Dynamic,2:Enabled-FixedWhenArmed,3:Enabled-Fixed
+    AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0),
+
+    // @Param: FSTRATE_DIV
+    // @DisplayName: Fast rate thread divisor
+    // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate divided by this value.
+    // @User: Advanced
+    // @Range: 1 10
+    AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1),
+#endif
+
     // ID 62 is reserved for the AP_SUBGROUPEXTENSION
 
     AP_GROUPEND

ArduCopter/Parameters.h

@@ -690,6 +690,9 @@ public:
     AP_Float pldp_range_finder_maximum_m;
     AP_Float pldp_delay_s;
     AP_Float pldp_descent_speed_ms;
+
+    AP_Int8 att_enable;
+    AP_Int8 att_decimation;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/mode_systemid.cpp

@@ -414,6 +414,7 @@ void ModeSystemId::log_data() const
 
     // Full rate logging of attitude, rate and pid loops
     copter.Log_Write_Attitude();
+    copter.Log_Write_Rate();
     copter.Log_Write_PIDS();
 
     if (is_poscontrol_axis_type()) {

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

ArduCopter/motors.cpp

@@ -133,7 +133,7 @@ void Copter::auto_disarm_check()
 }
 
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
-void Copter::motors_output()
+void Copter::motors_output(bool full_push)
 {
 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
     // this is to allow the failsafe module to deliberately crash
@@ -183,7 +183,19 @@ void Copter::motors_output()
     }
 
     // push all channels
-    srv.push();
+    if (full_push) {
+        srv.push();
+    } else {
+        hal.rcout->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

ArduCopter/rate_thread.cpp

@@ -0,0 +1,486 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#include "Copter.h"
+#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+
+#pragma GCC optimize("O2")
+
+/*
+ Attitude Rate controller thread design.
+
+ Rationale: running rate outputs linked to fast gyro outputs achieves two goals:
+
+ 1. High frequency gyro processing allows filters to be applied with high sample rates
+    which is advantageous in removing high frequency noise and associated aliasing
+ 2. High frequency rate control reduces the latency between control and action leading to 
+    better disturbance rejection and faster responses which generally means higher
+    PIDs can be used without introducing control oscillation
+
+ (1) is already mostly achieved through the higher gyro rates that are available via
+ INS_GYRO_RATE. (2) requires running the rate controller at higher rates via a separate thread
+
+
+ Goal: the ideal scenario is to run in a single cycle:
+
+    gyro read->filter->publish->rate control->motor output
+
+ This ensures the minimum latency between gyro sample and motor output. Other functions need 
+ to also run faster than they would normally most notably logging and filter frequencies - most
+ notably the harmonic notch frequency.
+
+ Design assumptions:
+
+ 1. The sample rate of the IMUs is consistent and accurate.
+    This is the most basic underlying assumption. An alternative approach would be to rely on
+    the timing of when samples are received but this proves to not work in practice due to
+    scheduling delays. Thus the dt used by the attitude controller is the delta between IMU
+    measurements, not the delta between processing cycles in the rate thread.
+ 2. Every IMU reading must be processed or consistently sub-sampled.
+    This is an assumption that follows from (1) - so it means that attitude control should
+    process every sample or every other sample or every third sample etc. Note that these are
+    filtered samples - all incoming samples are processed for filtering purposes, it is only
+    for the purposes of rate control that we are sub-sampling.
+ 3. The data that the rate loop requires is timely, consistent and accurate.
+    Rate control essentially requires two components - the target and the actuals. The actuals
+    come from the incoming gyro sample combined with the state of the PIDs. The target comes
+    from attitude controller which is running at a slower rate in the main loop. Since the rate
+    thread can read the attitude target at any time it is important that this is always available
+    consistently and is updated consistently.
+ 4. The data that the rest of the vehicle uses is the same data that the rate thread uses.
+    Put another way any gyro value that the vehicle uses (e.g. in the EKF etc), must have already
+    been processed by the rate thread. Where this becomes important is with sub-sampling - if 
+    rate gyro values are sub-sampled we need to make sure that the vehicle is also only using
+    the sub-sampled values.
+
+ Design:
+
+ 1. Filtered gyro samples are (sub-sampled and) pushed into an ObjectBuffer from the INS backend.
+ 2. The pushed sample is published to the INS front-end so that the rest of the vehicle only
+    sees published values that have been used by the rate controller. When the rate thread is not 
+    in use the filtered samples are effectively sub-sampled at the main loop rate. The EKF is unaffected
+    as it uses delta angles calculated from the raw gyro values. (It might be possible to avoid publishing
+    from the rate thread by only updating _gyro_filtered when a value is pushed).
+ 3. A notification is sent that a sample is available
+ 4. The rate thread is blocked waiting for a sample. When it receives a notification it:
+    4a. Runs the rate controller
+    4b. Pushes the new pwm values. Periodically at the main loop rate all of the SRV_Channels::push()
+        functionality is run as well.
+ 5. The rcout dshot thread is blocked waiting for a new pwm value. When it is signalled by the
+    rate thread it wakes up and runs the dshot motor output logic.
+ 6. Periodically the rate thread:
+    6a. Logs the rate outputs (1Khz)
+    6b. Updates the notch filter centers (Gyro rate/2)
+    6c. Checks the ObjectBuffer length and main loop delay (10Hz)
+        If the ObjectBuffer length has been longer than 2 for the last 5 cycles or the main loop has
+        been slowed down then the rate thread is slowed down by telling the INS to sub-sample. This
+        mechanism is continued until the rate thread is able to keep up with the sub-sample rate.
+        The inverse of this mechanism is run if the rate thread is able to keep up but is running slower
+        than the gyro sample rate.
+    6d. Updates the PID notch centers (1Hz)
+ 7. When the rate rate changes through sub-sampling the following values are updated:
+    7a. The PID notch sample rate
+    7b. The dshot rate is constrained to be never greater than the gyro rate or rate rate
+    7c. The motors dt
+ 8. Independently of the rate thread the attitude control target is updated in the main loop. In order
+    for target values to be consistent all updates are processed using local variables and the final
+    target is only written at the end of the update as a vector. Direct control of the target (e.g. in
+    autotune) is also constrained to be on all axes simultaneously using the new desired value. The
+    target makes use of the current PIDs and the "latest" gyro, it might be possible to use a loop
+    delayed gyro value, but that is currently out-of-scope.
+
+ Performance considerations:
+
+ On an H754 using ICM42688 and gyro sampling at 4KHz and rate thread at 4Khz the main CPU users are:
+
+ ArduCopter    PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=18.6%
+ idle          PRI=  1 sp=0x300217B0 STACK= 296/ 504 LOAD= 4.3%
+ rcout         PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD=10.7%
+ SPI1          PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=17.5%
+ SPI4          PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=18.3%
+ rate          PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=22.4%
+
+ There is a direct correlation between the rate rate and CPU load, so if the rate rate is half the gyro
+ rate (i.e. 2Khz) we observe the following:
+
+ ArduCopter    PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=16.7%
+ idle          PRI=  1 sp=0x300217B0 STACK= 296/ 504 LOAD=21.3%
+ rcout         PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 6.2%
+ SPI1          PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=16.7%
+ SPI4          PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=17.8%
+ rate          PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=11.5%
+
+ So we get almost a halving of CPU load in the rate and rcout threads. This is the main way that CPU
+ load can be handled on lower-performance boards, with the other mechanism being lowering the gyro rate.
+ So at a very respectable gyro rate and rate rate both of 2Khz (still 5x standard main loop rate) we see:
+
+ ArduCopter    PRI=182 sp=0x30000600 STACK=4440/7168 LOAD=15.6%
+ idle          PRI=  1 sp=0x300217B0 STACK= 296/ 504 LOAD=39.4%
+ rcout         PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 5.9%
+ SPI1          PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD= 8.9%
+ SPI4          PRI=181 sp=0x3002D4A0 STACK= 896/1464 LOAD= 9.1%
+ rate          PRI=182 sp=0x30029FB0 STACK=1296/1976 LOAD=11.8%
+
+ This essentially means that its possible to run this scheme successfully on all MCUs by careful setting of 
+ the maximum rates.
+
+ Enabling rate thread timing debug for 4Khz reads with fast logging and armed we get the following data:
+
+ Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
+ Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
+ Rate loop timing: gyro=177us, rate=13us, motors=46us, log=7us, ctrl=1us
+
+ The log output is an average since it only runs at 1Khz, so roughly 28us elapsed. So the majority of the time
+ is spent waiting for a gyro sample (higher is better here since it represents the idle time) updating the PIDs
+ and outputting to the motors. Everything else is relatively cheap. Since the total cycle time is 250us the duty
+ cycle is thus 29%
+ */
+
+#define DIV_ROUND_INT(x, d) ((x + d/2) / d)
+
+uint8_t Copter::calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz)
+{
+    return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz));
+}
+
+//#define RATE_LOOP_TIMING_DEBUG
+/*
+  thread for rate control
+*/
+void Copter::rate_controller_thread()
+{
+    uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400));
+    uint8_t rate_decimation = target_rate_decimation;
+    uint32_t rate_loop_count = 0;
+    uint32_t prev_loop_count = 0;
+
+    uint32_t last_run_us = AP_HAL::micros();
+    float max_dt = 0.0;
+    float min_dt = 1.0;
+    uint32_t now_ms = AP_HAL::millis();
+    uint32_t last_rate_check_ms = 0;
+    uint32_t last_rate_increase_ms = 0;
+#if HAL_LOGGING_ENABLED
+    uint32_t last_rtdt_log_ms = now_ms;
+#endif
+    uint32_t last_notch_sample_ms = now_ms;
+    bool was_using_rate_thread = false;
+    bool notify_fixed_rate_active = true;
+    bool was_armed = false;
+    uint32_t running_slow = 0;
+#ifdef RATE_LOOP_TIMING_DEBUG
+    uint32_t gyro_sample_time_us = 0;
+    uint32_t rate_controller_time_us = 0;
+    uint32_t motor_output_us = 0;
+    uint32_t log_output_us = 0;
+    uint32_t ctrl_output_us = 0;
+    uint32_t timing_count = 0;
+    uint32_t last_timing_msg_us = 0;
+#endif
+
+    // run the filters at half the gyro rate
+    uint8_t filter_rate_decimate = 2;
+    uint8_t log_fast_rate_decimate = calc_gyro_decimation(rate_decimation, 1000);   // 1Khz
+#if HAL_LOGGING_ENABLED
+    uint8_t log_med_rate_decimate = calc_gyro_decimation(rate_decimation, 10);      // 10Hz
+    uint8_t log_loop_count = 0;
+#endif
+    uint8_t main_loop_rate_decimate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
+    uint8_t main_loop_count = 0;
+    uint8_t filter_loop_count = 0;
+
+    while (true) {
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        uint32_t rate_now_us = AP_HAL::micros();
+#endif
+
+        // allow changing option at runtime
+        if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) {
+            if (was_using_rate_thread) {
+                disable_fast_rate_loop();
+                was_using_rate_thread = false;
+            }
+            hal.scheduler->delay_microseconds(500);
+            last_run_us = AP_HAL::micros();
+            continue;
+        }
+
+        // set up rate thread requirements
+        if (!using_rate_thread) {
+            enable_fast_rate_loop(rate_decimation);
+        }
+        ins.set_rate_decimation(rate_decimation);
+
+        // wait for an IMU sample
+        Vector3f gyro;
+        if (!ins.get_next_gyro_sample(gyro)) {
+            continue;   // go around again
+        }
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        gyro_sample_time_us += AP_HAL::micros() - rate_now_us;
+        rate_now_us = AP_HAL::micros();
+#endif
+
+        // we must use multiples of the actual sensor rate
+        const float sensor_dt = 1.0f * rate_decimation / ins.get_raw_gyro_rate_hz();
+        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;
+
+        max_dt = MAX(dt, max_dt);
+        min_dt = MIN(dt, min_dt);
+
+#if HAL_LOGGING_ENABLED
+// @LoggerMessage: RTDT
+// @Description: Attitude controller time deltas
+// @Field: TimeUS: Time since system startup
+// @Field: dt: current time delta
+// @Field: dtAvg: current time delta average
+// @Field: dtMax: Max time delta since last log output
+// @Field: dtMin: Min time delta since last log output
+
+        if (now_ms - last_rtdt_log_ms >= 10) {    // 100 Hz
+            AP::logger().WriteStreaming("RTDT", "TimeUS,dt,dtAvg,dtMax,dtMin", "Qffff",
+                                                AP_HAL::micros64(),
+                                                dt, sensor_dt, max_dt, min_dt);
+            max_dt = sensor_dt;
+            min_dt = sensor_dt;
+            last_rtdt_log_ms = now_ms;
+        }
+#endif
+
+        motors->set_dt(sensor_dt);
+        // check if we are falling behind
+        if (ins.get_num_gyro_samples() > 2) {
+            running_slow++;
+        } else if (running_slow > 0) {
+            running_slow--;
+        }
+        if (AP::scheduler().get_extra_loop_us() == 0) {
+            rate_loop_count++;
+        }
+
+        // run the rate controller on all available samples
+        // it is important not to drop samples otherwise the filtering will be fubar
+        // there is no need to output to the motors more than once for every batch of samples
+        attitude_control->rate_controller_run_dt(sensor_dt, gyro + ahrs.get_gyro_drift());
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        rate_controller_time_us += AP_HAL::micros() - rate_now_us;
+        rate_now_us = AP_HAL::micros();
+#endif
+
+        // immediately output the new motor values
+        if (main_loop_count++ >= main_loop_rate_decimate) {
+            main_loop_count = 0;
+        }
+        motors_output(main_loop_count == 0);
+
+        // process filter updates
+        if (filter_loop_count++ >= filter_rate_decimate) {
+            filter_loop_count = 0;
+            rate_controller_filter_update();
+        }
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        motor_output_us += AP_HAL::micros() - rate_now_us;
+        rate_now_us = AP_HAL::micros();
+#endif
+
+#if HAL_LOGGING_ENABLED
+        // fast logging output
+        if (should_log(MASK_LOG_ATTITUDE_FAST)) {
+            if (log_fast_rate_decimate > 0 && log_loop_count++ >= log_fast_rate_decimate) {
+                log_loop_count = 0;
+                rate_controller_log_update();
+            }
+        } else if (should_log(MASK_LOG_ATTITUDE_MED)) {
+            if (log_med_rate_decimate > 0 && log_loop_count++ >= log_med_rate_decimate) {
+                log_loop_count = 0;
+                rate_controller_log_update();
+            }
+        }
+#else
+        (void)log_fast_rate_decimate;
+#endif
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        log_output_us += AP_HAL::micros() - rate_now_us;
+        rate_now_us = AP_HAL::micros();
+#endif
+
+        now_ms = AP_HAL::millis();
+
+        // make sure we have the latest target rate
+        target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400));
+        if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
+            // update the PID notch sample rate at 1Hz if we are
+            // enabled at runtime
+            last_notch_sample_ms = now_ms;
+            attitude_control->set_notch_sample_rate(1.0 / sensor_dt);
+        }
+
+        // interlock for printing fixed rate active
+        if (was_armed != motors->armed()) {
+            notify_fixed_rate_active = !was_armed;
+            was_armed = motors->armed();
+        }
+
+        // Once armed, switch to the fast rate if configured to do so
+        if ((rate_decimation != target_rate_decimation || notify_fixed_rate_active)
+            && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed())
+                || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) {
+            rate_decimation = target_rate_decimation;
+            log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false);
+            notify_fixed_rate_active = false;
+        }
+
+        // check that the CPU is not pegged, if it is drop the attitude rate
+        if (now_ms - last_rate_check_ms >= 100
+            && (get_fast_rate_type() == FastRateType::FAST_RATE_DYNAMIC
+                || (get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && !motors->armed())
+                || target_rate_decimation > rate_decimation)) {
+            last_rate_check_ms = now_ms;
+            const uint32_t att_rate = ins.get_raw_gyro_rate_hz()/rate_decimation;
+            if (running_slow > 5 || AP::scheduler().get_extra_loop_us() > 0
+#if HAL_LOGGING_ENABLED
+                || AP::logger().in_log_download()
+#endif
+                || target_rate_decimation > rate_decimation) {
+                const uint8_t new_rate_decimation = MAX(rate_decimation + 1, target_rate_decimation);
+                const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation;
+                if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) {
+                    rate_decimation = new_rate_decimation;
+                    log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, true);
+                    prev_loop_count = rate_loop_count;
+                    rate_loop_count = 0;
+                    running_slow = 0;
+                }
+            } else if (rate_decimation > target_rate_decimation && rate_loop_count > att_rate/10 // ensure 100ms worth of good readings
+                && (prev_loop_count > att_rate/10   // ensure there was 100ms worth of good readings at the higher rate
+                    || prev_loop_count == 0         // last rate was actually a lower rate so keep going quickly
+                    || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry
+                rate_decimation = rate_decimation - 1;
+
+                log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false);
+                prev_loop_count = 0;
+                rate_loop_count = 0;
+                last_rate_increase_ms = now_ms;
+            }
+        }
+
+#ifdef RATE_LOOP_TIMING_DEBUG
+        timing_count++;
+        ctrl_output_us += AP_HAL::micros() - rate_now_us;
+        rate_now_us = AP_HAL::micros();
+
+        if (rate_now_us - last_timing_msg_us > 1e6) {
+            hal.console->printf("Rate loop timing: gyro=%uus, rate=%uus, motors=%uus, log=%uus, ctrl=%uus\n",
+                                unsigned(gyro_sample_time_us/timing_count), unsigned(rate_controller_time_us/timing_count),
+                                unsigned(motor_output_us/timing_count), unsigned(log_output_us/timing_count), unsigned(ctrl_output_us/timing_count));
+            last_timing_msg_us = rate_now_us;
+            timing_count = 0;
+            gyro_sample_time_us = rate_controller_time_us = motor_output_us = log_output_us = ctrl_output_us = 0;
+        }
+#endif
+
+        was_using_rate_thread = true;
+    }
+}
+
+/*
+  update rate controller filters. on an H7 this is about 30us
+*/
+void Copter::rate_controller_filter_update()
+{
+    // update the frontend center frequencies of notch filters
+    for (auto &notch : ins.harmonic_notches) {
+        update_dynamic_notch(notch);
+    }
+
+    // this copies backend data to the frontend and updates the notches
+    ins.update_backend_filters();
+}
+
+/*
+  update rate controller rates and return the logging rate
+*/
+uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high)
+{
+    const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation;
+    attitude_control->set_notch_sample_rate(attitude_rate);
+    hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), attitude_rate);
+    motors->set_dt(1.0f / attitude_rate);
+    gcs().send_text(warn_cpu_high ? MAV_SEVERITY_WARNING : MAV_SEVERITY_INFO,
+                    "Rate CPU %s, rate set to %uHz",
+                    warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate);
+#if HAL_LOGGING_ENABLED
+    if (attitude_rate > 1000) {
+        return calc_gyro_decimation(rate_decimation, 1000);
+    } else {
+        return calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
+    }
+#endif
+    return 0;
+}
+
+void Copter::enable_fast_rate_loop(uint8_t rate_decimation)
+{
+    ins.enable_fast_rate_buffer();
+    rate_controller_set_rates(rate_decimation, false);
+    hal.rcout->force_trigger_groups(true);
+    using_rate_thread = true;
+}
+
+void Copter::disable_fast_rate_loop()
+{
+    using_rate_thread = false;
+    rate_controller_set_rates(calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()), false);
+    hal.rcout->force_trigger_groups(false);
+    ins.disable_fast_rate_buffer();
+}
+
+/*
+  log only those items that are updated at the rate loop rate
+ */
+void Copter::rate_controller_log_update()
+{
+#if HAL_LOGGING_ENABLED
+    if (!copter.flightmode->logs_attitude()) {
+        Log_Write_Rate();
+        Log_Write_PIDS(); // only logs if PIDS bitmask is set
+    }
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
+    if (should_log(MASK_LOG_FTN_FAST)) {
+        AP::ins().write_notch_log_messages();
+    }
+#endif
+#endif
+}
+
+// run notch update at either loop rate or 200Hz
+void Copter::update_dynamic_notch_at_specified_rate_main()
+{
+    if (using_rate_thread) {
+        return;
+    }
+
+    update_dynamic_notch_at_specified_rate();
+}
+
+#endif // AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED