Copter: run rate loop at full filter rate in its own thread #27029

andyp1per · 2024-05-09T20:52:46Z

This is a redo of #26189

I have squashed the commits, rebased and started fixing the underlying problems. There were some fundamental problems with how the original PR was handling attitude control changes so I thought it was better to just open a new PR.

Support is enabled by setting:

FSTRATE_ENABLE = 1

which gives a variable attitude rate depending on load, and:

FSTRATE_ENABLE = 2

which gives an attitude rate locked to the gyro rate, but dynamic while disarmed. For testing there is also:

FSTRATE_ENABLE = 3

which is a locked rate at all times.

The output rate can be adjusted using FSTRATE_DIV which is a gyro divisor (default 1).
Two different rates - and therefore tunes - can be compared by using the lua applet switch_rates.lua which will persistently switch the appropriate tune/rate parameters with ones saved in names beginning with X_

ArduCopter/Attitude.cpp

libraries/AC_PID/AC_PID.cpp

andyp1per · 2024-05-25T17:40:21Z

Flow today on top of 4.5 - working well.

andyp1per · 2024-06-09T11:06:15Z

@rmackay9 this is close to being ready and I could use your input on what the configuration should look like. The basic premise is that we run the rate controller at the same rate as the gyros which is controlled by INS_GYRO_RATE (0=1Khz,1=2Khz,2=4Khz,3=8Khz). However, this is quite expensive and will not be possible on some hardware setups. In addition we need to be able to log the rate outputs at a faster rate than the main loop rate, but its unlikely that we could successfully log at the gyro rate. Finally we need to update the filters (notch filter in particular) at a faster rate than the main loop, but it does not make sense to do this any faster than at half the gyro rate.

Currently we have the following:
FLIGHT_OPTIONS=8 - enables the rate thread and dynamically schedules the relationship to the gyro rate based on load
FLIGHT_OPTIONS=16 - fixes the rate thread at the same rate as the gyro rate
ATT_FILTER_RATE - update rate of the filters in Hz or fixed at half the gyro rate if set to 0
ATT_LOG_RATE - update rate of the rate logs in Hz or set at the main loop rate if set to 0
The last two are not exact numbers - they are just the closest we can get to the required frequency by counting gyro samples so need to be an integer divisor or the gyro rate.

I'm not currently convinced about the naming or the configuration. I think this is an important enough feature that it needs its own enable flag. I also think it would be worth having a rate parameter that allows you to fix the rate. I also think that ATT is a little bit misleading. Maybe FAST_RATE_ or RATET_ or something. So an alternative might be:

FRATE_ENABLE - set to 1 to enable the rate thread
FRATE_RATE - set to 0 to dynamically throttle, fixed Hz otherwise
FRATE_LOG_RATE - as above
FRATE_FILT_RATE - as above

or something. It would also be possible to not use Hz at all but simply pick the divisor. So 2 would be gyro rate / 2 etc.
Your thoughts appreciated on this important topic!

ArduCopter/Attitude.cpp

…sable for scripting

restrict rate loop to H7 and F7

honour the requested dshot rate as near as possible

…tions.py and extract_features.py

…n non-copter builds

…hread decimate the gyro window locally configure rate loop buffer based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED allow backends to be updated from rate thread output debug error if rate loop buffer overruns add support for updating filter parameters independently of propagating samples add rate loop config abstraction that allows code to be elided on non-copter builds must be using harmonic notch to use rate thread mediate fast rate loop buffer using mutex and binary semaphore ensure gyro samples are used when the rate loop buffer isn't Co-Authored-By: Andrew Tridgell <[email protected]>

andyp1per · 2024-09-27T14:33:13Z

This is now a 100byte cost on plane, which is mainly due to the small restructuring of INS:

