Copter: hacks for copter with separate rate thread

ArduCopter/Attitude.cpp

@@ -4,18 +4,66 @@
  *  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()
+{
+    using_rate_thread = true;
+
+    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 last_report_ms = AP_HAL::millis();
+
+    while (true) {
+        // wait for an IMU sample
+        rate_sem.wait_blocking();
+        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;
+
+        // run the rate controller
+        attitude_control->rate_controller_run_dt(dt);
+
+#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(200);
+#endif
+
+        if (is_zero(dt_avg)) {
+            dt_avg = dt;
+        } else {
+            dt_avg = 0.99f * dt_avg + 0.01f * dt;
+        }
+
+        const uint32_t now_ms = AP_HAL::millis();
+        if (now_ms - last_report_ms >= 2000) {
+            last_report_ms = now_ms;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Rate: %.2fHz", 1.0/dt_avg);
+        }
+    }
+}
+
+/*
+  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,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

ArduCopter/Copter.h

@@ -645,6 +645,7 @@ 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
     };
 
     static constexpr int8_t _failsafe_priorities[] = {
@@ -728,7 +729,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(); }
@@ -1057,6 +1059,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 using_rate_thread;
+
 public:
     void failsafe_check();      // failsafe.cpp
 };

ArduCopter/Parameters.cpp

@@ -1014,7 +1014,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/system.cpp

@@ -204,6 +204,15 @@ void Copter::init_ardupilot()
     vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz);
     hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz);
 
+    // see if we should have a separate rate thread
+    if ((copter.g2.flight_options & uint32_t(FlightOptions::USE_RATE_LOOP_THREAD)) != 0) {
+        if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
+                                         "rate_controller",
+                                         2048, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
+            using_rate_thread = true;
+        }
+    }
+
     // flag that initialisation has completed
     ap.initialised = true;
 }