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;
}
}
