Copter: run rate loop at full filter rate in its own thread

ArduCopter/Attitude.cpp

@@ -4,18 +4,113 @@
  *  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();
     motors->set_dt(last_loop_time_s);
     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();
+    }
 }
 
 /*************************************************************

ArduCopter/Copter.cpp

@@ -113,15 +113,15 @@ 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
 #if FRAME_CONFIG == HELI_FRAME
     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()

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
 };

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),
 

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;
     }
 

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

@@ -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()
 {

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);
 

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;

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()
 {

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);

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

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