Copter: add parameters to control filter update rate and logging rate

ArduCopter/Attitude.cpp

@@ -14,7 +14,6 @@
 void Copter::rate_controller_thread()
 {
     uint8_t rate_decimation = 1;
-    uint32_t slow_loop_count = 0;
     uint32_t rate_loop_count = 0;
     uint32_t prev_loop_count = 0;
 
@@ -41,6 +40,11 @@ void Copter::rate_controller_thread()
     uint32_t last_timing_msg_us = 0;
 #endif
 
+    uint8_t filter_rate_decimate = uint8_t((ins.get_raw_gyro_rate_hz() + copter.g2.att_filter_rate_hz - 1) / copter.g2.att_filter_rate_hz);
+    uint8_t log_rate_decimate = uint8_t((ins.get_raw_gyro_rate_hz() + copter.g2.att_log_rate_hz - 1) / copter.g2.att_log_rate_hz);
+    uint8_t filter_loop_count = 0;
+    uint8_t log_loop_count = 0;
+
     while (true) {
 
 #ifdef RATE_LOOP_TIMING_DEBUG
@@ -129,17 +133,21 @@ void Copter::rate_controller_thread()
         rate_now_us = AP_HAL::micros();
 #endif
 
-        /*
-          immediately output the new motor values
-         */
+        // immediately output the new motor values
         motors_output();
 
-        /*
-          process slow loop update
-         */
-        if (slow_loop_count++ >= (uint8_t)copter.g2.rate_update_decimation.get()) {
-            slow_loop_count = 0;
-            rate_controller_slow_loop();
+        // process filter updates
+        if (filter_loop_count++ >= filter_rate_decimate) {
+            filter_loop_count = 0;
+            rate_controller_filter_update();
+        }
+
+        // fast logging output
+        if (log_loop_count++ >= log_rate_decimate) {
+            log_loop_count = 0;
+#if HAL_LOGGING_ENABLED
+            fast_logging();
+#endif
         }
 
 #ifdef RATE_LOOP_TIMING_DEBUG
@@ -150,7 +158,7 @@ void Copter::rate_controller_thread()
         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
+            // 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);
@@ -165,8 +173,7 @@ void Copter::rate_controller_thread()
 
         // check that the CPU is not pegged, if it is drop the attitude rate
         if (now_ms - last_rate_check_ms >= 200
-            && (!flight_option_is_set(FlightOptions::USE_FIXED_RATE_LOOP_THREAD)
-                || !motors->armed() || ap.land_complete)) {
+            && (!flight_option_is_set(FlightOptions::USE_FIXED_RATE_LOOP_THREAD) || !motors->armed())) {
             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
@@ -217,20 +224,19 @@ void Copter::rate_controller_thread()
 }
 
 /*
-  update rate controller slow loop. on an H7 this is about 30us
+  update rate controller filters. on an H7 this is about 30us
 */
-void Copter::rate_controller_slow_loop()
+void Copter::rate_controller_filter_update()
 {
     // update the frontend center frequencies of notch filters
-    update_dynamic_notch_at_specified_rate();
+    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();
-
-#if HAL_LOGGING_ENABLED
-    fast_logging();
-#endif
 }
+
 #endif
 
 /*

ArduCopter/Copter.h

@@ -723,7 +723,7 @@ class Copter : public AP_Vehicle {
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
     void rate_controller_thread();
-    void rate_controller_slow_loop();
+    void rate_controller_filter_update();
     void run_rate_controller_main();
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED

ArduCopter/Parameters.cpp

@@ -1229,13 +1229,23 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT),
 
 #if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED
-    // @Param: RATE_UPDATE
-    // @DisplayName: Rate thread update rate
-    // @Description: The decimation rate of rate loop thread updates. Updates include rate logging and notch frequency updates. A value of 1 means that these updates happen at the same rate as the gyro reads. A value of N means that updates occur at the gyro read rate / N.
-    // @Range: 1 8
+    // @Param: ATT_FILTER_RATE
+    // @DisplayName: Rate thread notch update rate
+    // @Description: The rate of rate thread filter updates in Hz. The actual rate is constrained to be a whole divisor of the gyro rate and thus cannot be larger than the gyro rate.
+    // @Range: 400 8000
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("RATE_UPDATE", 9, ParametersG2, rate_update_decimation, 4),
+    // @RebootRequired: True
+    AP_GROUPINFO("ATT_FILTER_RATE", 9, ParametersG2, att_filter_rate_hz, 1000),
+
+    // @Param: ATT_LOG_RATE
+    // @DisplayName: Rate thread fast logging rate
+    // @Description: The rate of rate thread fast logging in Hz. The actual rate is constrained to be a whole divisor of the gyro rate and thus cannot be larger than the gyro rate.
+    // @Range: 400 8000
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("ATT_LOG_RATE", 10, ParametersG2, att_log_rate_hz, 1000),
 #endif
 
     // ID 62 is reserved for the AP_SUBGROUPEXTENSION

ArduCopter/Parameters.h

@@ -687,7 +687,8 @@ class ParametersG2 {
     AP_Float pldp_descent_speed_ms;
 
 #if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED
-    AP_Int8 rate_update_decimation;
+    AP_Int16 att_filter_rate_hz;
+    AP_Int16 att_log_rate_hz;
 #endif
 };