I've always wanted to take part in arduplane development, so I finally have chance to do it =)
Here you can find info on how to do the same http://diydrones.com/profiles/blogs/how-to-contribute-to-a-diy-dron...
I have chosen this one -
8) Minimum altitude feature for modes >= FBW - Add a feature where the user can specify a minimum altitude that the autopilot will not go under when in higher modes
just because I was implementing some anti-crash functions on my own.
So here my little demo. APM is in FBW-B mode and I hold pitch all the way DOWN! until the very end of video. The jumps you will see is my code bringing the plane back to safe altitude. Minimum alt was set 30 meters above home altitude, so you can see ground. Enjoy and post your ideas!
UPDATE: Here is new video (new algorithm, no jumps):
And my patch has been applied to the ArduPlane source! =))) Man, I wish I could have a job related to drones...