private bool EnsureMahonyReady()
        {
            if (mahonyAHRS != null)
            {
                return(true);
            }

            if (motionPlusPeriodCounter.Update())
            {
                mahonyAHRS = new MahonyAHRS(motionPlusPeriodCounter.SamplePeriod);
                motionPlusPeriodCounter.Stop();
                return(true);
            }

            return(false);
        }
Exemple #2
0
        public void Update(byte[] bytes)
        {
            var  dataInPackage = (Flags)bytes[1];
            bool raw;
            bool orientation;

            SetFlags(dataInPackage, out raw, out orientation);

            if (!freeqSampled && raw)
            {
                if (samples == 0)
                {
                    started = DateTime.Now;
                }

                samples++;
                var delta = (DateTime.Now - started).TotalSeconds;
                if (delta > 1)
                {
                    var freq = samples / (float)delta;
                    freeqSampled = true;
                    ahrs         = new MahonyAHRS(1f / freq, 0.1f);
                }
                else
                {
                    return;
                }
            }

            var index = 2;

            if (raw)
            {
                var ax = Raw.ax = GetFloat(bytes, index, 0);
                var ay = Raw.ay = GetFloat(bytes, index, 4);
                var az = Raw.az = GetFloat(bytes, index, 8);

                var gx = Raw.gx = GetFloat(bytes, index, 12);
                var gy = Raw.gy = GetFloat(bytes, index, 16);
                var gz = Raw.gz = GetFloat(bytes, index, 20);

                var mx = Raw.mx = GetFloat(bytes, index, 24);
                var my = Raw.my = GetFloat(bytes, index, 28);
                var mz = Raw.mz = GetFloat(bytes, index, 32);

                ahrs.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)mx, (float)my, (float)mz);
                quaternion.Update(ahrs.Quaternion[0], ahrs.Quaternion[1], ahrs.Quaternion[2], ahrs.Quaternion[3]);

                index += 36;
            }

            if (orientation)
            {
                GoogleYaw   = GetFloat(bytes, index, 0);
                GooglePitch = GetFloat(bytes, index, 4);
                GoogleRoll  = GetFloat(bytes, index, 8);
                index      += 12;
            }

            NewData = true;
        }
Exemple #3
0
        private void BackGroundWorker()
        {
            var endpoint = new IPEndPoint(IPAddress.Any, udpPort);

            udpClient = new UdpClient(endpoint);

            MahonyAHRS ahrs = null;

            quaternion = new Quaternion();

            var freeqSampled = false;
            int samples      = 0;
            var started      = DateTime.Now;

            OnStarted(this, new EventArgs());

            while (true)
            {
                byte[] bytes;

                try
                {
                    bytes = udpClient.Receive(ref endpoint);
                }
                catch (SocketException)
                {
                    if (!stopping)
                    {
                        throw;
                    }
                    break;
                }

                var flag            = bytes[0];
                var sendRaw         = (flag & 1) == 1;
                var sendOrientation = (flag & 2) == 2;

                if (!freeqSampled && sendRaw)
                {
                    if (samples == 0)
                    {
                        started = DateTime.Now;
                    }

                    samples++;
                    var delta = (DateTime.Now - started).TotalSeconds;
                    if (delta > 1)
                    {
                        var freq = samples / (float)delta;
                        freeqSampled = true;

                        System.Diagnostics.Debug.WriteLine("Samples / s: {0}", samples);
                        ahrs = new MahonyAHRS(1f / freq, 0.1f);
                    }
                    else
                    {
                        continue;
                    }
                }

                var index = 1;

                if (sendRaw)
                {
                    var ax = GetFloat(bytes, index, 0);
                    var ay = GetFloat(bytes, index, 4);
                    var az = GetFloat(bytes, index, 8);

                    var gx = GetFloat(bytes, index, 12);
                    var gy = GetFloat(bytes, index, 16);
                    var gz = GetFloat(bytes, index, 20);

                    var mx = GetFloat(bytes, index, 24);
                    var my = GetFloat(bytes, index, 28);
                    var mz = GetFloat(bytes, index, 32);

                    ahrs.Update(gx, gy, gz, ax, ay, az, mx, my, mz);
                    quaternion.Udate(ahrs.Quaternion[0], ahrs.Quaternion[1], ahrs.Quaternion[2], ahrs.Quaternion[3]);

                    index += 36;
                }

                if (sendOrientation)
                {
                    GoogleYaw   = GetFloat(bytes, index, 0);
                    GooglePitch = GetFloat(bytes, index, 4);
                    GoogleRoll  = GetFloat(bytes, index, 8);
                }

                newData = true;
            }
        }