Skip to content
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

In-flight dynamic FFT analysis #11886

Open
wants to merge 13 commits into
base: master
from

Conversation

@andyp1per
Copy link
Contributor

commented Jul 26, 2019

This is a port of the betaflight dynamic FFT analysis that they use to dynamically adjust filter frequencies in flight. This port currently only exposes the analysis as a log message allowing the user to see the calculated motor peak frequency via the FTUN log message. The betaflight implementation was flawed in many ways and this implementation is substantially better and more configurable. The features are fully optional and can be controlled through hwdef. I have tested this on a Pixracer and KakuteF7. I have changed the build flags to appropriately optimize for the available target MCU.

The analyzer is controlled via the variables:

FFT_ENABLE - set to 1 to enable
FFT_MINHZ - the minimum frequency for analysis
FFT_MAXHZ - the maximum frequency for analysis
FFT_SAMPLE_MODE - the desired sample frequency, 0 for gyro rate, 1 for fastloop rate 2 for fastloop/ 2 etc
FFT_WINDOW_SIZE - the FFT window size, large values give more resolution but higher CPU load
FFT_WINDOW_OLAP - the FFT window overlap percentage for each analyzed frame
FFT_FREQ_HOVER - the learned hover frequency
FFT_THR_REF - the learned throttle ref value for harmonic notches (see HNTCH_THR_REF)
FFT_SNR_REF - the SNR level in dB at which the FFT energy is above the calibrated noise to be considered a signal
FFT_ATT_REF - attenuation value in dB to use for calculating bandwidth
FTT_BW_HOVER - learned bandwidth in Hz at hover
INS_HNTCH_MODE=2 will drive the harmonic notch using the FFT support

The analyser uses the ARM CMSIS DSP library to do the calculations, it will only work on ChibiOS on M4 and M7 chips, which I believe all AP supported chips are. To update the CMSIS module you must have git-lfs installed. https://help.github.com/en/articles/installing-git-large-file-storage

@amilcarlucas amilcarlucas requested a review from tridge Jul 26, 2019
@amilcarlucas

This comment has been minimized.

Copy link
Contributor

commented Jul 26, 2019

Wasn't there a CMSIS license issue in the past ?

@proficnc

This comment has been minimized.

Copy link
Contributor

commented Jul 27, 2019

Assuming license issues are sorted out, this looks great!
Can this also be applied to vibration FFT?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 27, 2019

Assuming license issues are sorted out, this looks great!
Can this also be applied to vibration FFT?

I'm not following - this is designed to capture the main vibration frequency?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 27, 2019

This won't work on Linux boards - of which we support several. I'll add defines to make it an optional compile-time feature.

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 27, 2019

Wasn't there a CMSIS license issue in the past ?

CMSIS 5 is ASL:

https://github.com/ARM-software/CMSIS_5/blob/develop/LICENSE.txt

So no issue as far as I can see.

@auturgy

This comment has been minimized.

Copy link
Contributor

commented Jul 27, 2019

The license issue related to the Liberty license that the STM Standard Peripherals Library uses, not the ARM CMSIS (which is generally packaged with the SPL, but is not under the ST license). CMSIS under Apache 2 is compatible with GPLv3+

@khancyr

This comment has been minimized.

Copy link
Contributor

commented Jul 27, 2019

Thanks for that !
About the implementation, from fast reading it will need some rework.
We should use the stdint type instead of default int type etc.
About the arm stuff, instead of using a define, a better option would be to hide arm stuff under hal with a fft_acceleration class or something like that. That would allow use to use material acceleration on different board type more cleanly.

Why do we need git lfs?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 27, 2019

Thanks for that !
About the implementation, from fast reading it will need some rework.
We should use the stdint type instead of default int type etc.

That's fine - I already did a fair amount of rework to AP standards, but expect some more. I'm not 100% convinced it is working properly as yet hence the request for some help from @peterbarker

About the arm stuff, instead of using a define, a better option would be to hide arm stuff under hal with a fft_acceleration class or something like that. That would allow use to use material acceleration on different board type more cleanly.

I guess I was trying to keep the implementation light touch for now. Happy to go the HAL route if that's the right way. I can only ever see this being used on Copter though...

Why do we need git lfs?

The CMSIS module uses it to store the binaries. I guess its more efficient for large files. If you sync without it you get garbage.

@bnsgeyer

This comment has been minimized.

Copy link
Contributor

commented Jul 28, 2019