Binary Name      Text [B]         Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  ---------------  -----------  -------------  ----------------------------  -------------------------
antennatracker   104 (+0.0077%)   0 (0.0000%)  0 (0.0000%)    104 (+0.0077%)                                   621084
arducopter       3400 (+0.1877%)  0 (0.0000%)  0 (0.0000%)    3400 (+0.1872%)                                  146872
ardusub          104 (+0.0065%)   0 (0.0000%)  0 (0.0000%)    104 (+0.0065%)                                   359460
blimp            104 (+0.0076%)   0 (0.0000%)  0 (0.0000%)    104 (+0.0076%)                                   599032
arduplane        116 (+0.0064%)   0 (0.0000%)  4 (+0.0015%)   116 (+0.0064%)                                   152832
ardurover        108 (+0.0065%)   0 (0.0000%)  -4 (-0.0015%)  108 (+0.0065%)                                   302264
arducopter-heli  3404 (+0.1880%)  0 (0.0000%)  -4 (-0.0015%)  3404 (+0.1876%)                                  147768

andyp1per · 2024-10-07T09:07:30Z

libraries/AP_InertialSensor/FastRateBuffer.cpp

+{
+ return fast_rate_buffer->get_num_gyro_samples();
+}
+


add comments

andyp1per · 2024-10-07T09:07:49Z

libraries/AP_InertialSensor/FastRateBuffer.cpp

+
+uint32_t AP_InertialSensor::get_num_gyro_samples()
+{
+ return fast_rate_buffer->get_num_gyro_samples();


check for nullptr?

rmackay9 · 2024-10-07T09:12:11Z

libraries/AP_InertialSensor/FastRateBuffer.cpp

+ _notifier.wait_blocking();
+ }
+
+ WITH_SEMAPHORE(_mutex);


I always thought this had to be within curly braces or else it was equivalent to having it at the top of the method

andyp1per · 2024-10-07T09:19:53Z

libraries/AP_HAL_ChibiOS/RCOutput.cpp

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


add comments

andyp1per · 2024-10-07T09:20:41Z

libraries/AP_HAL_ChibiOS/RCOutput.h

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


"This is used by "

andyp1per · 2024-10-07T09:23:26Z

ArduCopter/Copter.cpp

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


add a comment - "don't delete"

andyp1per · 2024-10-07T09:28:58Z

ArduCopter/Parameters.cpp

+ AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0),
+
+ // @Param: FSTRATE_DIV
+ // @DisplayName: Fast rate thread divisor


Is there an easier way?

add the value to the used value

andyp1per · 2024-10-07T09:36:02Z

ArduCopter/motors.cpp

