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