2024-02-11 01:19:56 -04:00
|
|
|
/*
|
|
|
|
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)
|
|
|
|
|
2024-08-25 12:20:29 -03:00
|
|
|
uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz)
|
2024-02-11 01:19:56 -04:00
|
|
|
{
|
2024-10-05 14:26:22 -03:00
|
|
|
return MAX(uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)), 1U);
|
2024-02-11 01:19:56 -04:00
|
|
|
}
|
|
|
|
|
2024-08-25 12:20:29 -03:00
|
|
|
static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count)
|
|
|
|
{
|
|
|
|
return decimation_rate > 0 && ++decimation_count >= decimation_rate;
|
|
|
|
}
|
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
//#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;
|
2024-08-25 12:20:29 -03:00
|
|
|
|
|
|
|
// set up the decimation rates
|
|
|
|
RateControllerRates rates;
|
|
|
|
rate_controller_set_rates(rate_decimation, rates, false);
|
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
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) {
|
2024-08-25 12:20:29 -03:00
|
|
|
disable_fast_rate_loop(rates);
|
2024-02-11 01:19:56 -04:00
|
|
|
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) {
|
2024-08-25 12:20:29 -03:00
|
|
|
enable_fast_rate_loop(rate_decimation, rates);
|
2024-02-11 01:19:56 -04:00
|
|
|
}
|
|
|
|
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;
|
|
|
|
|
|
|
|
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
|
2024-08-25 12:20:29 -03:00
|
|
|
attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt);
|
2024-02-11 01:19:56 -04:00
|
|
|
|
|
|
|
#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
|
2024-08-25 12:20:29 -03:00
|
|
|
if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) {
|
2024-02-11 01:19:56 -04:00
|
|
|
main_loop_count = 0;
|
|
|
|
}
|
|
|
|
motors_output(main_loop_count == 0);
|
|
|
|
|
|
|
|
// process filter updates
|
2024-08-25 12:20:29 -03:00
|
|
|
if (run_decimated_callback(rates.filter_rate, filter_loop_count)) {
|
2024-02-11 01:19:56 -04:00
|
|
|
filter_loop_count = 0;
|
2024-08-25 12:20:29 -03:00
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
rate_controller_filter_update();
|
|
|
|
}
|
|
|
|
|
2024-10-25 01:04:08 -03:00
|
|
|
max_dt = MAX(dt, max_dt);
|
|
|
|
min_dt = MIN(dt, min_dt);
|
|
|
|
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
|
|
if (now_ms - last_rtdt_log_ms >= 100) { // 10 Hz
|
|
|
|
Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt);
|
|
|
|
max_dt = sensor_dt;
|
|
|
|
min_dt = sensor_dt;
|
|
|
|
last_rtdt_log_ms = now_ms;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
#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)) {
|
2024-08-25 12:20:29 -03:00
|
|
|
if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) {
|
2024-02-11 01:19:56 -04:00
|
|
|
log_loop_count = 0;
|
|
|
|
rate_controller_log_update();
|
2024-08-25 12:20:29 -03:00
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
}
|
|
|
|
} else if (should_log(MASK_LOG_ATTITUDE_MED)) {
|
2024-08-25 12:20:29 -03:00
|
|
|
if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) {
|
2024-02-11 01:19:56 -04:00
|
|
|
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);
|
2024-08-25 12:20:29 -03:00
|
|
|
#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
|
2024-02-11 01:19:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
2024-08-25 12:20:29 -03:00
|
|
|
rate_controller_set_rates(rate_decimation, rates, false);
|
2024-02-11 01:19:56 -04:00
|
|
|
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;
|
2024-08-25 12:20:29 -03:00
|
|
|
rate_controller_set_rates(rate_decimation, rates, true);
|
2024-02-11 01:19:56 -04:00
|
|
|
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;
|
|
|
|
|
2024-08-25 12:20:29 -03:00
|
|
|
rate_controller_set_rates(rate_decimation, rates, false);
|
2024-02-11 01:19:56 -04:00
|
|
|
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
|
|
|
|
*/
|
2024-08-25 12:20:29 -03:00
|
|
|
uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high)
|
2024-02-11 01:19:56 -04:00
|
|
|
{
|
|
|
|
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) {
|
2024-08-25 12:20:29 -03:00
|
|
|
rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz
|
2024-02-11 01:19:56 -04:00
|
|
|
} else {
|
2024-08-25 12:20:29 -03:00
|
|
|
rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
|
2024-02-11 01:19:56 -04:00
|
|
|
}
|
2024-08-25 12:20:29 -03:00
|
|
|
rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz
|
2024-02-11 01:19:56 -04:00
|
|
|
#endif
|
2024-08-25 12:20:29 -03:00
|
|
|
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);
|
|
|
|
|
2024-02-11 01:19:56 -04:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2024-10-25 01:04:08 -03:00
|
|
|
// enable the fast rate thread using the provided decimation rate and record the new output rates
|
2024-08-25 12:20:29 -03:00
|
|
|
void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)
|
2024-02-11 01:19:56 -04:00
|
|
|
{
|
|
|
|
ins.enable_fast_rate_buffer();
|
2024-08-25 12:20:29 -03:00
|
|
|
rate_controller_set_rates(rate_decimation, rates, false);
|
2024-02-11 01:19:56 -04:00
|
|
|
hal.rcout->force_trigger_groups(true);
|
|
|
|
using_rate_thread = true;
|
|
|
|
}
|
|
|
|
|
2024-10-25 01:04:08 -03:00
|
|
|
// disable the fast rate thread and record the new output rates
|
2024-08-25 12:20:29 -03:00
|
|
|
void Copter::disable_fast_rate_loop(RateControllerRates& rates)
|
2024-02-11 01:19:56 -04:00
|
|
|
{
|
|
|
|
using_rate_thread = false;
|
2024-08-25 12:20:29 -03:00
|
|
|
uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz());
|
|
|
|
rate_controller_set_rates(rate_decimation, rates, false);
|
2024-02-11 01:19:56 -04:00
|
|
|
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
|