The other thing to consider is to set the WP_NAVALT_MIN to 50 cm. This will keep the flight controller from trying to control the aircraft before liftoff. Be aware though that you may see it start to drift on takeoff and then it will quickly correct once it gets above 50 cm.