From 01d205d78476dfbdf45b65155fa5c2a26c4e76a0 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 01/21] 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 --- ArduCopter/Attitude.cpp | 17 +- ArduCopter/Copter.cpp | 39 ++- ArduCopter/Copter.h | 31 ++- ArduCopter/Log.cpp | 4 + ArduCopter/Parameters.cpp | 17 ++ ArduCopter/Parameters.h | 3 + ArduCopter/mode_systemid.cpp | 1 + ArduCopter/motor_test.cpp | 6 + ArduCopter/motors.cpp | 16 +- ArduCopter/rate_thread.cpp | 486 +++++++++++++++++++++++++++++++++++ 10 files changed, 602 insertions(+), 18 deletions(-) create mode 100644 ArduCopter/rate_thread.cpp diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 3e71782c18c00..02bbee27497fb 100644 --- a/ArduCopter/Attitude.cpp +++ b/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 -void Copter::run_rate_controller() +/* + 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); + attitude_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) { + 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(); } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 84e31f3ebb326..0deee9d500ddd 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -75,6 +75,7 @@ */ #include "Copter.h" +#include #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, @@ -570,12 +574,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 @@ -593,10 +600,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)) { @@ -741,11 +753,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() diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 48b6141c9a84d..ba11e5485734c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -631,6 +631,17 @@ class Copter : public AP_Vehicle { 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; @@ -726,7 +737,18 @@ 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 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(); } @@ -876,6 +898,7 @@ class Copter : public AP_Vehicle { // 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); @@ -919,7 +942,8 @@ class Copter : public AP_Vehicle { // 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 @@ -1080,6 +1104,9 @@ 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; + bool using_rate_thread; + public: void failsafe_check(); // failsafe.cpp }; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index fffea298f10fd..b446904e1ab05 100644 --- a/ArduCopter/Log.cpp +++ b/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); } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index fd5eea3e8626f..37309440fbfd3 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1,6 +1,7 @@ #include "Copter.h" #include +#include /* 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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0cf0a8cb51959..2a85bc9ee9bfa 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -690,6 +690,9 @@ class ParametersG2 { 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[]; diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index db1e0ddc2600a..1462454b78279 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/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()) { diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 2313c30953d5d..b40e5bc4ae407 100644 --- a/ArduCopter/motor_test.cpp +++ b/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 diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index c37a4d55a2fb3..784580b8ecb7d 100644 --- a/ArduCopter/motors.cpp +++ b/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 diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp new file mode 100644 index 0000000000000..29434215aaf6b --- /dev/null +++ b/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 . +*/ +#include "Copter.h" +#include +#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 ¬ch : 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 From 61c47c210c12d554f7efbc2dd68ea08283916b0a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 21:35:30 +0100 Subject: [PATCH 02/21] autotest: add rate thread autotest --- Tools/autotest/arducopter.py | 72 ++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 97792527db53d..07b604b3097d1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6293,6 +6293,77 @@ def DynamicRpmNotches(self): if ex is not None: raise ex + def DynamicRpmNotchesRateThread(self): + """Use dynamic harmonic notch to control motor noise via ESC telemetry.""" + self.progress("Flying with ESC telemetry driven dynamic notches") + self.context_push() + self.set_rc_default() + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour + "LOG_BITMASK": 959, + "LOG_DISARMED": 0, + "SIM_VIB_MOT_MAX": 350, + "SIM_GYR1_RND": 20, + "SIM_ESC_TELEM": 1, + "FSTRATE_ENABLE": 1 + }) + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + + # find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe. + # there is a second harmonic at 380Hz which should be avoided to make the test reliable + # detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB + freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320) + + # now add a dynamic notch and check that the peak is squashed + self.set_parameters({ + "INS_LOG_BAT_OPT": 4, + "INS_HNTCH_ENABLE": 1, + "INS_HNTCH_FREQ": 80, + "INS_HNTCH_REF": 1.0, + "INS_HNTCH_HMNCS": 5, # first and third harmonic + "INS_HNTCH_ATT": 50, + "INS_HNTCH_BW": 40, + "INS_HNTCH_MODE": 3, + "FSTRATE_ENABLE": 1 + }) + self.reboot_sitl() + + # -10dB is pretty conservative - actual is about -25dB + freq, hover_throttle, peakdb1, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb1 = psd["X"][int(esc_hz)] + + # now add notch-per motor and check that the peak is squashed + self.set_parameter("INS_HNTCH_OPTS", 2) + self.reboot_sitl() + + freq, hover_throttle, peakdb2, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb2 = psd["X"][int(esc_hz)] + + # notch-per-motor will be better at the average ESC frequency + if esc_peakdb2 > esc_peakdb1: + raise NotAchievedException( + "Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" % + (esc_peakdb2, esc_peakdb1)) + + # check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily + # testing shows this to be -58dB on average + if esc_peakdb2 > -25: + raise NotAchievedException( + "Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2) + self.context_pop() + self.reboot_sitl() + def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None): '''do a simple up-and-down test flight with current vehicle state. Check that the onboard filter comes up with the same peak-frequency that @@ -12175,6 +12246,7 @@ def tests2b(self): # this block currently around 9.5mins here Test(self.DynamicNotches, attempts=4), self.PositionWhenGPSIsZero, self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug + self.DynamicRpmNotchesRateThread, self.PIDNotches, self.StaticNotches, self.RefindGPS, From 482ba225213719a0e5c40c2ad739cad85d6750ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Feb 2024 12:06:22 +1100 Subject: [PATCH 03/21] AP_Vehicle: use update_dynamic_notch() directly in rate loop --- libraries/AP_Vehicle/AP_Vehicle.cpp | 3 ++- libraries/AP_Vehicle/AP_Vehicle.h | 15 ++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 3005e40d3adcb..8773302c13158 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -3,6 +3,7 @@ #if AP_VEHICLE_ENABLED #include "AP_Vehicle.h" +#include #include #include @@ -621,7 +622,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210), #endif -#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && !AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215), #endif #if AP_VIDEOTX_ENABLED diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 02895d1bc2740..471cd1155e19f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -510,6 +510,13 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // Check if this mode can be entered from the GCS bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const; +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + // update the harmonic notch + void update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch); + // run notch update at either loop rate or 200Hz + void update_dynamic_notch_at_specified_rate(); +#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + private: #if AP_SCHEDULER_ENABLED @@ -524,13 +531,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED // update the harmonic notch for throttle based notch void update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch); - - // update the harmonic notch - void update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch); - - // run notch update at either loop rate or 200Hz - void update_dynamic_notch_at_specified_rate(); -#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED +#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED // decimation for 1Hz update uint8_t one_Hz_counter; From 7bd1afc092b4252b915dd6147808964dfeb9e2eb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 10:30:02 +0100 Subject: [PATCH 04/21] AP_HAL: allow forcing of trigger_groups() --- libraries/AP_HAL/RCOutput.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 68a84b1a10fcd..dee9068138a63 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -376,6 +376,11 @@ class AP_HAL::RCOutput { */ virtual void write_gpio(uint8_t chan, bool active) {}; + /* + Force group trigger from all callers rather than just from the main thread + */ + virtual void force_trigger_groups(bool onoff) {}; + /* * calculate the prescaler required to achieve the desire bitrate */ From cfff230d21570624fc6eb1b0ba74a004414a4330 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 20:00:44 +0100 Subject: [PATCH 05/21] AP_HAL: set HAL_INS_RATE_LOOP in boards restrict rate loop to H7 and F7 --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/board/chibios.h | 6 ++++++ libraries/AP_HAL/board/linux.h | 4 ++++ libraries/AP_HAL/board/sitl.h | 4 ++++ 4 files changed, 18 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 26a47e2c8cd99..b5dddb89cb4f4 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -385,4 +385,8 @@ #define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024 #endif +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 0 +#endif + #define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON) diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index 9e5cd5911f5a1..9c51ac58ea9e0 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -140,3 +140,9 @@ #endif #define HAL_USE_QUADSPI (HAL_USE_QUADSPI1 || HAL_USE_QUADSPI2) #define HAL_USE_OCTOSPI (HAL_USE_OCTOSPI1 || HAL_USE_OCTOSPI2) + +#if defined(STM32H7) || defined(STM32F7) +#define HAL_INS_RATE_LOOP 1 +#else +#define HAL_INS_RATE_LOOP 0 +#endif diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index a7e6c3fc1740f..c55fec67e24ae 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -422,3 +422,7 @@ #else #define HAL_LINUX_USE_VIRTUAL_CAN 0 #endif + +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 1 +#endif diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index 773881489857c..e4d1afff91c5c 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -96,3 +96,7 @@ #endif #define HAL_SOLO_GIMBAL_ENABLED 1 + +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 1 +#endif From f4ce0aa09a10b6fc168d76e67731b1aeac07ba8a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 16:36:08 +0100 Subject: [PATCH 06/21] AP_HAL_ChibiOS: allow forcing of trigger_groups() --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 3 ++- libraries/AP_HAL_ChibiOS/RCOutput.h | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 3d3245444fecd..1e0fbae0a39d6 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1398,7 +1398,8 @@ void RCOutput::trigger_groups() 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() || force_trigger) && rcout_thread_ctx) { chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND); } #endif diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 87aa9948dbd9c..a935de7f45874 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -285,6 +285,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput */ void rcout_thread(); + /* + Force group trigger from all callers rather than just from the main thread + */ + void force_trigger_groups(bool onoff) override { force_trigger = onoff; } + /* timer information */ @@ -579,6 +584,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t _dshot_cycle; // virtual timer for post-push() pulses virtual_timer_t _dshot_rate_timer; + // force triggering of groups + bool force_trigger; #if HAL_DSHOT_ENABLED // dshot commands From e37a21169084ef464f7270d22f773cf5b527d9ca Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 3 Jul 2024 18:26:11 +0100 Subject: [PATCH 07/21] AP_HAL_ChibiOS: compile MambaH743v4 at -O2 --- libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat index 661909c0d7091..f3179a3cb4832 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat @@ -20,6 +20,7 @@ define CH_CFG_ST_RESOLUTION 16 # leave 1 sectors free FLASH_RESERVE_START_KB 384 +env OPTIMIZE -O2 # use last 2 pages for flash storage # H743 has 16 pages of 128k each From 61308dc500e348b454bba16dd6557ac3883551f7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 11 Jul 2024 20:51:31 +0100 Subject: [PATCH 08/21] AP_HAL_ChibiOS: ensure dshot rate can be set dynamically honour the requested dshot rate as near as possible --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1e0fbae0a39d6..33fbe4ac69e2e 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -553,19 +553,19 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) } uint16_t drate = dshot_rate * loop_rate_hz; - _dshot_rate = dshot_rate; // BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap // at 732Hz so never allow rates below 800hz while (drate < 800) { - _dshot_rate++; - drate = _dshot_rate * loop_rate_hz; + dshot_rate++; + drate = dshot_rate * loop_rate_hz; } - // prevent stupidly high rates, ideally should also prevent high rates + // prevent stupidly high rate multiples, ideally should also prevent high rates // with slower dshot variants - if (drate > 4000) { - _dshot_rate = 4000 / loop_rate_hz; - drate = _dshot_rate * loop_rate_hz; + while (dshot_rate > 1 && drate > MAX(4096, loop_rate_hz)) { + dshot_rate--; + drate = dshot_rate * loop_rate_hz; } + _dshot_rate = dshot_rate; _dshot_period_us = 1000000UL / drate; #if HAL_WITH_IO_MCU if (iomcu_dshot) { From 47328c6ba0318697d12a3c01beded8eab5a4731f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 12:34:29 +0100 Subject: [PATCH 09/21] scripts: add AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED to build_options.py and extract_features.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index f31edfaf87ff3..20a6f11c41969 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -341,6 +341,7 @@ def config_option(self): Feature('IMU', 'HarmonicNotches', 'AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', 'Enable InertialSensor harmonic notch filters', 0, None), # noqa Feature('IMU', 'BatchSampler', 'AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', 'Enable Batch sampler', 0, None), # noqa + Feature('Other', 'RateLoopThread', 'AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', 'Enable Rate Loop Thread', 0, None), # noqa Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight gyro FFT calculations', 0, None), Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA output', 0, None), Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable Formatting of microSD cards', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index de6eec6589523..a020bc6d5266c 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -209,6 +209,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',), ('AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', r'AP_InertialSensor::HarmonicNotch::update_params\b',), ('AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', r'AP_InertialSensor::BatchSampler::init'), + ('AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', r'FastRateBuffer::get_next_gyro_sample\b',), ('HAL_GYROFFT_ENABLED', r'AP_GyroFFT::AP_GyroFFT\b',), ('HAL_DISPLAY_ENABLED', r'Display::init\b',), ('HAL_NMEA_OUTPUT_ENABLED', r'AP_NMEA_Output::update\b',), From 0d3e47506754e8b48fab25a5ae40474e5c1b8de2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 7 Aug 2024 16:56:16 +0100 Subject: [PATCH 10/21] waf: add rate loop config abstraction that allows code to be elided on non-copter builds --- Tools/ardupilotwaf/ap_library.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index 9d0971c1e816f..fbd355db39211 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -54,7 +54,8 @@ def _vehicle_index(vehicle): # note that AP_NavEKF3_core.h is needed for AP_NavEKF3_feature.h _vehicle_macros = ['APM_BUILD_DIRECTORY', 'AP_BUILD_TARGET_NAME', 'APM_BUILD_TYPE', 'APM_BUILD_COPTER_OR_HELI', - 'AP_NavEKF3_core.h', 'lua_generated_bindings.h'] + 'AP_NavEKF3_core.h', 'lua_generated_bindings.h', + 'AP_InertialSensor_rate_config.h'] _macros_re = re.compile(r'\b(%s)\b' % '|'.join(_vehicle_macros)) # some cpp files are not available at the time we run this check so need to be @@ -174,6 +175,7 @@ class ap_library_check_headers(Task.Task): 'libraries/AP_Scripting/lua_generated_bindings.h', 'libraries/AP_NavEKF3/AP_NavEKF3_feature.h', 'libraries/AP_LandingGear/AP_LandingGear_config.h', + 'libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h', ) whitelist = tuple(os.path.join(*p.split('/')) for p in whitelist) From e673b70c67d1ab0c235c0fcd2d8242085f4ac894 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 25 Aug 2024 16:20:29 +0100 Subject: [PATCH 11/21] Copter: correctly set fast rate thread rates --- ArduCopter/Copter.h | 15 ++++++--- ArduCopter/rate_thread.cpp | 63 +++++++++++++++++++++++--------------- 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ba11e5485734c..943552114b85d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -740,13 +740,20 @@ class Copter : public AP_Vehicle { 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); + struct RateControllerRates { + uint8_t fast_logging_rate; + uint8_t medium_logging_rate; + uint8_t filter_rate; + uint8_t main_loop_rate; + }; + + uint8_t calc_gyro_decimation(uint8_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(); + uint8_t rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); + void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates); + void disable_fast_rate_loop(RateControllerRates& rates); void update_dynamic_notch_at_specified_rate_main(); // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index 29434215aaf6b..115c589729d96 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -150,11 +150,16 @@ #define DIV_ROUND_INT(x, d) ((x + d/2) / d) -uint8_t Copter::calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz) +uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz) { return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); } +static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count) +{ + return decimation_rate > 0 && ++decimation_count >= decimation_rate; +} + //#define RATE_LOOP_TIMING_DEBUG /* thread for rate control @@ -163,6 +168,11 @@ 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; + + // set up the decimation rates + RateControllerRates rates; + rate_controller_set_rates(rate_decimation, rates, false); + uint32_t rate_loop_count = 0; uint32_t prev_loop_count = 0; @@ -191,13 +201,9 @@ void Copter::rate_controller_thread() #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; @@ -210,7 +216,7 @@ void Copter::rate_controller_thread() // 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(); + disable_fast_rate_loop(rates); was_using_rate_thread = false; } hal.scheduler->delay_microseconds(500); @@ -220,7 +226,7 @@ void Copter::rate_controller_thread() // set up rate thread requirements if (!using_rate_thread) { - enable_fast_rate_loop(rate_decimation); + enable_fast_rate_loop(rate_decimation, rates); } ins.set_rate_decimation(rate_decimation); @@ -278,7 +284,7 @@ void Copter::rate_controller_thread() // 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()); + attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt); #ifdef RATE_LOOP_TIMING_DEBUG rate_controller_time_us += AP_HAL::micros() - rate_now_us; @@ -286,14 +292,15 @@ void Copter::rate_controller_thread() #endif // immediately output the new motor values - if (main_loop_count++ >= main_loop_rate_decimate) { + if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) { main_loop_count = 0; } motors_output(main_loop_count == 0); // process filter updates - if (filter_loop_count++ >= filter_rate_decimate) { + if (run_decimated_callback(rates.filter_rate, filter_loop_count)) { filter_loop_count = 0; + rate_controller_filter_update(); } @@ -305,18 +312,17 @@ void Copter::rate_controller_thread() #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) { + if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) { 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) { + if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) { log_loop_count = 0; rate_controller_log_update(); } } -#else - (void)log_fast_rate_decimate; #endif #ifdef RATE_LOOP_TIMING_DEBUG @@ -333,6 +339,10 @@ void Copter::rate_controller_thread() // enabled at runtime last_notch_sample_ms = now_ms; attitude_control->set_notch_sample_rate(1.0 / sensor_dt); +#ifdef RATE_LOOP_TIMING_DEBUG + hal.console->printf("Sample rate %.1f, main loop %u, fast rate %u, med rate %u\n", 1.0 / sensor_dt, + rates.main_loop_rate, rates.fast_logging_rate, rates.medium_logging_rate); +#endif } // interlock for printing fixed rate active @@ -346,7 +356,7 @@ void Copter::rate_controller_thread() && ((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); + rate_controller_set_rates(rate_decimation, rates, false); notify_fixed_rate_active = false; } @@ -366,7 +376,7 @@ void Copter::rate_controller_thread() 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); + rate_controller_set_rates(rate_decimation, rates, true); prev_loop_count = rate_loop_count; rate_loop_count = 0; running_slow = 0; @@ -377,7 +387,7 @@ void Copter::rate_controller_thread() || 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); + rate_controller_set_rates(rate_decimation, rates, false); prev_loop_count = 0; rate_loop_count = 0; last_rate_increase_ms = now_ms; @@ -420,7 +430,7 @@ void Copter::rate_controller_filter_update() /* update rate controller rates and return the logging rate */ -uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high) +uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, 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); @@ -431,26 +441,31 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate); #if HAL_LOGGING_ENABLED if (attitude_rate > 1000) { - return calc_gyro_decimation(rate_decimation, 1000); + rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz } else { - return calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); } + rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz #endif + rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2); + return 0; } -void Copter::enable_fast_rate_loop(uint8_t rate_decimation) +void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates) { ins.enable_fast_rate_buffer(); - rate_controller_set_rates(rate_decimation, false); + rate_controller_set_rates(rate_decimation, rates, false); hal.rcout->force_trigger_groups(true); using_rate_thread = true; } -void Copter::disable_fast_rate_loop() +void Copter::disable_fast_rate_loop(RateControllerRates& rates) { using_rate_thread = false; - rate_controller_set_rates(calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()), false); + uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()); + rate_controller_set_rates(rate_decimation, rates, false); hal.rcout->force_trigger_groups(false); ins.disable_fast_rate_buffer(); } From b961a48a7c62bc89e4e7a64e2d95224c340f690d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 12/21] AP_InertialSensor: keep a queue of gyro samples for use by the rate thread decimate the gyro window locally configure rate loop buffer based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED allow backends to be updated from rate thread output debug error if rate loop buffer overruns add support for updating filter parameters independently of propagating samples add rate loop config abstraction that allows code to be elided on non-copter builds must be using harmonic notch to use rate thread mediate fast rate loop buffer using mutex and binary semaphore ensure gyro samples are used when the rate loop buffer isn't Co-Authored-By: Andrew Tridgell --- .../AP_InertialSensor/AP_InertialSensor.h | 33 ++++- .../AP_InertialSensor_Backend.cpp | 14 ++ .../AP_InertialSensor_Backend.h | 9 ++ .../AP_InertialSensor_rate_config.h | 9 ++ .../AP_InertialSensor/FastRateBuffer.cpp | 124 ++++++++++++++++++ libraries/AP_InertialSensor/FastRateBuffer.h | 50 +++++++ 6 files changed, 237 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h create mode 100644 libraries/AP_InertialSensor/FastRateBuffer.cpp create mode 100644 libraries/AP_InertialSensor/FastRateBuffer.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 30e60bec6c231..446a46f2855a9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -9,6 +9,8 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + #include #include @@ -34,6 +36,7 @@ class AP_InertialSensor_Backend; class AuxiliaryBus; class AP_AHRS; +class FastRateBuffer; /* forward declare AP_Logger class. We can't include logger.h @@ -51,6 +54,7 @@ class AP_Logger; class AP_InertialSensor : AP_AccelCal_Client { friend class AP_InertialSensor_Backend; + friend class FastRateBuffer; public: AP_InertialSensor(); @@ -161,12 +165,12 @@ class AP_InertialSensor : AP_AccelCal_Client const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; } FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; } FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_first_usable_gyro, axis); } - uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } - uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED bool has_fft_notch() const; #endif #endif + uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } + uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } bool set_gyro_window_size(uint16_t size); // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); } @@ -263,6 +267,7 @@ class AP_InertialSensor : AP_AccelCal_Client AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance); void detect_backends(void); + void update_backends(); // accel peak hold detector void set_accel_peak_hold(uint8_t instance, const Vector3f &accel); @@ -798,6 +803,30 @@ class AP_InertialSensor : AP_AccelCal_Client bool raw_logging_option_set(RAW_LOGGING_OPTION option) const { return (raw_logging_options.get() & int32_t(option)) != 0; } + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // Support for the fast rate thread in copter + FastRateBuffer* fast_rate_buffer; + +public: + // enable the fast rate buffer and start pushing samples to it + void enable_fast_rate_buffer(); + // disable the fast rate buffer and stop pushing samples to it + void disable_fast_rate_buffer(); + // get the next available gyro sample from the fast rate buffer + bool get_next_gyro_sample(Vector3f& gyro); + // get the number of available gyro samples in the fast rate buffer + uint32_t get_num_gyro_samples(); + // set the rate at which samples are collected, unused samples are dropped + void set_rate_decimation(uint8_t rdec); + // are gyro samples being sourced from the rate loop buffer + bool use_rate_loop_gyro_samples() const; + // push a new gyro sample into the fast rate buffer + bool push_next_gyro_sample(const Vector3f& gyro); + // run the filter parmeter update code. + void update_backend_filters(); + // are rate loop samples enabled for this instance? + bool is_rate_loop_gyro_enabled(uint8_t instance) const; + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED }; namespace AP { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 944b20661a131..30d06b02b16c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -2,6 +2,7 @@ #include #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" #include @@ -254,9 +255,21 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const notch.filter[instance].reset(); } #endif + gyro_filtered = _imu._gyro_filtered[instance]; + } + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + if (_imu.is_rate_loop_gyro_enabled(instance)) { + if (_imu.push_next_gyro_sample(gyro_filtered)) { + // if we used the value, record it for publication to the front-end + _imu._gyro_filtered[instance] = gyro_filtered; + } } else { _imu._gyro_filtered[instance] = gyro_filtered; } +#else + _imu._gyro_filtered[instance] = gyro_filtered; +#endif } void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -772,6 +785,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ if (has_been_killed(instance)) { return; } + if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); #if HAL_GYROFFT_ENABLED diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 817cb2e947c85..7ab5b420d817e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -53,6 +53,15 @@ class AP_InertialSensor_Backend */ virtual bool update() = 0; /* front end */ + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* + * Update the filter parameters. Called by the frontend to propagate + * filter parameters to the frontend structure via the + * update_gyro_filters() and update_accel_filters() functions + */ + void update_filters() __RAMFUNC__; /* front end */ + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h new file mode 100644 index 0000000000000..8bf45fecab7f0 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h @@ -0,0 +1,9 @@ +#pragma once + +#include +#include +#include + +#ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter)) +#endif diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp new file mode 100644 index 0000000000000..6617915fecd62 --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -0,0 +1,124 @@ +/* + 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 . +*/ + +#include +#include "AP_InertialSensor_rate_config.h" +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#include "FastRateBuffer.h" +#include + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +// hal.console can be accessed from bus threads on ChibiOS +#define debug(fmt, args ...) do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0) +#else +#define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) +#endif + +void AP_InertialSensor::enable_fast_rate_buffer() +{ + fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); +} + +void AP_InertialSensor::disable_fast_rate_buffer() +{ + delete fast_rate_buffer; + fast_rate_buffer = nullptr; +} + +uint32_t AP_InertialSensor::get_num_gyro_samples() +{ + return fast_rate_buffer->get_num_gyro_samples(); +} + +void AP_InertialSensor::set_rate_decimation(uint8_t rdec) +{ + fast_rate_buffer->set_rate_decimation(rdec); +} + +// are gyro samples being sourced from the rate loop buffer +bool AP_InertialSensor::use_rate_loop_gyro_samples() const +{ + return fast_rate_buffer != nullptr; +} + +// whether or not to push the current gyro sample +bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const +{ + return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); +} + +bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + return fast_rate_buffer->get_next_gyro_sample(gyro); +} + +bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + if (_rate_loop_gyro_window.available() == 0) { + _notifier.wait_blocking(); + } + + WITH_SEMAPHORE(_mutex); + + return _rate_loop_gyro_window.pop(gyro); +} + +bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) +{ + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { + return false; + } + /* + tell the rate thread we have a new sample + */ + WITH_SEMAPHORE(fast_rate_buffer->_mutex); + + if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) { + debug("dropped rate loop sample"); + } + fast_rate_buffer->rate_decimation_count = 0; + fast_rate_buffer->_notifier.signal(); + return true; +} + +void AP_InertialSensor::update_backend_filters() +{ + for (uint8_t i=0; i<_backend_count; i++) { + _backends[i]->update_filters(); + } +} + +void AP_InertialSensor_Backend::update_filters() +{ + WITH_SEMAPHORE(_sem); + + update_accel_filters(accel_instance); + update_gyro_filters(gyro_instance); +} + +#endif // AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h new file mode 100644 index 0000000000000..712500fb8d9bb --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -0,0 +1,50 @@ +/* + 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 . +*/ +#pragma once + +#include "AP_InertialSensor.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + +#include +#include +#include +#include + +class FastRateBuffer +{ + friend class AP_InertialSensor; +public: + bool get_next_gyro_sample(Vector3f& gyro); + uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); } + void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; } + // whether or not to push the current gyro sample + bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; } + bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } + +private: + /* + binary semaphore for rate loop to use to start a rate loop when + we hav finished filtering the primary IMU + */ + ObjectBuffer _rate_loop_gyro_window{AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE}; + uint8_t rate_decimation; // 0 means off + uint8_t rate_decimation_count; + HAL_BinarySemaphore _notifier; + HAL_Semaphore _mutex; +}; +#endif \ No newline at end of file From 353b2898ba7e474a2414e1c7e6ba13979d627b38 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 5 Oct 2024 18:26:22 +0100 Subject: [PATCH 13/21] Copter: ensure decimated rates are never 0 in rate thread --- ArduCopter/rate_thread.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index 115c589729d96..ef8b624c54dd1 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -152,7 +152,7 @@ uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz) { - return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); + return MAX(uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)), 1U); } static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count) From 26d6de4355530e50cfb1401d241b267059e1a42b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 13:04:27 +0900 Subject: [PATCH 14/21] AP_InertialSensor: avoid multiple allocations of rate loop buffer add nullptr checks and comments to FastRateBuffer --- .../AP_InertialSensor/AP_InertialSensor.h | 3 +- .../AP_InertialSensor/FastRateBuffer.cpp | 45 ++++++++++++++----- libraries/AP_InertialSensor/FastRateBuffer.h | 1 + 3 files changed, 36 insertions(+), 13 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 446a46f2855a9..8cd1fd2307648 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -806,6 +806,7 @@ class AP_InertialSensor : AP_AccelCal_Client // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // Support for the fast rate thread in copter FastRateBuffer* fast_rate_buffer; + bool fast_rate_buffer_enabled; public: // enable the fast rate buffer and start pushing samples to it @@ -818,8 +819,6 @@ class AP_InertialSensor : AP_AccelCal_Client uint32_t get_num_gyro_samples(); // set the rate at which samples are collected, unused samples are dropped void set_rate_decimation(uint8_t rdec); - // are gyro samples being sourced from the rate loop buffer - bool use_rate_loop_gyro_samples() const; // push a new gyro sample into the fast rate buffer bool push_next_gyro_sample(const Vector3f& gyro); // run the filter parmeter update code. diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index 6617915fecd62..c634c84d878bb 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -31,48 +31,62 @@ extern const AP_HAL::HAL& hal; #define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) #endif +// enable the fast rate buffer and start pushing samples to it void AP_InertialSensor::enable_fast_rate_buffer() { - fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); + if (fast_rate_buffer_enabled) { + return; + } + if (fast_rate_buffer == nullptr) { + fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); + } + fast_rate_buffer_enabled = true; } +// disable the fast rate buffer and stop pushing samples to it void AP_InertialSensor::disable_fast_rate_buffer() { - delete fast_rate_buffer; - fast_rate_buffer = nullptr; + fast_rate_buffer_enabled = false; + if (fast_rate_buffer != nullptr) { + fast_rate_buffer->reset(); + } } +// get the number of available gyro samples in the fast rate buffer uint32_t AP_InertialSensor::get_num_gyro_samples() { + if (fast_rate_buffer == nullptr) { + return 0; + } return fast_rate_buffer->get_num_gyro_samples(); } +// set the rate at which samples are collected, unused samples are dropped void AP_InertialSensor::set_rate_decimation(uint8_t rdec) { + if (fast_rate_buffer == nullptr) { + return; + } fast_rate_buffer->set_rate_decimation(rdec); } -// are gyro samples being sourced from the rate loop buffer -bool AP_InertialSensor::use_rate_loop_gyro_samples() const -{ - return fast_rate_buffer != nullptr; -} - // whether or not to push the current gyro sample bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const { - return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); + return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); } +// get the next available gyro sample from the fast rate buffer bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) { - if (!use_rate_loop_gyro_samples()) { + if (!fast_rate_buffer_enabled) { return false; } return fast_rate_buffer->get_next_gyro_sample(gyro); } + bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) { if (!use_rate_loop_gyro_samples()) { @@ -88,8 +102,17 @@ bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) return _rate_loop_gyro_window.pop(gyro); } +void FastRateBuffer::reset() +{ + _rate_loop_gyro_window.clear(); +} + bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) { + if (fast_rate_buffer == nullptr) { + return false; + } + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { return false; } diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h index 712500fb8d9bb..c4f53b7bf6d81 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.h +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -35,6 +35,7 @@ class FastRateBuffer // whether or not to push the current gyro sample bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; } bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } + void reset(); private: /* From b8d0a00e6d05f7af552c4e8a8bf80ac69e1e833b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 19:21:31 +0900 Subject: [PATCH 15/21] AP_HAL_ChibiOS: add comments to force_push on rcout --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 3 +++ libraries/AP_HAL_ChibiOS/RCOutput.h | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 33fbe4ac69e2e..5da86d9558943 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1399,6 +1399,9 @@ void RCOutput::trigger_groups() #if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED // trigger a PWM send if (!in_soft_serial() && + // we always trigger an output if we are in the main thread + // we also always trigger an output if we are in the rate thread and thus + // force_trigger has been set (hal.scheduler->in_main_thread() || force_trigger) && rcout_thread_ctx) { chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND); } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index a935de7f45874..c1ace289398c5 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -584,7 +584,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t _dshot_cycle; // virtual timer for post-push() pulses virtual_timer_t _dshot_rate_timer; - // force triggering of groups + // force triggering of groups, this is used by the rate thread to ensure output occurs bool force_trigger; #if HAL_DSHOT_ENABLED From 0d346b2dd3f16470082d759534c7f12ff181cc4b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 13:04:08 +0900 Subject: [PATCH 16/21] Copter: move RTDT logging to fast path log after motor output in fast rate thread --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 1 + ArduCopter/Log.cpp | 38 ++++++++++++++++++++++++++++++++++++++ ArduCopter/Parameters.cpp | 4 ++-- ArduCopter/defines.h | 3 ++- ArduCopter/motors.cpp | 5 ++++- ArduCopter/rate_thread.cpp | 36 ++++++++++++++---------------------- 7 files changed, 62 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0deee9d500ddd..43c48a0ee1a61 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -261,6 +261,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168), #endif #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215), #endif }; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 943552114b85d..f48b6346dce79 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -920,6 +920,7 @@ class Copter : public AP_Vehicle { void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); + void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin); #endif // HAL_LOGGING_ENABLED // mode.cpp diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b446904e1ab05..2e06ecf80ffdd 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -1,4 +1,5 @@ #include "Copter.h" +#include #if HAL_LOGGING_ENABLED @@ -343,6 +344,16 @@ struct PACKED log_Guided_Attitude_Target { float climb_rate; }; +// rate thread dt stats +struct PACKED log_Rate_Thread_Dt { + LOG_PACKET_HEADER; + uint64_t time_us; + float dt; + float dtAvg; + float dtMax; + float dtMin; +}; + // Write a Guided mode position target // pos_target is lat, lon, alt OR offset from ekf origin in cm // terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain @@ -390,6 +401,21 @@ void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, f logger.WriteBlock(&pkt, sizeof(pkt)); } +void Copter::Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin) +{ +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + const log_Rate_Thread_Dt pkt { + LOG_PACKET_HEADER_INIT(LOG_RATE_THREAD_DT_MSG), + time_us : AP_HAL::micros64(), + dt : dt, + dtAvg : dtAvg, + dtMax : dtMax, + dtMin : dtMin + }; + logger.WriteBlock(&pkt, sizeof(pkt)); +#endif +} + // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -531,6 +557,18 @@ const struct LogStructure Copter::log_structure[] = { { LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target), "GUIA", "QBffffffff", "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true }, + +// @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 + + { LOG_RATE_THREAD_DT_MSG, sizeof(log_Rate_Thread_Dt), + "RTDT", "Qffff", "TimeUS,dt,dtAvg,dtMax,dtMin", "sssss", "F----" , true }, + }; uint8_t Copter::get_num_log_structures() const diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 37309440fbfd3..a90c06b789acd 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1236,14 +1236,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // @Param: FSTRATE_ENABLE // @DisplayName: Enable the fast Rate thread - // @Description: Enable the fast Rate thread + // @Description: Enable the fast Rate thread. In the default case the fast rate divisor, which controls the update frequency of the thread, is dynamically scaled from FSTRATE_DIV to avoid overrun in the gyro sample buffer and main loop slow-downs. Other values can be selected to fix the divisor to FSTRATE_DIV on arming or always. // @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. + // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate in Hz divided by this value. This value is scaled depending on the configuration of FSTRATE_ENABLE. // @User: Advanced // @Range: 1 10 AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index acba81d953960..6603c1846e922 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -87,7 +87,8 @@ enum LoggingParameters { LOG_GUIDED_POSITION_TARGET_MSG, LOG_SYSIDD_MSG, LOG_SYSIDS_MSG, - LOG_GUIDED_ATTITUDE_TARGET_MSG + LOG_GUIDED_ATTITUDE_TARGET_MSG, + LOG_RATE_THREAD_DT_MSG }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 784580b8ecb7d..0b2193107ffca 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -133,6 +133,7 @@ void Copter::auto_disarm_check() } // motors_output - send output to motors library which will adjust and send to ESCs and servos +// full_push is true when slower rate updates (e.g. servo output) need to be performed at the main loop rate. void Copter::motors_output(bool full_push) { #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED @@ -184,13 +185,15 @@ void Copter::motors_output(bool full_push) // push all channels if (full_push) { + // motor output including servos and other updates that need to run at the main loop rate srv.push(); } else { + // motor output only at main loop rate or faster hal.rcout->push(); } } -// motors_output from main thread +// motors_output from main thread at main loop rate void Copter::motors_output_main() { if (!using_rate_thread) { diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index ef8b624c54dd1..ac533165e3b1d 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -248,28 +248,6 @@ void Copter::rate_controller_thread() 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) { @@ -304,6 +282,18 @@ void Copter::rate_controller_thread() rate_controller_filter_update(); } + max_dt = MAX(dt, max_dt); + min_dt = MIN(dt, min_dt); + +#if HAL_LOGGING_ENABLED + if (now_ms - last_rtdt_log_ms >= 100) { // 10 Hz + Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt); + max_dt = sensor_dt; + min_dt = sensor_dt; + last_rtdt_log_ms = now_ms; + } +#endif + #ifdef RATE_LOOP_TIMING_DEBUG motor_output_us += AP_HAL::micros() - rate_now_us; rate_now_us = AP_HAL::micros(); @@ -453,6 +443,7 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControlle return 0; } +// enable the fast rate thread using the provided decimation rate and record the new output rates void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates) { ins.enable_fast_rate_buffer(); @@ -461,6 +452,7 @@ void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& using_rate_thread = true; } +// disable the fast rate thread and record the new output rates void Copter::disable_fast_rate_loop(RateControllerRates& rates) { using_rate_thread = false; From c57c6d50d9363a09d6336b4b31a5a074654b5f06 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 10 Nov 2024 14:34:41 +0000 Subject: [PATCH 17/21] Copter: address review comments --- ArduCopter/Copter.h | 2 +- ArduCopter/rate_thread.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f48b6346dce79..175a0d0586115 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -751,7 +751,7 @@ class Copter : public AP_Vehicle { void rate_controller_thread(); void rate_controller_filter_update(); void rate_controller_log_update(); - uint8_t rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); + void rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates); void disable_fast_rate_loop(RateControllerRates& rates); void update_dynamic_notch_at_specified_rate_main(); diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index ac533165e3b1d..5b64301fb7bec 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -248,7 +248,6 @@ void Copter::rate_controller_thread() const float dt = dt_us * 1.0e-6; last_run_us = now_us; - motors->set_dt(sensor_dt); // check if we are falling behind if (ins.get_num_gyro_samples() > 2) { running_slow++; @@ -420,7 +419,7 @@ void Copter::rate_controller_filter_update() /* update rate controller rates and return the logging rate */ -uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high) +void Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, 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); @@ -439,8 +438,6 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControlle #endif rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2); - - return 0; } // enable the fast rate thread using the provided decimation rate and record the new output rates From 90daff6829ec61a19f5a03f6c155b0070d0e054e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 10 Nov 2024 14:34:55 +0000 Subject: [PATCH 18/21] AP_InertialSensor: address review comments --- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 -- libraries/AP_InertialSensor/FastRateBuffer.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 8cd1fd2307648..bae72d86f9e70 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -9,8 +9,6 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout -#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop - #include #include diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h index c4f53b7bf6d81..77ce3e267915c 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.h +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -48,4 +48,4 @@ class FastRateBuffer HAL_BinarySemaphore _notifier; HAL_Semaphore _mutex; }; -#endif \ No newline at end of file +#endif From 89bc8ceff5aca61baba7997c511d2d8c62075827 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 13 Nov 2024 13:30:29 +0000 Subject: [PATCH 19/21] AP_HAL_ChibiOS: add cork()/push() check --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5da86d9558943..5d4274ac706a4 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1329,6 +1329,9 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) */ void RCOutput::cork(void) { + if (corked) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } corked = true; #if HAL_WITH_IO_MCU if (iomcu_enabled) { @@ -1342,6 +1345,9 @@ void RCOutput::cork(void) */ void RCOutput::push(void) { + if (!corked) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } corked = false; push_local(); #if HAL_WITH_IO_MCU From 83b104b072e3302cb0bd7c944f42e8e58609c174 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 16 Nov 2024 16:30:46 +0000 Subject: [PATCH 20/21] AP_InertialSensor: belt and braces checks --- libraries/AP_InertialSensor/FastRateBuffer.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index c634c84d878bb..29601965d38ed 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -40,7 +40,7 @@ void AP_InertialSensor::enable_fast_rate_buffer() if (fast_rate_buffer == nullptr) { fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); } - fast_rate_buffer_enabled = true; + fast_rate_buffer_enabled = fast_rate_buffer != nullptr; } // disable the fast rate buffer and stop pushing samples to it @@ -55,7 +55,7 @@ void AP_InertialSensor::disable_fast_rate_buffer() // get the number of available gyro samples in the fast rate buffer uint32_t AP_InertialSensor::get_num_gyro_samples() { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return 0; } return fast_rate_buffer->get_num_gyro_samples(); @@ -64,7 +64,7 @@ uint32_t AP_InertialSensor::get_num_gyro_samples() // set the rate at which samples are collected, unused samples are dropped void AP_InertialSensor::set_rate_decimation(uint8_t rdec) { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return; } fast_rate_buffer->set_rate_decimation(rdec); @@ -73,13 +73,16 @@ void AP_InertialSensor::set_rate_decimation(uint8_t rdec) // whether or not to push the current gyro sample bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const { - return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { + return false; + } + return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); } // get the next available gyro sample from the fast rate buffer bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) { - if (!fast_rate_buffer_enabled) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return false; } @@ -109,7 +112,7 @@ void FastRateBuffer::reset() bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return false; } From 6fefbf064e05a7b11b608e30f3fa79d811ff3208 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 16 Nov 2024 16:31:21 +0000 Subject: [PATCH 21/21] Copter: allocation failure on rate thread start --- ArduCopter/Copter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 43c48a0ee1a61..a388ffdc16242 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -769,6 +769,8 @@ void Copter::one_hz_loop() "rate", 1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { started_rate_thread = true; + } else { + AP_BoardConfig::allocation_error("rate thread"); } } #endif