mc_pos_control: path following and smooth transitions on waypoints in AUTO

Implemented L1-like, 3D path following:
 - Copter follows path even if was disturbed.
 - Horizontal and vertical axes normally have different max speeds (MPC_XY_VEL_MAX and MPC_Z_VEL_MAX), caused by physical limits. In 3D case need to use not sphere of radius L1, but ellipsoid. This done using space to unitless, so 1 == position error resulting max allowed speed.
 - When passing via waypoint and the next one located on the same line copter will not slow down (if passes correctly) even if acceptance radius is smaller than L1.
 - In the same time all waypoints, even with very small acceptance radius will be reached, because of smart control near waypoint.
 - Position setpoint that used to calculate velocity moved continuously with <=max allowed speed. As result all transitions (even start of mission) are more smooth.

Tested in HIL.

TODO
 - Add "L1 multiplier" parameter that will control how fast copter will go to desired path.
 - More HIl and flight testing, and corner cases inspection.