Turn altitude correction into a helper function

[ClayMav]

Currently, the code for altitude correction is essentially the same across multiple tasks (namely LinearMovementTask and HoverTask). It is like that future tasks will need the same altitude correction code.

The job to be done is factoring out this common code into a helper function or class.