private bool EnsureMahonyReady() { if (mahonyAHRS != null) { return(true); } if (motionPlusPeriodCounter.Update()) { mahonyAHRS = new MahonyAHRS(motionPlusPeriodCounter.SamplePeriod); motionPlusPeriodCounter.Stop(); return(true); } return(false); }
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; }
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; } }