Esempio n. 1
0
        public void AddMeasurement(IBandGyroscopeReading reading)
        {
            var calibratedXReading = reading.AngularVelocityX - 0.0232528209480614;
            var calibratedYReading = reading.AngularVelocityY + 0.648518794348248;
            var calibratedZReading = reading.AngularVelocityZ - 0.162816270729729;

            if (_lastGyroStamp == default(DateTimeOffset))
            {
                _lastVelocity = new Vector(calibratedXReading, calibratedYReading, calibratedZReading);
                _lastGyroStamp = reading.Timestamp;
                return;
            }

            var msSinceLast = (reading.Timestamp - _lastGyroStamp).TotalMilliseconds;
            _lastGyroStamp = reading.Timestamp;
            if (msSinceLast < 0.01)
                return;

            var orientationX = (calibratedXReading + (calibratedXReading - _lastVelocity.X) / 2.0) * msSinceLast / 1000.0;
            var orientationY = (calibratedYReading + (calibratedYReading - _lastVelocity.Y) / 2.0) * msSinceLast / 1000.0;
            var orientationZ = (calibratedZReading + (calibratedZReading - _lastVelocity.Z) / 2.0) * msSinceLast / 1000.0;

            _orientation = _orientation.Add(orientationX, orientationY, orientationZ);
            _lastVelocity = new Vector(calibratedXReading, calibratedYReading, calibratedZReading);
        }
Esempio n. 2
0
        private void CalibratePointer()
        {
            if (!_gotFirstCalib)
            {
                _maxValues = _bandInputOutput.GyroReading;
                _minValues = _bandInputOutput.GyroReading;
                _gotFirstCalib = true;
                return;
            }

            _maxValues = _maxValues.Max(_bandInputOutput.GyroReading);
            _minValues = _minValues.Min(_bandInputOutput.GyroReading);

            _zOffset = -_minValues.Z;
            _zGain = 1.0 / (_maxValues.Z - _minValues.Z);

            _yOffset = -_minValues.Y;
            _yGain = 1.0 / (_maxValues.Y - _minValues.Y);
        }
        public void AddMeasurement(IBandAccelerometerReading reading)
        {
            _numMeasPoints++;
            var acc = new Vector(reading.AccelerationX, reading.AccelerationY, reading.AccelerationZ);
            if (_numMeasPoints < 10)
            {
                _calibrationValues.Add(acc);
                return;
            }
            if (_numMeasPoints == 10)
            {
                _calibrationValue = 1.0 / _calibrationValues.AverageValue;
                _calibrationValues = null;

                Debug.WriteLine($"Calibrated to {_calibrationValue}");
            }

            _lastAccValue = acc.Multiply(9.81 * _calibrationValue);
            //Debug.WriteLine($"Acc: {acc}, value: {acc.Value}");
        }
 public void Add(Vector pt)
 {
     _movingAverage.Add(pt);
     if (_movingAverage.Count > 10000)
         _movingAverage.RemoveAt(0);
 }
Esempio n. 5
0
 public Vector Min(Vector gyroReading)
 {
     return new Vector(Math.Min(X, gyroReading.X), Math.Min(Y, gyroReading.Y), Math.Min(Z, gyroReading.Z));
 }
Esempio n. 6
0
 public Vector Add(Vector other)
 {
     return new Vector(X + other.X, Y + other.Y, Z + other.Z);
 }