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.
the vehicle yaws towards the next waypoint before accelerating. This is
required for drones with front vision and aerodynamic multicopters such
as standard vtol planes or highspeed multirotors.
The linear mapping from position error to cruise velocity is changed by
a combination of that linear mapping and a nonlinear function containing
the maximum acceleration and jerk to avoid overshoots at waypoints due to
overoptimistic breaking distance.
This is done to avoid generating large yaw changes when the velocity
vector is small; for example when switching into loiter or reaching the
last waypoint.
- move the update after the integration: a new computed jerk has an impact at the next epoch only
- add jerk reduction in case of too large integration time: when a jerk of "min_jerk" during dt is too much
- add jerk reduction if the integration time is larger than the predicted one and that integrating that jerk would lead to an acceleration overshoot
- rename some variables
bresch