AHRSInterface.AHRS.handle_packet_ony_quat C# (CSharp) Method

        private void handle_packet_ony_quat(byte type, int length, byte[] data)
        {
            int act_channels = 0xFFFF;
            int i = 4;

            byte[] Data_tmp = new byte[4];

            Data_tmp[0] = data[i + 0];
            Data_tmp[1] = data[i + 1];
            Data_tmp[2] = data[i + 2];
            Data_tmp[3] = data[i + 3];
            m_recentData[Q1_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
            m_q1 = (float)m_recentData[Q1_INDEX];
            i += 4;
            // Pitch angle
            Data_tmp[0] = data[i + 0];
            Data_tmp[1] = data[i + 1];
            Data_tmp[2] = data[i + 2];
            Data_tmp[3] = data[i + 3];
            m_recentData[Q2_INDEX] = BitConverter.ToSingle(Data_tmp, 0);

            m_q2 = (float)m_recentData[Q2_INDEX];
            i += 4;

            Data_tmp[0] = data[i + 0];
            Data_tmp[1] = data[i + 1];
            Data_tmp[2] = data[i + 2];
            Data_tmp[3] = data[i + 3];
            m_recentData[Q3_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
            m_q3 = (float)m_recentData[Q3_INDEX];
            i += 4;

            // quarto campo
            Data_tmp[0] = data[i + 0];
            Data_tmp[1] = data[i + 1];
            Data_tmp[2] = data[i + 2];
            Data_tmp[3] = data[i + 3];
            m_recentData[Q4_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
            m_q4 = (float)m_recentData[Q4_INDEX];
            m_accelX = (float)0.0;
            m_accelY = (float)0.0;
            m_accelZ = (float)0.0;
            m_gyroX = (float)0.0;
            m_gyroY = (float)0.0;
            m_gyroZ = (float)0.0;
            m_magX = (float)0.0;
            m_magY = (float)0.0;
            m_magZ = (float)0.0;
            m_latitude = (float)0.0;
            m_longitude = (float)0.0;
            m_velGPS = (float)0.0;
            m_altitudine = (float)0.0;
            m_satellite_number = (float)0.0;
            m_ID_DISP = (float)0.0;

               // m_crc = (float)m_recentData[CRC_INDEX];

            m_q1s = m_q1;
            m_q2s = m_q2;
            m_q3s = m_q3;
            m_q4s = m_q4;
            float[] Quaternion = new float[] { (float)m_q1, (float)m_q2, (float)m_q3, (float)m_q4 };

            float pitch, roll, yaw;
            double phi2;
            double theta2;
            double psi2;

            float z = Quaternion[0];
            float y = Quaternion[1];
            float x = Quaternion[2];
            float w = Quaternion[3];

            // Used to determine if near vertical where asin() function is not
            //defined.
            double ASIN_LIMIT = 0.9999939;

            yaw = (float)Math.Atan2(2 * (x * y + z * w), 2 * w * w + 2 * x * x - 1);
            pitch = 2 * (x * z - y * w);
            roll = 0;   //roll has no meaning if vertical

            if (pitch < ASIN_LIMIT)
            {
                if (pitch > -ASIN_LIMIT)
                {
                    roll = (float)Math.Atan2(2 * (y * z + x * w), 2 * w * w + 2 * z * z - 1);
                    pitch = 2 * (x * z - y * w);
                    pitch = -(float)Math.Asin(pitch);
                }
                else
                {
                    pitch = -(float)Math.PI / 2;
                }
            }
            else
            {
                pitch = (float)Math.PI / 2;
            }

            m_rollAngle = yaw * 57.2957795130823;
            m_pitchAngle = -pitch * 57.2957795130823;
            m_yawAngle = -roll * 57.2957795130823;
            roll = -roll;
            phi2 = (+yaw * Math.Cos(roll) + pitch * Math.Sin(roll)) / 2.0;
            theta2 = (-yaw * Math.Sin(roll) + pitch * Math.Cos(roll)) / 2.0;

            psi2 = roll / 2.0;// yaw / 2.0;

            double sinphi2 = Math.Sin(phi2);
            double cosphi2 = Math.Cos(phi2);
            double sintheta2 = Math.Sin(theta2);
            double costheta2 = Math.Cos(theta2);
            double sinpsi2 = Math.Sin(psi2);
            double cospsi2 = Math.Cos(psi2);
            double cba = 0;//m_q1;
            double cbb = 0;//m_q2;
            double cbc = 0;//m_q3;
            double cbd = 0;//m_q4;

            cba = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
            cbb = sinphi2 * costheta2 * cospsi2 - cosphi2 * sintheta2 * sinpsi2;
            cbc = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
            cbd = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
            m_q1 = cba;
            m_q2 = cbb;
            m_q3 = cbc;
            m_q4 = cbd;

            DataReceivedEvent(act_channels);
        }