diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 3e71782c18..02bbee2749 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 0f263b89c0..a3ac456c8a 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, @@ -618,12 +622,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 @@ -641,10 +648,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)) { @@ -789,11 +801,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 db74c31dc4..0ccdeed0da 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -631,6 +631,17 @@ private: 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; @@ -728,7 +739,18 @@ private: 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(); } @@ -878,6 +900,7 @@ private: // 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); @@ -921,7 +944,8 @@ private: // 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 @@ -1086,6 +1110,9 @@ private: 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 fffea298f1..b446904e1a 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 fd5eea3e86..37309440fb 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 0cf0a8cb51..2a85bc9ee9 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -690,6 +690,9 @@ public: 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 db1e0ddc26..1462454b78 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 2313c30953..b40e5bc4ae 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 c37a4d55a2..784580b8ec 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 0000000000..29434215aa --- /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