Auto Center Line tracking during Auto Takeoff

Hey all,
I have been trying to implement a ground tracking code to use the front wheel to help keeping the plane at center line during the takeoff, and finally got it working! wohoo!

I am sure many of you who use fixed wings rc planes have faced this annoying problem of getting the plane off track (probably crashed) during the auto takeoff phase (using APM 2.x)

So here is the video that I made to show the change.

I am using APM 2.5 with firmware version 2.76

If you are interested to test the code just let me know!
Basically I introduced a PID controller in the AUTO mode to control the front wheel steering in order to keep the current heading. The feedback signal is the integrated value of the z-axis of the gyroscope. Yes, it seems simple, but it worked great with me, and solved a lot of trouble.

Would flying out a lead from the rudder output not have helped??

tried that, but didn’t work! Did it work with you?