Core: Move Takeoff code to GuidedPoint

Since it's part of the guided flight mode
Core: When doing a takeoff command update the internal fields of the Guided variable

Mainly the altitude should be set correctly.