private static void ProcessorMainFunction()
{
try
{
while (true)
{
lock (_locker)
while (!_go)
{
Monitor.Wait(_locker);
}
string odppayload = "";
lock (_locker)
{
int count = m_odppayloads.Count();
if (count > 0)
{
// Dequeue the "freshest" odp report! (the one at the end/bottom
// of the queue)
odppayload = m_odppayloads.ElementAt(m_odppayloads.Count() - 1);
m_odppayloads.Clear(); //kill all the older ones
}
_go = false;
}
HeadsetTrackingData headsetdata = ExtractHeadsetData(odppayload);
if (!headsetdata.badpacket)
{
m_headsetDataHandler.HeadsetTrackingUpdate(headsetdata);
}
}
}
catch (Exception)
{
// thread exit
}
}
private static HeadsetTrackingData ExtractHeadsetData(string odpreport)
{
HeadsetTrackingData retval = new HeadsetTrackingData();
// extract the headtracking payload:
int lastcomma = odpreport.LastIndexOf(",");
string odppayload = odpreport.Substring(lastcomma + 1, odpreport.Length - lastcomma - 2);
// convert the headtracking payload into values:
long[] quatn;
double[] quat;
quatn = new long[16];
quat = new double[4];
int gyrocalib = 0;
//int temperature = 0;
int freefall = 0;
int tapdir = 0;
int tapcount = 0;
int magcalib = 0;
int vermaj = 0;
int vermin = 0;
int pedometersteps = 0;
bool badpacket = false;
try
{
quatn[0] = ExtractQuatFromHex(odppayload.Substring(6, 4), 0); //, Quat0_hex_label); // quat 0 is 4 hex chars from char 6 (0-based index)
quatn[1] = ExtractQuatFromHex(odppayload.Substring(14, 4), 1); //, Quat1_hex_label); // quat 1 is 4 hex chars from char 14 (0-based index)
quatn[2] = ExtractQuatFromHex(odppayload.Substring(22, 4), 2); //, Quat2_hex_label); // quat 2 is 4 hex chars from char 22 (0-based index)
quatn[3] = ExtractQuatFromHex(odppayload.Substring(30, 4), 3); //, Quat3_hex_label); // quat 3 is 4 hex chars from char 30 (0-based index)
gyrocalib = ExtractIntFromHex(odppayload.Substring(36, 2)); // gyrocalibration info is 2 hex chars from char 36 (0-based index)
// extract other sensor info...
//temperature = ExtractIntFromHex(odppayload.Substring(4, 2));
freefall = ExtractIntFromHex(odppayload.Substring(12, 2));
tapdir = ExtractIntFromHex(odppayload.Substring(26, 2));
tapcount = ExtractIntFromHex(odppayload.Substring(28, 2));
magcalib = ExtractIntFromHex(odppayload.Substring(34, 2));
vermaj = ExtractIntFromHex(odppayload.Substring(38, 2));
vermin = ExtractIntFromHex(odppayload.Substring(40, 2));
pedometersteps = ExtractIntFromHex(odppayload.Substring(18, 4));
}
catch (Exception)
{
badpacket = true;
retval.badpacket = true;
}
if (!badpacket)
{
//process for use
for (int i = 0; i < 4; i++)
{
if (quatn[i] > 32767)
{
quatn[i] -= 65536;
}
quat[i] = ((double)quatn[i]) / 16384.0f;
}
retval.gyrocalib = gyrocalib;
// other data
//retval.temperature = temperature;
retval.freefall = freefall == 1 ? true : false;
retval.tapdir = (TapDirection)tapdir;
retval.taps = tapcount;
retval.gyrocalib = gyrocalib;
retval.magnetometercalib = magcalib;
retval.vermaj = vermaj;
retval.vermin = vermin;
retval.pedometersteps = pedometersteps;
// auto calibrate by headset:
if (!initialcalibrate && gyrocalib == 3)
{
calon = true;
initialcalibrate = true;
}
// handle calibrate request (auto or manual):
if (calon)
{
if (!m_usercalquat)
{
calquat = quat;
}
calon = false;
}
double[] newquat;
if (m_doCalibrate)
{
//correct for calibration
newquat = new double[4];
//create inverse of cal vector
newquat = quatinv(calquat);
////reapply current state to cal (old version, params in wrong order)
//newquat = quatmult(newquat, quat);
//reapply current state to cal (fixed reverse parameters, thanks Doug)
//newquat = quatmult(quat, newquat);
newquat = quatmult(newquat, quat); // Douglas Wong proposed fix
}
else
{
// no calibration
newquat = quat;
}
if (m_doOrientation)
{
// Old C# version, problem with roll:
////bank-roll
//retval.phi_roll = -180.0 / 3.14159 * Math.Asin(-2.0 * newquat[1] * newquat[3] + 2.0 * newquat[0] * newquat[2]);
////heading
//retval.psi_heading = -180.0 / 3.14159 * Math.Atan2((newquat[2] * newquat[1] + newquat[0] * newquat[3]), (newquat[0] * newquat[0] + newquat[1] * newquat[1] - (double)0.5));
////elevation-pitch
//retval.theta_pitch = -180.0 / 3.14159 * Math.Atan2((newquat[2] * newquat[3] + newquat[0] * newquat[1]), (newquat[0] * newquat[0] + newquat[3] * newquat[3] - (double)0.5));
// C# version, new and improved, thanks Doug:
//heading
retval.psi_heading = -180.0 / 3.14159 * Math.Atan2((newquat[2] * newquat[1] - newquat[0] * newquat[3]), (newquat[0] * newquat[0] + newquat[2] * newquat[2] - (double)0.5));
// elevation-pitch
retval.theta_pitch = 180.0 / 3.14159 * Math.Asin(2.0 * newquat[2] * newquat[3] + 2.0 * newquat[0] * newquat[1]);
//retval.theta_pitch = -180.0 / 3.14159 * Math.Atan2((newquat[1] * newquat[3] - newquat[0] * newquat[2]), (newquat[0] * newquat[0] + newquat[3] * newquat[3] - (double)0.5));
// bank-roll
retval.phi_roll = -180.0 / 3.14159 * Math.Atan2((newquat[1] * newquat[3] - newquat[0] * newquat[2]), (newquat[0] * newquat[0] + newquat[3] * newquat[3] - (double)0.5));
//retval.phi_roll = 180.0 / 3.14159 * Math.Asin(2.0 * newquat[2] * newquat[3] + 2.0 * newquat[0] * newquat[1]);
}
//debug
retval.rawreport = odppayload;
retval.quatcalib_q0 = newquat[0];
retval.quatcalib_q1 = newquat[1];
retval.quatcalib_q2 = newquat[2];
retval.quatcalib_q3 = newquat[3];
retval.quatraw_q0 = quat[0];
retval.quatraw_q1 = quat[1];
retval.quatraw_q2 = quat[2];
retval.quatraw_q3 = quat[3];
}
return(retval);
}
