More flight mode, vtol mode, armed into main status

This was lost when moving the handling the the trajectory points from within the vehicle.

Fix crash caused by clear method not resetting internals correctly

Better flight trajectory point algorithm