Velocity Controller and Additional Filtering

[stephanbro]

This PR adds a velocity controller as an intermediary between the position and attitude controllers which gives a lot better performance on holding position as well as gives the ability to limit the velocity on board. I've done some basic tuning on the controller, but the tunings could still use work!

I've also added an LPF filter to the gyro at 80 Hz to help filter out some noise for doing attitude rate controls.

A few other changes are listed below:

• Increase yaw rate KP to 120
• Add error max and integral disable to PID library
• Add quaternion to kalman log
• Change position/velocity PID parameters to modify stored pid values instead of init values

[tobbeanton]

This will be interesting to test. I've seen this used in many flight controllers so it is most likely the right way to go. I've recently discovered this pull request for cleanflight which might be of interest.

[tobbeanton]

I did a quick test and I'm not sure it made any improvement. It is now wobbling much more, maybe because of delay in D-Term caused by LPF? Are you seeing the same? Maybe the stiffer motor mounts you are using could make a difference?

[stephanbro]

Interesting... Thanks for linking that issue, it was a very interesting read. Definitely something to think about going forward.

I wasn't able to reproduce the wobbling that you speak of. Are you using the stock PID tunings in the build? In what way is there wobbling? They delay in the D-Term due to the filter can definitely create some issues. Could you try increasing the cutoff frequency and see if that helps?

I did some data collection on the stiffer motor mounts I was experimenting with and I wasn't able to find anything conclusive to say that they are better, so I've been using the stock arms for this work. I've also begun work on separating the position and velocity controllers. Have you started work on that yet?

[ataffanel]

Hi Stephan, we have not started on separating the controllers yet. It feels that getting a good setup of the controller with position and velocity separated would be a good first step to improve them. Maybe we can sync on #136?

[stephanbro]

@tobbeanton I increased some of the cutoff frequencies which will hopefully alleviate your issues. Let me know if that helps.

I've also implemented a velocity controller as an intermediary between the position and attitude controllers here: c3c72d7. My qualitative impression is it a major improvement on the overall ability to hold position, so please give it a try!

[tobbeanton]

@stephanbro We tested the velocity controller and it works great! It is very stable holding the position now in x,y. Now the height isn't that precise but @ataffanel might have some tuning settings for that. Interesting times indeed!

As for the filtering I don't see the wobble any more when flying manually but still some small oscillations. It is very hard to tell if the performance is better, maybe you can test that easier? Without a performance increase there is just added computations for the filtering so it would be nice to have some kind of measurement. If I set the cutoff frequencies high will it be the same as disabling the filtering (getting rid of the delay)?

[stephanbro]

@tobbeanton Thanks for the feedback! The Z position and velocity generally looked okay with the tunings I had, but I was having my usual issues of the Z position estimate being a bit jumpy so it was hard to get it super good.

I decided to remove the LPF on the D term after doing some test flights and not seeing much improvement in the flight (besides the setpoints looking a lot smoother). In the future we may want to revisit what is the best solution is to the setpoint spikes I had posted before on this PR.

[stephanbro]

Is there anything else needed to get this merged? Maybe more tuning?

[ataffanel]

This is good to merge as such, I just wanted to test it but we did not get the time today, will be merged tomorrow. We just talked about re-adding the filter with a compile flag so that it will be easy to enable if someone wants to play with it. The plan is to test and merge tomorrow morning (in europe :-). Sorry for the delay.

More tuning can come later, this is already much better than master!

[ataffanel]

I reverted your last commit and added a configuration flag to enable the filter per-pid. When doing some tests I found that the filter actually helps for the position controller so I re-enabled it for position controller and merged the PR.

Thanks a lot, this is starting to fly really well now!