From a48c49f110228a9bcb5a4a2a39636738907d48c5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 01/14] 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 47e45969020fc..5d2df84ad48f8 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)) { @@ -724,11 +736,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 3bdba074d5467..c4b1790ccb71b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -630,6 +630,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; @@ -725,7 +736,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(); } @@ -875,6 +897,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); @@ -918,7 +941,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 @@ -1079,6 +1103,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 a25aea6cbbe8f..893ea45542190 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 0eee4058331d7..0c1ac69bd9191 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -685,6 +685,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 5c5deae8995cd..b85103be2b63c 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 45eccc3e84f99..341c2fbe36b4a 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 ADVANCED_FAILSAFE // this is to allow the failsafe module to deliberately crash @@ -181,7 +181,19 @@ void Copter::motors_output() } // push all channels - SRV_Channels::push(); + if (full_push) { + SRV_Channels::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 ca90b8fa7f29d32129812701369895e84a88be71 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 21:35:30 +0100 Subject: [PATCH 02/14] 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 1747c72fc9451..eb74ba0cce4e1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6250,6 +6250,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 @@ -12069,6 +12140,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 621d0179c187d01a89c033bd33e0efbcb55fc91e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 25 May 2024 09:31:12 +0100 Subject: [PATCH 03/14] AP_AHRS: correct get_accel() to use primary accel rather than first usable for scripting --- libraries/AP_AHRS/AP_AHRS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e6aa24064a097..43951c34dbee3 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -620,7 +620,7 @@ class AP_AHRS { // return primary accels, for lua const Vector3f &get_accel(void) const { - return AP::ins().get_accel(); + return AP::ins().get_accel(_get_primary_accel_index()); } // return primary accel bias. This should be subtracted from From 82eee362439e6559139dfdd1803cc23696c1c4db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Feb 2024 12:06:22 +1100 Subject: [PATCH 04/14] 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 562ac606075a4..c3d77169c5b51 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 bec42ebf19af5ff9f0f41c527f42611bbdaac4ee Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 10:30:02 +0100 Subject: [PATCH 05/14] 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 d8a3d161f5ef2f22d4471cc738b21cae147a9419 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 20:00:44 +0100 Subject: [PATCH 06/14] 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 874782fb9187b..169841b974ffe 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -383,4 +383,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 b92ea9c40663b..272b54f257e9e 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -421,3 +421,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 04f18ef9729605f0c35cc4ebc0c4f72e66a1182a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 16:36:08 +0100 Subject: [PATCH 07/14] 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 252256e1d1ece0bf94f250e5343e8d596bf5d316 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 3 Jul 2024 18:26:11 +0100 Subject: [PATCH 08/14] 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 20d9f5b1501ffa401617034ac1ca6756fca0ba3a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 11 Jul 2024 20:51:31 +0100 Subject: [PATCH 09/14] 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 d68b1bf4a9c65f5757dbb8495f3c4f34eeee1b15 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 12:34:29 +0100 Subject: [PATCH 10/14] 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 c975752aa4a2a..b2401e7dc44aa 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -339,6 +339,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 f71c3eea7712f..c8ff3b2436194 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -208,6 +208,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 e7e681999025cb52e5270fba4ee47c9cb5945bce Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 7 Aug 2024 16:56:16 +0100 Subject: [PATCH 11/14] 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 c97d76ac539d96712188bfbfb76063e46b699577 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 25 Aug 2024 16:20:29 +0100 Subject: [PATCH 12/14] 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 c4b1790ccb71b..99e3615076a43 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -739,13 +739,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 f59b5c918df6bfd916180f323e5b8baee41a84a1 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 13/14] 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 00d149e149223..051ac1346489c 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 25e2a6ba035086e35a81bb3b2bdbc14a33b9d554 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 5 Oct 2024 18:26:22 +0100 Subject: [PATCH 14/14] 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)