@andyp1per This is some really good stuff. I'm not sure that you saw the work i was trying to do with the RPM library. I was using FFT analysis determine the rotor speed for a helicopter based on the 1/rev. The PR is here. I pulled a subset of the CMSIS library and incorporated it as a library in the AP_Math. I had gone with a deprecated version of the code using cfft_radix4_f32. I had tried to pull over the arm_rfft_fast_f32 but got tripped up on a assembly language version of the bit_reversal routine that the new code used. After I had struggled for a week and finally got one of the earlier version running, I had consulted @tridge and he said that they have incorporated assembly language code in the past. Anyway, was wondering how you were able to get it to compile properly. I probably couldn't understand that anyway. Just wondering if you saw any issues compiling since it is using an assembly language bit reversal code in the function you are using?

I assume if you incorporate this as an external library, other parts of the code (like the RPM library) could use it.

You mentioned that your weren't sure if it was working properly. When I was working through the code, trying to understand the steps it was taking, I found it curious that it could take N real samples and use the complex FFT which takes an input of 2*N because it includes the imaginary part in the array. However the code pushes only the N real samples as the real + imaginary array to the complex FFT solver. Thus the complex solver sees the fft length as N/2. So the freq resolution of the complex sover should be sample freq /(N/2-1) but the real fft freq resolution comes out as sample freq/(N-1). I am not a DSP guru so maybe there is more to it that I don't understand.

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 29, 2019

@bnsgeyer thanks - no secret insights here I am afraid - I merely copied the beta flight code, imported the CMISIS binaries as part of the module (so no compilation required) and fixed it up to work in AP. I've not looked into the actual implementation in a massive amount of detail, but do note in passing that they are using some undocumented sub-functions I presume so as to break the FFT analysis into smaller chunks and allow it to run inside a fast gyro loop. What I really need to do is buy a cheap F4 board and one of the vibration speakers that @priseborough recommended and do some proper testing. It gives out values that seem reasonable correct but vary a lot over time.

@bnsgeyer

This comment has been minimized.

Copy link
Contributor

commented Jul 29, 2019

@andyp1per is there any reason why this code won’t run in the SITL? If It does, why not just overlay a sinusoid of a specific frequency on top of a signal you are feeding it and look at the results?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 29, 2019

@andyp1per is there any reason why this code won’t run in the SITL? If It does, why not just overlay a sinusoid of a specific frequency on top of a signal you are feeding it and look at the results?

@bnsgeyer I believe these functions use built-in features of the ARM processor so its not possible to emulate in SITL. That's why they are fast and efficient.

@bnsgeyer

This comment has been minimized.

Copy link
Contributor

commented Jul 29, 2019

@andyp1per So what boards are capable of running this? Pixhawk series?

In any case, you could still artificially add a sinusoidal waveform to the signal before processing to check the output. you would just have to flash it on the board to do the test. just a thought.

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 29, 2019

@@ -156,6 +156,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
SCHED_TASK_CLASS(Analyse_Noise, &copter.analyse_noise, periodic, 400, 50),
SCHED_TASK_CLASS(Analyse_Noise, &copter.analyse_noise, analyse, 400, 100),

This comment has been minimized.

Copy link
@bnsgeyer

bnsgeyer Jul 29, 2019

Contributor

@andyp1per the periodic task is being called at 400hz however you allow users to specify sample frequency up to 1000hz. How are you able to do this if you are adding samples to your buffer at a rate of 400hz?

This comment has been minimized.

Copy link
@andyp1per

andyp1per Jul 29, 2019

Author Contributor

The sample frequency is actually limited to the loop rate. This is in the unlikely event you want it lower, but I'm certain this parameter is not correct as of yet.

This comment has been minimized.

Copy link
@bnsgeyer

bnsgeyer Jul 29, 2019

Contributor

I still don’t get it. If you are scheduling it for 400 Hz how can you be getting it at the loop rate of the fast loop. I would think that it would have to be in the fast loop to be then scheduled according to what you specify as your fast loop rate via the parameter.

}

