Quaterinions and autopilot questions

[Topper]

Hello after a few years I started to continue some projects for Orbiter.
But I'm a bit out from my thoughts years ago and cannot remember all the staff so I have some questions:

First of all I want to remove some "Bug" I did using quaternions so I guess I builded them wrong and corrected somhow on another position in the code...
So I have two methods to create a quaternion and my question is which one is correct:

First method is this (currently active):

        void yprToQuat(VECTOR3 ypr)
        {
            // Create quaternian from setpoints
            double c1 = cos((ypr.z - PI05)/2);
            double s1 = sin((ypr.z - PI05)/2);
            double c2 = cos(ypr.y/2);
            double s2 = sin(ypr.y/2);
            double c3 = cos(-ypr.x/2);
            double s3 = sin(-ypr.x/2);
            double c1c2 = c1*c2;
            double s1s2 = s1*s2;

            w = c1c2*c3 - s1s2*s3;
              x = c1c2*s3 + s1s2*c3;
            y = s1*c2*c3 + c1*s2*s3;
            z = c1*s2*c3 - s1*c2*s3;
        }

second method is this (seen at wikipedia):

        void set(double yaw, double pitch, double roll)
        {
            // Abbreviations for the various angular functions
            double cy = cos(yaw * 0.5);
            double sy = sin(yaw * 0.5);
            double cp = cos(pitch * 0.5);
            double sp = sin(pitch * 0.5);
            double cr = cos(roll * 0.5);
            double sr = sin(roll * 0.5);

            //Quaternion q;
            this->w = cr * cp * cy + sr * sp * sy;
            this->x = sr * cp * cy - cr * sp * sy;
            this->y = cr * sp * cy + sr * cp * sy;
            this->z = cr * cp * sy - sr * sp * cy;
        }

To get the rotation matrix from the quaternion, I used this method (for both):

        MATRIX3 getMatrix()
        {
            // create matrix from the set point quaternions
            double sqw = w*w;
            double sqx = x*x;
            double sqy = y*y;
            double sqz = z*z;

            // invers (inverse square length) is only required if quaternion is not already normalised
            double invs = 1 / (sqx + sqy + sqz + sqw);

            MATRIX3 mq;
            mq.m11 = ( sqx - sqy - sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
            mq.m22 = (-sqx + sqy - sqz + sqw)*invs ;
            mq.m33 = (-sqx - sqy + sqz + sqw)*invs ;
    
            double tmp1 = x*y;
            double tmp2 = z*w;
            mq.m21 = 2.0 * (tmp1 + tmp2)*invs ;
            mq.m12 = 2.0 * (tmp1 - tmp2)*invs ;
    
            tmp1 = x*z;
            tmp2 = y*w;
            mq.m31 = 2.0 * (tmp1 - tmp2)*invs ;
            mq.m13 = 2.0 * (tmp1 + tmp2)*invs ;
            tmp1 = y*z;
            tmp2 = x*w;
            mq.m32 = 2.0 * (tmp1 + tmp2)*invs ;
            mq.m23 = 2.0 * (tmp1 - tmp2)*invs ; 
            return mq;
        }

And I got this matrices which are different, when I set yaw pitch and roll to 0.



So am I correct when I assume that the " void yprToQuat(VECTOR3 ypr)" method is wrong somehow?
If this is the case more question will follow I guess ?