Also split a few functions into smaller ones for readability, fix
formatting, use geters to get the current state of the trajectory
instead of return arguments.
This solves many numerical issues when the trajectory is close to the
primary NE axes (small velocities). It is also more robust when dt is
large and has some jitter.
It was broken because _velocity_setpoint is used for input and output
and was assumed to be the input at a place where it was already overwitten
To clarify this, the input setpoint is renamed "target"
This is done to allow proper initialization of the new FlightTask and
give it a chance to continue the setpoints without discontinuity. The
function checkSetpoints replaces the setpoints containing NANs with an
estimate of the state. The estimate is usually the current estimate of
the EKF or zero.
The transition FlightTask also provides an estimate of the current
acceleration to properly initialize the next FlightTask after
back-transition. This avoid having to initialize the accelerations to
zero knowing that the actual acceleration is usually far from zero.
bresch