Vector attitude and position controller

This is fully vector attitude and position controller. Tested in HIL (jMAVSim), not tested in flight yet. WIP but ready for very careful testing.

Attitude controller (mc_att_control_vector)
---
Initially attitude controller based on article "Minimum Snap Trajectory Generation and Control for Quadrotors", http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf. But now it reviewed and should have better performance on simultaneous large roll/pitch and yaw changes. Depending on setpoint and angular error controller uses two approaches with different weights:
 - Move thrust vector by shortest trajectory for small attitude errors, control yaw independently. I.e. rotate vehicle around changing axis.
 - Move around fixed axis, used for large attitude errors (e.g. transition from level flight to upside down), where first approach will have singularity - all thrust vector paths will have almost equal length.

Attitude controller uses rotation matrix as setpoint, but can also use euler angles if rotation matrix not defined, for backward compatibility. It can work in any orientation and should handle 3D acrobatic maneures.

Attitude controller is P, attitude rate controller is PD. Integrals not implemented yet.

Parameters are consistent with previous controller, no tuning required.

Position controller (mc_pos_control)
---
Position controller also rewritten to use more clear dynamics. Also it rewritten to C++ to use new mathlib. Outer loop position controllers generate desired velocity and inner controller calculates desired acceleration == thrust vector. After thrust vector limiting attitude setpoint rotation matrix calculated from thrust vector and desired yaw. Vertical component of thrust vector has higher priority when limiting occures, to make sure copter will not loose altitude bacause of very aggressive horizontal acceleration. It's possible to tune position controller in way, where vehicle will be flipped upside down for large downside acceleration. Safe mode with limited tilt is also possible. Roll / pitch / yaw setpoint generated only for logging and control, alltitude controller uses rotation matrix directly. Added parameter for tuning velocity feed-forward in SEATBELT/EASY modes to avoid overshot and returning back when roll/pitch stick returned to center. Now Vehicle follows setpoint on some distance so it will have some time to slow down.

Position controller is P, velocity controller is PID.

Parameters are consistent with previous version, but some tuning may be required.