mc_pos_control: refactor smooth takeoff call

There were two rather confusing function calls one to check
if smooth takeoff needs to be ran and one that updates it.
I combined them and documented the interface correctly
making the parameters non-reference const floats.