some of you might have seen my post here: http://forum.bitcraze.se/viewtopic.php? ... t=10#p2405
A while back I was experimenting with using the barometer to implement a hover mode. It worked relatively well in stable pressure conditions. I never released the code because I wanted to clean it up and do it properly (EKF, etc). However Ive not got around to doing that yet so I decided to expose the code. Yes its messy, yes lots is redundant, yes lots is suboptimal, but yes it works too:). I not going to continue to develope this fork (maybe some minor bug fixes and gui updates), but someone might find it useful.
How to use:
- Set up your hover button from the GUI. By default for PS3_Mode_1 its the L1 button of the PS3 controller.
- When you press this button, it sets the target altitude as the current altitude.
- Hold the button to remain in hover mode.
- While in hover mode, the throttle can be used to change the target altitude. Eg holding throttle up for a second raises it about a meter.
- Release the button to return to manual mode.
- Next time you enter hover mode, the target altitude is reset to the current altitude again.
- A good tip: Let go of the throttle immediately after entering hover mode. Its very easy to forget that one is holding it up and the flie will continue to rise.
The ms5611 driver has been partly rewritten to enable pressure measurements at 50-100hz.
I wrote the code a while ago, but if I remember correctly it sort of works as follows:
All pressure readings are converted to an altitude above sea level.
When entering hover mode, we set the target altitude.
We can then define a PID controller that should take the flie from its current altitude to the target altitude. The P part is just the difference, eg 1 meter too high.
For the D term we use the vertical speed...here the code is ugly. First one needs to compute the vertical acceleration, then subtract gravity. This vertical acceleration is then integrated to get a speed estimate. To stop it accumulating error forever, it converges to the speed estimate from the barometer. This is also computed in a non mathematical way: some factor * (super_smoothed_altitude-smoothed_altitude).
The I term is just the integrated error - and is very very very important as it makes up for the voltage drop. The P and D term are reset every time hover mode is entered, and the I term is only reset when you start charging the flie. The default I value right now is set up to be a pretty good value for a stock flie at 80% battery. The default values takes around 1-2 to converge on a flie with a depleted battery during which time the flie might oscillate within a meter range or so.
Note that hover mode only works well in pressure stable environments. Trying to hover with people opening/closing windows/doors or during a thunderstorm does not work very well!
Here are videos of it working:
http://www.youtube.com/watch?v=aRsvPyRQaFA - static
http://www.youtube.com/watch?v=0oYzMVUKZKI - moving
Here is the firmware:
https://bitbucket.org/omwdunkley/crazyflie-firmware
Here is the client:
https://bitbucket.org/omwdunkley/crazyflie-pc-client
Good luck
![:-) :)](./images/smilies/icon_e_smile.gif)
EDIT: corrected the link