From f0efd095163a459ff7cd28b8c3cbbb2549eb5832 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 5 Jun 2024 20:52:59 +0100 Subject: [PATCH] Copter: add parameters to control filter update rate and logging rate --- ArduCopter/Attitude.cpp | 46 ++++++++++++++++++++++----------------- ArduCopter/Copter.h | 2 +- ArduCopter/Parameters.cpp | 20 ++++++++++++----- ArduCopter/Parameters.h | 3 ++- 4 files changed, 44 insertions(+), 27 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index a51eee5bd9a8be..941c80782c05c8 100644 --- a/ArduCopter/Attitude.cpp +++ b/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 ¬ch : 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 /* diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b62d5ea6378c94..4993441fa80fcd 100644 --- a/ArduCopter/Copter.h +++ b/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 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ea6316980622eb..ef5d1d05934a99 100644 --- a/ArduCopter/Parameters.cpp +++ b/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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c47a60e32be316..86a6415f10e50a 100644 --- a/ArduCopter/Parameters.h +++ b/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 };