Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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 <andrew@tridgell.net>
This commit is contained in:
Andy Piper 2024-02-11 16:19:56 +11:00 committed by Andrew Tridgell
parent eaf20db6ea
commit f84c855dd1
10 changed files with 602 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/Attitude.cpp
View File

@ -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();
}

39
ArduCopter/Copter.cpp
View File

@ -75,6 +75,7 @@
*/
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#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()

31
ArduCopter/Copter.h
View File

@ -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
};

4
ArduCopter/Log.cpp
View File

@ -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);
}

17
ArduCopter/Parameters.cpp
View File

@ -1,6 +1,7 @@
#include "Copter.h"
#include <AP_Gripper/AP_Gripper.h>
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
/*
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

3
ArduCopter/Parameters.h
View File

@ -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[];

1
ArduCopter/mode_systemid.cpp
View File

@ -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()) {

6
ArduCopter/motor_test.cpp
View File

@ -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

16
ArduCopter/motors.cpp
View File

@ -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

486
ArduCopter/rate_thread.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#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 &notch : 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