//-----------------------------------------------------------------------
int GetSamples()
{
float yaw, pitch, roll;
int err = PFS.GetOrientation(out yaw, out pitch, out roll);
if (err == 0)
{
// Float assignments are atomic in C# so I don't have to worry about
// thread access protection to these samples
float previous_yaw = YawSample;
YawSample = yaw;
PitchSample = pitch;
RollSample = roll;
float delta = YawSample - previous_yaw;
float HALF_CIRCLE = (float)Math.PI;
if (Math.Abs(delta) > HALF_CIRCLE)
{
// We turned across the discontinuity at 180 degrees so modify the delta to
// reflect the probable angular motion
if (delta > 0)
{
delta -= (2 * HALF_CIRCLE);
}
else
{
delta += (2 * HALF_CIRCLE);
}
}
ContinuousYaw += delta;
}
return(err);
}
//-----------------------------------------------------------------------
public override void Stop()
{
Thread thread = PollThread;
if (thread != null && !Stopped)
{
Stopped = true;
// Block until the polling thread is dead.
//thread.Join();
// Unfortunately I can't join the thread since FreePIE keeps it
// hostage for a long time, so I have to lock on a different semaphore
lock (PollThreadLock) {
// Now I can close the tracker without worrying about synchronization
PFS.Close();
}
}
}
//-----------------------------------------------------------------------
public override Action Start()
{
int err = PFS.Connect();
if (err == 0)
{
// Grab the first sample just to make sure everything is linked
// up properly and to initialize the values
err = GetSamples();
if (err == 0 || err == PFS.FREESPACE_ERROR_TIMEOUT)
{
return(RunSensorPoll); // start the poll thread
}
else
{
throw new Exception("Failed to initialize the FreeSpace tracker with error " + err);
}
}
else
{
throw new Exception("Failed to connect to FreeSpace tracker with error " + err);
}
}