-
Notifications
You must be signed in to change notification settings - Fork 17.9k
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 #26189
Conversation
5bc2b1b
to
bee6dec
Compare
bee6dec
to
f9a29af
Compare
I had a go at flying this but nearly had a fly away due to frame resonance. My guess would be that the PID notch sample frequency is not being updated correctly with the right values but have not looked in detail at what is going on. |
you're right, it was using the wrong frequency for PID notch, I've fixed it in the PR |
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.
Looks good. I think the main thing for me is that we should call AP_Vehicle::update_dynamic_notch_at_specified_rate() from the rate loop thread since these are all filtering related. Also without this people will not be able to significantly lower their loop rate without losing update rates. We obviously would need some options to control the actual rate.
ArduCopter/Attitude.cpp
Outdated
|
||
while (true) { | ||
// allow changing option at runtime | ||
if (!flight_option_is_set(FlightOptions::USE_RATE_LOOP_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.
I think in the final PR you should make this a boot time option only. It increases the complexity of the code and I'm not sure it really adds a lot of value.
// see if we should have a separate rate thread | ||
if (!started_rate_thread && flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD)) { | ||
if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void), | ||
"rate_controller", |
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.
Please can we shorten the thread name as it gets truncated in @SYS/threads.txt
easy to do, we just need to assess the CPU cost to see how often we can call it reasonably |
I'd actually be curious to test how much this impacts performance - maybe updating the notches at the rate loop rate gives better performance than leaving the notches updating slowly and running the PIDs faster. |
6a37231
to
4d81a6e
Compare
Quick test - now seems to fly fine. Couple of issues:
|
allows the rate thread to trigger pulses
push up RCOutput thread priority to be above main thread
for in-flight impact testing
this better reflects the actual dt between IMU samples
when using the rate thread the PID notches run at a very different sample rate
avoids race condition that leads to motors being unable to switch off
4d81a6e
to
3e9a51b
Compare
|
||
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; |
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.
const float dt = dt_us * 1.0e-6; | |
const float dt = dt_us * 1.0e-6f; |
I tried running autotune on my copter with this change and it completely failed. Very strange noises coming from the motors on twitches. Autotune picked minimum gains for everything. So there is definitely something not right in the implementation that needs to be resolved before I can dig in to whether it makes a worthwhile difference or not. The noise levels look ok, so I am not sure what it could be. I wonder whether its something to do with the notches being updated very fast which leads to cascading shot noise like we have seen in the past. |
So the problem appeared to be extreme jitter in the motor outputs cause by extreme jitter in the SPI reads which never actually got anywhere the required rate (running at about 1.6Khz vs 4Khz), so I think that made all the maths be messed up. This was ultimately caused by the SPI thread not having a high enough priority and the device scheduler introducing arbitrary delays. I have made some changes to resolve these and the branch is here - https://github.com/andyp1per/ardupilot/tree/pr-fast-rate-thread (includes a rebase). The output now looks pretty regular at 4Khz. Too wet to fly, but indoor testing looks promising. |
Did a quick flight test outside - certainly the grinding noises are all but gone. Will need to see if I can successfully do an autotune but the log looks promising |
@andyp1per interesting! hope a full flight test goes well |
Still wouldn't tune. Its certainly a lot better, but still something strange going on. I also tried using the actual dt rather than the average and had a flyaway - would barely respond to RC input. There is a real danger of thread starvation of the main loop with the higher priority threads which is what I just saw 😮 |
@lthall has pointed out that autotune is not zeroing D_FF (which I will fix) and the target is showing spurious changes: |
replaced by #27029 |
This adds FLIGHT_OPTIONS=8 which creates a rate loop thread, allowing rate updates at the full filtered gyro rate (2kHz on most copters), settable with existing INS_GYRO_RATE parameter.
This was test flown successfully on a 5" quad from @MichelleRos, the rate loop did run at 2kHz (I added a send text for average rate every 2s)
We've now had successful flights at 2kHz, 4kHz and 8kHz on a MatekH743-bdshot 5" quad. At 8kHz we had to disable the 2nd IMU with IMU_ENABLE_MASK or we had R/C issues