예제 #1
0
        private void applyCalibs(ref int yaw, ref int pitch, ref int roll,
                                 ref int accelX, ref int accelY, ref int accelZ)
        {
            CalibData current = calibrationData[0];

            temInt = pitch - current.bias;
            pitch  = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));

            current = calibrationData[1];
            temInt  = yaw - current.bias;
            yaw     = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));

            current = calibrationData[2];
            temInt  = roll - current.bias;
            roll    = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));

            current = calibrationData[3];
            temInt  = accelX - current.bias;
            accelX  = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));

            current = calibrationData[4];
            temInt  = accelY - current.bias;
            accelY  = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));

            current = calibrationData[5];
            temInt  = accelZ - current.bias;
            accelZ  = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom));
        }
예제 #2
0
 private void Calib(ref int value, CalibData current)
 => value = (int)((value - current.Bias) * (current.SensNumer / (float)current.SensDenom));