Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: correctly set fast rate thread rates

Browse Source
This commit is contained in:
Andy Piper 2024-08-25 16:20:29 +01:00 committed by Andrew Tridgell
parent 21106380b3
commit dcf25200c6
2 changed files with 50 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/Copter.h
View File

@ -742,13 +742,20 @@ private:
void run_rate_controller_main(); void run_rate_controller_main();
// if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
uint8_t calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz); struct RateControllerRates {
uint8_t fast_logging_rate;
uint8_t medium_logging_rate;
uint8_t filter_rate;
uint8_t main_loop_rate;
};
uint8_t calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz);
void rate_controller_thread(); void rate_controller_thread();
void rate_controller_filter_update(); void rate_controller_filter_update();
void rate_controller_log_update(); void rate_controller_log_update();
uint8_t rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high); 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); void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates);
void disable_fast_rate_loop(); void disable_fast_rate_loop(RateControllerRates& rates);
void update_dynamic_notch_at_specified_rate_main(); void update_dynamic_notch_at_specified_rate_main();
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED

63
ArduCopter/rate_thread.cpp
View File

@ -150,11 +150,16 @@
#define DIV_ROUND_INT(x, d) ((x + d/2) / d) #define DIV_ROUND_INT(x, d) ((x + d/2) / d)
uint8_t Copter::calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz) uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz)
{ {
return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz));
} }
static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count)
{
return decimation_rate > 0 && ++decimation_count >= decimation_rate;
}
//#define RATE_LOOP_TIMING_DEBUG //#define RATE_LOOP_TIMING_DEBUG
/* /*
thread for rate control thread for rate control
@ -163,6 +168,11 @@ void Copter::rate_controller_thread()
{ {
uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400)); uint8_t 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; 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 rate_loop_count = 0;
uint32_t prev_loop_count = 0; uint32_t prev_loop_count = 0;
@ -191,13 +201,9 @@ void Copter::rate_controller_thread()
#endif #endif
// run the filters at half the gyro rate // 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 #if HAL_LOGGING_ENABLED
uint8_t log_med_rate_decimate = calc_gyro_decimation(rate_decimation, 10); // 10Hz
uint8_t log_loop_count = 0; uint8_t log_loop_count = 0;
#endif #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 main_loop_count = 0;
uint8_t filter_loop_count = 0; uint8_t filter_loop_count = 0;
@ -210,7 +216,7 @@ void Copter::rate_controller_thread()
// allow changing option at runtime // allow changing option at runtime
if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) { if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) {
if (was_using_rate_thread) { if (was_using_rate_thread) {
disable_fast_rate_loop(); disable_fast_rate_loop(rates);
was_using_rate_thread = false; was_using_rate_thread = false;
} }
hal.scheduler->delay_microseconds(500); hal.scheduler->delay_microseconds(500);
@ -220,7 +226,7 @@ void Copter::rate_controller_thread()
// set up rate thread requirements // set up rate thread requirements
if (!using_rate_thread) { if (!using_rate_thread) {
enable_fast_rate_loop(rate_decimation); enable_fast_rate_loop(rate_decimation, rates);
} }
ins.set_rate_decimation(rate_decimation); ins.set_rate_decimation(rate_decimation);
@ -278,7 +284,7 @@ void Copter::rate_controller_thread()
// run the rate controller on all available samples // run the rate controller on all available samples
// it is important not to drop samples otherwise the filtering will be fubar // 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 // there is no need to output to the motors more than once for every batch of samples
attitude_control->rate_controller_run_dt(sensor_dt, gyro + ahrs.get_gyro_drift()); attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt);
#ifdef RATE_LOOP_TIMING_DEBUG #ifdef RATE_LOOP_TIMING_DEBUG
rate_controller_time_us += AP_HAL::micros() - rate_now_us; rate_controller_time_us += AP_HAL::micros() - rate_now_us;
@ -286,14 +292,15 @@ void Copter::rate_controller_thread()
#endif #endif
// immediately output the new motor values // immediately output the new motor values
if (main_loop_count++ >= main_loop_rate_decimate) { if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) {
main_loop_count = 0; main_loop_count = 0;
} }
motors_output(main_loop_count == 0); motors_output(main_loop_count == 0);
// process filter updates // process filter updates
if (filter_loop_count++ >= filter_rate_decimate) { if (run_decimated_callback(rates.filter_rate, filter_loop_count)) {
filter_loop_count = 0; filter_loop_count = 0;
rate_controller_filter_update(); rate_controller_filter_update();
} }
@ -305,18 +312,17 @@ void Copter::rate_controller_thread()
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
// fast logging output // fast logging output
if (should_log(MASK_LOG_ATTITUDE_FAST)) { if (should_log(MASK_LOG_ATTITUDE_FAST)) {
if (log_fast_rate_decimate > 0 && log_loop_count++ >= log_fast_rate_decimate) { if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) {
log_loop_count = 0; log_loop_count = 0;
rate_controller_log_update(); rate_controller_log_update();
} }
} else if (should_log(MASK_LOG_ATTITUDE_MED)) { } else if (should_log(MASK_LOG_ATTITUDE_MED)) {
if (log_med_rate_decimate > 0 && log_loop_count++ >= log_med_rate_decimate) { if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) {
log_loop_count = 0; log_loop_count = 0;
rate_controller_log_update(); rate_controller_log_update();
} }
} }
#else
(void)log_fast_rate_decimate;
#endif #endif
#ifdef RATE_LOOP_TIMING_DEBUG #ifdef RATE_LOOP_TIMING_DEBUG
@ -333,6 +339,10 @@ void Copter::rate_controller_thread()
// enabled at runtime // enabled at runtime
last_notch_sample_ms = now_ms; last_notch_sample_ms = now_ms;
attitude_control->set_notch_sample_rate(1.0 / sensor_dt); 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 // interlock for printing fixed rate active
@ -346,7 +356,7 @@ void Copter::rate_controller_thread()
&& ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed()) && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed())
|| get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) { || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) {
rate_decimation = target_rate_decimation; rate_decimation = target_rate_decimation;
log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); rate_controller_set_rates(rate_decimation, rates, false);
notify_fixed_rate_active = false; notify_fixed_rate_active = false;
} }
@ -366,7 +376,7 @@ void Copter::rate_controller_thread()
const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation; 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()) { if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) {
rate_decimation = new_rate_decimation; rate_decimation = new_rate_decimation;
log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, true); rate_controller_set_rates(rate_decimation, rates, true);
prev_loop_count = rate_loop_count; prev_loop_count = rate_loop_count;
rate_loop_count = 0; rate_loop_count = 0;
running_slow = 0; running_slow = 0;
@ -377,7 +387,7 @@ void Copter::rate_controller_thread()
|| now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry
rate_decimation = rate_decimation - 1; rate_decimation = rate_decimation - 1;
log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); rate_controller_set_rates(rate_decimation, rates, false);
prev_loop_count = 0; prev_loop_count = 0;
rate_loop_count = 0; rate_loop_count = 0;
last_rate_increase_ms = now_ms; last_rate_increase_ms = now_ms;
@ -420,7 +430,7 @@ void Copter::rate_controller_filter_update()
/* /*
update rate controller rates and return the logging rate update rate controller rates and return the logging rate
*/ */
uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high) uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high)
{ {
const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation; const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation;
attitude_control->set_notch_sample_rate(attitude_rate); attitude_control->set_notch_sample_rate(attitude_rate);
@ -431,26 +441,31 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu
warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate); warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate);
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
if (attitude_rate > 1000) { if (attitude_rate > 1000) {
return calc_gyro_decimation(rate_decimation, 1000); rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz
} else { } else {
return calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
} }
rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz
#endif #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; return 0;
} }
void Copter::enable_fast_rate_loop(uint8_t rate_decimation) void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)
{ {
ins.enable_fast_rate_buffer(); ins.enable_fast_rate_buffer();
rate_controller_set_rates(rate_decimation, false); rate_controller_set_rates(rate_decimation, rates, false);
hal.rcout->force_trigger_groups(true); hal.rcout->force_trigger_groups(true);
using_rate_thread = true; using_rate_thread = true;
} }
void Copter::disable_fast_rate_loop() void Copter::disable_fast_rate_loop(RateControllerRates& rates)
{ {
using_rate_thread = false; using_rate_thread = false;
rate_controller_set_rates(calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()), false); uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz());
rate_controller_set_rates(rate_decimation, rates, false);
hal.rcout->force_trigger_groups(false); hal.rcout->force_trigger_groups(false);
ins.disable_fast_rate_buffer(); ins.disable_fast_rate_buffer();
} }