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..99e3615076a43 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,25 @@ 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 + 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, 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 #if AC_CUSTOMCONTROL_MULTI_ENABLED void run_custom_controller() { custom_control.update(); } @@ -875,6 +904,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 +948,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 +1110,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..ef8b624c54dd1 --- /dev/null +++ b/ArduCopter/rate_thread.cpp @@ -0,0 +1,501 @@ +/* + 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(uint8_t gyro_decimation, uint16_t 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) +{ + return decimation_rate > 0 && ++decimation_count >= decimation_rate; +} + +//#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; + + // 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; + + 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 +#if HAL_LOGGING_ENABLED + uint8_t log_loop_count = 0; +#endif + 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(rates); + 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, rates); + } + 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(gyro + ahrs.get_gyro_drift(), sensor_dt); + +#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 (run_decimated_callback(rates.main_loop_rate, main_loop_count)) { + main_loop_count = 0; + } + motors_output(main_loop_count == 0); + + // process filter updates + if (run_decimated_callback(rates.filter_rate, filter_loop_count)) { + 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 (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 (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) { + log_loop_count = 0; + rate_controller_log_update(); + } + } +#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); +#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 + 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; + rate_controller_set_rates(rate_decimation, rates, 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; + rate_controller_set_rates(rate_decimation, rates, 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; + + rate_controller_set_rates(rate_decimation, rates, 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, 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); + 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) { + rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz + } else { + 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, RateControllerRates& rates) +{ + ins.enable_fast_rate_buffer(); + rate_controller_set_rates(rate_decimation, rates, false); + hal.rcout->force_trigger_groups(true); + using_rate_thread = true; +} + +void Copter::disable_fast_rate_loop(RateControllerRates& rates) +{ + using_rate_thread = 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(); +} + +/* + 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 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) 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, 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',), 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 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/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 */ 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 diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 3d3245444fecd..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) { @@ -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 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 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 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;