+// motors_output from main thread
+void Copter::motors_output_main()
+{
+ if (!using_rate_thread) {


how does this affect servos?

andyp1per · 2024-10-07T09:37:53Z

ArduCopter/motors.cpp

@@ -181,7 +181,19 @@ void Copter::motors_output()
 }

 // push all channels
- SRV_Channels::push();
+ if (full_push) {


Needs a comment

rmackay9 · 2024-10-07T09:40:38Z

ArduCopter/motors.cpp

@@ -181,7 +181,19 @@ void Copter::motors_output()
 }

 // push all channels
- SRV_Channels::push();
+ if (full_push) {


It's a shame that the copter code needs to worry about the detail of which push() to call. The copter code essentially knows that the SRV_Channels cannot do what we want it to. It would be nicer if the SRV_Channel::push() call could figure that out itself somehow

rmackay9 · 2024-10-07T09:47:34Z

ArduCopter/rate_thread.cpp

+
+ // set up rate thread requirements
+ if (!using_rate_thread) {
+ enable_fast_rate_loop(rate_decimation, rates);


not a blocker, feel free to resolve immediately but we need to be careful that we don't somehow end up in a loop where we continuously delete and re-create the buffer.

andyp1per · 2024-10-07T09:48:26Z

ArduCopter/rate_thread.cpp

+// @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",


Do this the fast way

Do we actually need this?

andyp1per · 2024-10-07T10:02:36Z

ArduCopter/rate_thread.cpp

+ return 0;
+}
+
+void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)


add some comments

andyp1per · 2024-10-07T10:10:41Z

ArduCopter/rate_thread.cpp

+ motors_output(main_loop_count == 0);
+
+ // process filter updates
+ if (run_decimated_callback(rates.filter_rate, filter_loop_count)) {


Should this be called before the motors output or attitude controller?

IamPete1 reviewed May 9, 2024

View reviewed changes

ArduCopter/Attitude.cpp Outdated Show resolved Hide resolved

libraries/AC_PID/AC_PID.cpp Outdated Show resolved Hide resolved

andyp1per added the WIP label May 10, 2024

andyp1per force-pushed the pr-fast-rate-thread branch 6 times, most recently from e67b845 to 679611b Compare May 22, 2024 20:40

andyp1per added Enhancement Copter and removed WIP labels May 25, 2024

andyp1per mentioned this pull request May 26, 2024

Run rate controller at full gyro rate #7613

Open

andyp1per linked an issue May 26, 2024 that may be closed by this pull request

Run rate controller at full gyro rate #7613

Open

andyp1per force-pushed the pr-fast-rate-thread branch 13 times, most recently from 2370654 to 6398bd3 Compare June 7, 2024 19:29

andyp1per added the DevCallEU label Jun 11, 2024

IamPete1 reviewed Jun 16, 2024

View reviewed changes

ArduCopter/Attitude.cpp Outdated Show resolved Hide resolved

andyp1per and others added 5 commits September 26, 2024 11:27

autotest: add rate thread autotest

ca90b8f

AP_AHRS: correct get_accel() to use primary accel rather than first u…

621d017

…sable for scripting

AP_Vehicle: use update_dynamic_notch() directly in rate loop

82eee36

AP_HAL: allow forcing of trigger_groups()

bec42eb

AP_HAL: set HAL_INS_RATE_LOOP in boards

d8a3d16

restrict rate loop to H7 and F7

andyp1per force-pushed the pr-fast-rate-thread branch from cc0198f to 31fe295 Compare September 26, 2024 10:28

andyp1per and others added 7 commits September 26, 2024 17:13

AP_HAL_ChibiOS: allow forcing of trigger_groups()

04f18ef

AP_HAL_ChibiOS: compile MambaH743v4 at -O2

252256e

AP_HAL_ChibiOS: ensure dshot rate can be set dynamically

20d9f5b

honour the requested dshot rate as near as possible

scripts: add AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED to build_op…

d68b1bf

…tions.py and extract_features.py

waf: add rate loop config abstraction that allows code to be elided o…

e7e6819

…n non-copter builds

Copter: correctly set fast rate thread rates

c97d76a

andyp1per force-pushed the pr-fast-rate-thread branch from 31fe295 to f59b5c9 Compare September 26, 2024 16:14

Copter: ensure decimated rates are never 0 in rate thread

25e2a6b

andyp1per commented Oct 7, 2024

View reviewed changes

libraries/AP_InertialSensor/FastRateBuffer.cpp

{

return fast_rate_buffer->get_num_gyro_samples();

}

Copy link

Collaborator Author

andyp1per Oct 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add comments

andyp1per commented Oct 7, 2024

View reviewed changes

rmackay9 reviewed Oct 7, 2024

View reviewed changes

andyp1per commented Oct 7, 2024

View reviewed changes

rmackay9 reviewed Oct 7, 2024

View reviewed changes

andyp1per commented Oct 7, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Copter: run rate loop at full filter rate in its own thread #27029

Copter: run rate loop at full filter rate in its own thread #27029

andyp1per commented May 9, 2024 •

edited

Loading

andyp1per commented May 25, 2024

andyp1per commented Jun 9, 2024 •

edited

Loading

andyp1per commented Sep 27, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

rmackay9 Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

rmackay9 Oct 7, 2024

rmackay9 Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

andyp1per Oct 7, 2024

Copter: run rate loop at full filter rate in its own thread #27029

Are you sure you want to change the base?

Copter: run rate loop at full filter rate in its own thread #27029

Conversation

andyp1per commented May 9, 2024 • edited Loading

andyp1per commented May 25, 2024

andyp1per commented Jun 9, 2024 • edited Loading

andyp1per commented Sep 27, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andyp1per commented May 9, 2024 •

edited

Loading

andyp1per commented Jun 9, 2024 •

edited

Loading