void Analyse_Noise::push_sample(const Vector3f& sample)
{

This comment has been minimized.

Copy link
@bnsgeyer

bnsgeyer Jul 29, 2019

Contributor

@andyp1per this is the function which is called by periodic (called at 400 hz from the main loop). these are single samples right? so you are only able to collect data at a maximum of 400 hz?

This comment has been minimized.

Copy link
@andyp1per

andyp1per Jul 29, 2019

Author Contributor

Well the loop rate which is 800Hz in my case. If I can get it producing results that look sane I might think about integrating with the gyro loop - but even then that's basically a max rate of 1000Hz (even with fast sampling on - we average samples to the backend rate), so of dubious benefit.

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 30, 2019

@guglie

This comment has been minimized.

Copy link
Contributor

commented Jul 30, 2019

Hi @bnsgeyer, as I told to @andyp1per, to compile you just need to

  • add this submodule https://github.com/ARM-software/CMSIS_5
  • include CMSIS/DSP/Include/arm_math.h
  • add '-llibarm_cortexM4lf_math', '-DARM_MATH_CM4' to env.CXXFLAGS in class chibios of boards.py
  • add the following to chibios.py
bld.env.LIB += ['ch', 'arm_cortexM4lf_math']
bld.env.LIBPATH += ['modules/ChibiOS/', '<path to CMSIS module>/CMSIS/DSP/Lib/GCC/']

this for an M4, CMSIS support also M0, M3, and M7, but we need some configuration rule to make it portable to the other ARM supported.

Unfortunately this also means that you can't use this library in SITL... maybe HITL?

Discuss thread here

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 30, 2019

@guglie

This comment has been minimized.

Copy link
Contributor

commented Jul 30, 2019

@andyp1per right, I was already using that but I forgot to update the paths in my notes 😃
(paths updated above)

@bnsgeyer

This comment has been minimized.

Copy link
Contributor

commented Jul 30, 2019

You are right and the problem is Nyquist!

@andyp1per So you agree that you need to put the periodic function in the fast loop. that way, it is sampling at the fast loop and your FFT would go to half the fast loop rate (nyquist). the other way to set up the sampling is an interrupt request (i think that what it was) like @tridge did for me in the RPM estimator that I was working on. here

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Jul 30, 2019

@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 1292bdf to 3ea2dc1 Aug 3, 2019
@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Aug 3, 2019

@bnsgeyer yeah that makes all the difference, looks like it's working now:

image

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Aug 8, 2019

This is working well now. I have constructed a little test rig using a KakuteF7 and a vibration speaker as recommended by @priseborough

IMG_1576

I am able to drive clear signals through the gyros and the results are very pleasing:

image

I added some self tests to make sure the full frequency range can be detected - and it can, but there is some filtering or something going on that makes frequencies above about 325Hz disappear. Am going to investigate to see if this is the hardware filter on the gyro (ICM 20689)

@bnsgeyer

This comment has been minimized.

Copy link
Contributor

commented Aug 8, 2019

@andyp1per why aren’t you getting any readings below 100? Shouldn’t the readings be zero if there is no input? I guess my big question is will this pick up frequencies down to 20 Hz? Most of our first rotor modes occur between 20 and 30 hz.

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Aug 8, 2019

@andyp1per why aren’t you getting any readings below 100? Shouldn’t the readings be zero if there is no input? I guess my big question is will this pick up frequencies down to 20 Hz? Most of our first rotor modes occur between 20 and 30 hz.

Because the MINHZ value defaults to 80. I'm happy to lower this and rerun the tests if you would be interested? It's written like this because the BF code targets the notch frequency which you don't want to go below a certain frequency.

But you may have problems down at 20Hz. The default resolution is 25Hz bins and to get better than that you have to increase the FFT window size which means more processing power. I think this is probably doable by increasing the number of steps (which means a slower update rate - but you probably don't care for Heli), but would require some careful coding to avoid lots of compile time problems (or else provide different defaults for Heli I suppose).

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Aug 9, 2019

@khancyr my last 3 commits create a HAL implementation of the guts of this for ChibiOS. Before I go too far down this refactoring can you tell me whether this is what you had in mind?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Aug 10, 2019

@bnsgeyer turns out the 100Hz is actually the electrical noise on the gyros. I've made a change to measure this at startup and then only record the frequency when above this reference and it works great! Now can detect down to about 50Hz which is close to the resolution of the FFT window that I am using. Will have to play a little with the numbers - I suspect I could go to twice the reference to get a cleaner lower bound.

@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 28a2055 to 62f77a9 Sep 27, 2019
@amilcarlucas

This comment has been minimized.

Copy link
Contributor

commented Oct 2, 2019

This needs a rebase.

@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 62f77a9 to 394947f Oct 2, 2019
@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Oct 2, 2019

This needs a rebase.

Done!

@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 394947f to 86ff65b Oct 10, 2019
@amilcarlucas

This comment has been minimized.

Copy link
Contributor

commented Oct 10, 2019

Are you planning to merge this in for Arducopter 4.0 ?

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Oct 10, 2019

Are you planning to merge this in for Arducopter 4.0 ?

It would be great if that were possible, but it's not up to me.

@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 622e252 to 94fea62 Oct 10, 2019
@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Oct 10, 2019

@tridge I have re-based, squashed and added the dynamic notch support

Copy link
Contributor

left a comment

The last commit can be squashed into the first.

Copy link
Contributor

left a comment

I put some comments. Most a just style or really small performance improvement (mostly change some float divide by multiplication).
But there is surely and issue with a define
I am really looking to test this ! thanks for your hard work !

libraries/AP_HAL/DSP.cpp Outdated Show resolved Hide resolved
libraries/AP_HAL/DSP.cpp Outdated Show resolved Hide resolved
libraries/AP_HAL/DSP.cpp Outdated Show resolved Hide resolved
libraries/AP_HAL/DSP.cpp Show resolved Hide resolved
libraries/AP_GyroFFT/AP_GyroFFT.cpp Outdated Show resolved Hide resolved
libraries/AP_HAL_Empty/DSP.h Outdated Show resolved Hide resolved
libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h Outdated Show resolved Hide resolved
libraries/AP_HAL_SITL/DSP.cpp Show resolved Hide resolved
libraries/AP_HAL_SITL/DSP.cpp Outdated Show resolved Hide resolved
andyp1per added 4 commits Jul 18, 2019
…expose as a log message

add arming checks to validate FFT performance
allow gyros to be sample at either the fastloop rate or gyro rate.
add update and parameter update loops for GyroFFT
add GYRO_FFT aux function
add harmonic notch tracking mode
…on of FFT based on HAL_WITH_DSP and GYROFFT_ENABLED. target appropriate ARM cpus

autotest for Gyro FFT
…lysis and allow sampling to gyro window for FFT analysis.

FFT windows can be dynamically allocated
add harmonic notch dynamic tracking mode
@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 364f44e to 9a51c41 Oct 11, 2019
andyp1per added 9 commits Aug 9, 2019
control inclusion of FFT based on HAL_WITH_DSP and GYROFFT_ENABLED. target appropriate ARM cpus
define hanning window and quinn's estimator
…L FFT abstraction

implements an FFT state machine based on the betaflight feature using ARM hardware accelerated CMSIS library
make the FFT feature optional
add dynamic gyro windows
split FFT state machine based on window size
add quinns and candans estimators and record in DSP state
disable DSP for boards with limited flash
calculate power spectrum rather than amplitude
…traction

add dynamic gyro windows
control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED
target appropriate ARM cpus
constrain window sizes to be achievable
improve FFT signal accuracy through configurable window overlap and quinn's estimator
calculate energy weighted center frequency
add support for learning hover frequency and throttle reference
make provision for timing cycles and tune allowable window sizes
calculate power spectrum rather than amplitude
record noise as a per-bin power spectrum
calculate true SNR per-bin and use that to determine there is a signal
add user config for SNR signal level
constrain frequency scanning to MAXHZ
calculate and learn the peak bandwidth at the configured attenuation
allow enabling/disabling dynamically
MAXHZ should be below Nyquist
Incorporate full range of MAXHZ to MINHZ
@andyp1per andyp1per force-pushed the andyp1per:pr-tune-fft branch from 9a51c41 to b177f0a Oct 11, 2019
@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Oct 11, 2019

The last commit can be squashed into the first.

Thanks fixed @amilcarlucas

@BaptisteEspivent

This comment has been minimized.

Copy link

commented Oct 14, 2019

@andyp1per , really nice work! I intend to work on vibration filtering as well but I don't have a test bench yet. Would you have some recommandation on the setup you used? Thank you very much!

@andyp1per

This comment has been minimized.

Copy link
Contributor Author

commented Oct 14, 2019

@andyp1per , really nice work! I intend to work on vibration filtering as well but I don't have a test bench yet. Would you have some recommandation on the setup you used? Thank you very much!

I used these:
https://www.amazon.co.uk/gp/product/B074H4S71B
https://www.amazon.co.uk/gp/product/B0725PJQQV

And I have a KakuteF7 AIO and an OmnibusF4Pro mounted on a PDB stuck to the speaker with double-sided tape. I had to stick the speaker down as well. I recommend you use a flight controller with an SD card slot.

For generating the sounds I recommend:
https://apps.apple.com/us/app/multitone-generator/id1161245288

@andyp1per andyp1per added the Library label Oct 14, 2019
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
8 participants
You can’t perform that action at this time.