px4位置控制 旋轉矩陣變換爲四元數

  多旋翼位置控制：
    matrix::Quatf q_sp = R;//由方向餘弦矩陣R得到四元數，並copy到_att_sp.q_d[ ]
    /**
     * Constructor from dcm
     *
     * Instance is initialized from a dcm representing coordinate transformation
     * from frame 2 to frame 1.
     *
     * @param dcm dcm to set quaternion to
     */
    Quaternion(const Dcm<Type> &dcm) :
        Vector<Type, 4>()
    {
        Quaternion &q = *this;
        q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
                                     dcm(1, 1) + dcm(2, 2)));
        q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
                    (Type(4) * q(0)));
        q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
                    (Type(4) * q(0)));
        q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
                    (Type(4) * q(0)));
    }