-
Notifications
You must be signed in to change notification settings - Fork 17.4k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Copter: run rate loop at full filter rate in its own thread #27029
base: master
Are you sure you want to change the base?
Conversation
e67b845
to
679611b
Compare
Flow today on top of 4.5 - working well. |
2370654
to
6398bd3
Compare
@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 Currently we have the following: 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:
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. |
cc0198f
to
31fe295
Compare
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 <andrew@tridgell.net>
31fe295
to
f59b5c9
Compare
This is now a 100byte cost on plane, which is mainly due to the small restructuring of INS:
|
{ | ||
return fast_rate_buffer->get_num_gyro_samples(); | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add comments
|
||
uint32_t AP_InertialSensor::get_num_gyro_samples() | ||
{ | ||
return fast_rate_buffer->get_num_gyro_samples(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
check for nullptr?
_notifier.wait_blocking(); | ||
} | ||
|
||
WITH_SEMAPHORE(_mutex); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I always thought this had to be within curly braces or else it was equivalent to having it at the top of the method
@@ -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() && |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add comments
@@ -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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
"This is used by "
@@ -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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add a comment - "don't delete"
AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0), | ||
|
||
// @Param: FSTRATE_DIV | ||
// @DisplayName: Fast rate thread divisor |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is there an easier way?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add the value to the used value
// motors_output from main thread | ||
void Copter::motors_output_main() | ||
{ | ||
if (!using_rate_thread) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
how does this affect servos?
@@ -181,7 +181,19 @@ void Copter::motors_output() | |||
} | |||
|
|||
// push all channels | |||
SRV_Channels::push(); | |||
if (full_push) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Needs a comment
@@ -181,7 +181,19 @@ void Copter::motors_output() | |||
} | |||
|
|||
// push all channels | |||
SRV_Channels::push(); | |||
if (full_push) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
|
||
// set up rate thread requirements | ||
if (!using_rate_thread) { | ||
enable_fast_rate_loop(rate_decimation, rates); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
// @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", |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do this the fast way
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do we actually need this?
return 0; | ||
} | ||
|
||
void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add some comments
motors_output(main_loop_count == 0); | ||
|
||
// process filter updates | ||
if (run_decimated_callback(rates.filter_rate, filter_loop_count)) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should this be called before the motors output or attitude controller?
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:
which gives a variable attitude rate depending on load, and:
which gives an attitude rate locked to the gyro rate, but dynamic while disarmed. For testing there is also:
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 withX_