void Update()
{
if (ovrIsInitialized)
{
var raw = capiHmd.GetTrackingState().RawSensorData;
Accelerometer.Set(raw.Accelerometer.x, raw.Accelerometer.y, raw.Accelerometer.z);
Gyro.Set(raw.Gyro.x, raw.Gyro.y, raw.Gyro.z);
Magnetometer.Set(raw.Magnetometer.x, raw.Magnetometer.y, raw.Magnetometer.z);
// For Unity 5.1 see : http://docs.unity3d.com/ScriptReference/VR.InputTracking.html
OVRPose pose = capiHmd.GetTrackingState().HeadPose.ThePose.ToPose();
Position = pose.position;
Orientation = pose.orientation;
AHRS.Update(
Gyro.x, Gyro.y, Gyro.z,
Accelerometer.x, Accelerometer.y, Accelerometer.z,
Magnetometer.x, Magnetometer.y, Magnetometer.z);
MARGOrientation.Set(AHRS.Quaternion[1], AHRS.Quaternion[2], AHRS.Quaternion[3], AHRS.Quaternion[0]);
//transform.rotation = MARGOrientation;
transform.rotation = pose.orientation;
transform.position = pose.position + DisplayedPosition;
}
}
private void AHRSUpdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz)
{
madgwick.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)mx, (float)my, (float)mz);
q0 = madgwick.Quaternion[0];
q1 = madgwick.Quaternion[1];
q2 = madgwick.Quaternion[2];
q3 = madgwick.Quaternion[3];
// Different quaternion signs for different orientation of IMU.
// Not sure if there is a way to keep it consistent. Look into it further.
// This is for when the IMU is face up
//orientation = new Quaternion(-q1, -q0, q2, q3);
// Face down
orientation = new Quaternion(q1, -q0, q2, -q3);
// Test
//orientation = new Quaternion(q2, -q1, q3, q0);
// Test2
//orientation = new Quaternion(q3, q2, q0, q1);
// Test3
//orientation = new Quaternion(-q1, q0, q2, -q3);
//Saving rotation history
double currentTimeMillis = System.TimeSpan.FromTicks(System.DateTime.Now.Ticks).TotalMilliseconds;
History.Enqueue(new RotationHistory()
{
Milliseconds = currentTimeMillis, Rotation = orientation, Angles = IMUangles
});
RotationHistory his = History.Peek();
while (his.Milliseconds < (currentTimeMillis - MillisecondsOfHistory))
{
History.Dequeue();
his = History.Peek();
}
}
void Update()
{
if (isActive == false)
{
return;
}
Vector3 a = Input.acceleration;
Vector3 g = Input.gyro.rotationRateUnbiased;
AHRS.Update(g.x, g.y, g.z, a.x, a.y, a.z);
Quaternion quat = new Quaternion(AHRS.Quaternion[2], -AHRS.Quaternion[1], AHRS.Quaternion[0], AHRS.Quaternion[3]);
quat.eulerAngles = new Vector3(quat.eulerAngles.x * flipingVector.x, quat.eulerAngles.y * flipingVector.y, quat.eulerAngles.z * flipingVector.z);
quat *= Quaternion.Euler(offsetVector);
Quaternion inverseQuat = Quaternion.Inverse(quat);
transform.rotation = Quaternion.Slerp(transform.rotation, inverseQuat, rotationSpeed * Time.deltaTime);
}
void Update()
{
if (sp.IsOpen)
{
try
{
string st = sp.ReadLine();
Debug.Log(st);
Debug.Log(Time.deltaTime);
string[] stp = st.Split('\t');
acc = new Vector3(float.Parse(stp[0]), float.Parse(stp[1]), float.Parse(stp[2]));
gyr = new Vector3(float.Parse(stp[3]), float.Parse(stp[4]), float.Parse(stp[5])) * (float)Math.PI / 180f;
mag = new Vector3(float.Parse(stp[6]), float.Parse(stp[7]), float.Parse(stp[8]));
ahrs.Update(gyr.x, gyr.z, gyr.y, acc.x, acc.z, acc.y);
transform.rotation = ahrs.quaternion;
}
catch
{
}
}
}
// Update is called once per frame
void Update()
{
if (true == isPlay)
{
if (EndData != frame)
{
//in Unity, ZXY euler, while ZYX euler in Matlab
// in Unity
try
{
//calculate euler angle
AHRS.Update((float)xGyro[frame] * (Mathf.PI / 180), (float)yGyro[frame] * (Mathf.PI / 180), (float)zGyro[frame] * (Mathf.PI / 180), (float)xAcc[frame], (float)yAcc[frame], (float)zAcc[frame], (float)xMag[frame], (float)yMag[frame], (float)zMag[frame]);
euler = quatern2euler(qConj(AHRS.Quaternion));
euler[0] *= (180 / Mathf.PI);
euler[1] *= (180 / Mathf.PI);
euler[2] *= (180 / Mathf.PI);
roll.Add(euler[0]);
pitch.Add(euler[1]);
yaw.Add(euler[2]);
//calculate norm of acceleromater
norm = (float)(xAcc[frame] * xAcc[frame] + yAcc[frame] * yAcc[frame] + zAcc[frame] * zAcc[frame]);
//Judge whether the boomerang flights
if (Mathf.Sqrt(norm) > 50)
{
Debug.Log("achieved!");
if (60 == update_frequency)
{
// Display rotation speed of boomerang
RotationSpeed.GetComponent <Text>().text = "Rotation speed: " + Mathf.Abs((float)zGyro[frame]) + "degree/s";
//update once in 60 frame
update_frequency = 1;
}
// Detect throwing speed
if (maxSpeed < Mathf.Abs((float)zGyro[frame]))
{
maxSpeed = Mathf.Abs((float)zGyro[frame]);
}
//else if (maxSpeed > Mathf.Abs((float)zGyro[frame]))
//{
// ThrowingSpeed.GetComponent<Text>().text = "Throwing speed: " + Mathf.Abs((float)maxSpeed) + "degree/s";
//}
if (!isFlight)
{
isFlight = true;
// Display boomerang status;
BoomerangStatus.GetComponent <Text>().text = "Boomerang status: Flight";
}
update_frequency++;
}
else
{
// Holding boomerang
//rotate using phi, theta and psi caalculated by Madgwick
//transform.rotation = Quaternion.Euler((float)-phi[frame], (float)-theta[frame], (float)-psi[frame]);
transform.rotation = Quaternion.Euler((float)-roll[frame], (float)-pitch[frame], (float)-yaw[frame]);
// Display inclination to horizontality
//Inclination.GetComponent<Text>().text = "Inclination to horizontality: " + (int)Mathf.Abs((float)phi[frame]) + "°, " + (int)Mathf.Abs((float)theta[frame]);
Inclination.GetComponent <Text>().text = "Angle from wind: " + (int)Mathf.Abs((float)roll[frame]) + "°\nAngle from vertical: " + (int)Mathf.Abs((float)pitch[frame]) + "°";
if (isFlight)
{
isFlight = false;
// Display boomerang status
BoomerangStatus.GetComponent <Text>().text = "Boomerang status: Holding";
ThrowingSpeed.GetComponent <Text>().text = "Throwing speed: " + Mathf.Abs((float)maxSpeed) + "degree/s";
}
}
}
catch
{
frame = EndData;
isPlay = false;
maxSpeed = 0;
Play_button.GetComponentInChildren <Text>().text = "start";
Debug.Log("range out!!");
transform.rotation = Quaternion.Euler(0, 0, 0);
}
if (EndData != frame)
{
frame++;
}
}
}
}
void Update()
{
dk2 = GameObject.FindWithTag("DK2").GetComponent <DK2Controller>();
foreach (MoveManager move in moves)
{
if (move.Disconnected)
{
continue;
}
CorrectedPosition = TransformMatrix.MultiplyPoint3x4(move.Position);
/*Accelerometer.Set(move.Acceleration.x, move.Acceleration.y, move.Acceleration.z);
* Gyro.Set(move.Gyro.x, move.Gyro.y, move.Gyro.z);
* Magnetometer.Set(move.Magnetometer.x, move.Magnetometer.y, move.Magnetometer.z);*/
/*Accelerometer.Set(-move.Acceleration.y * 10, move.Acceleration.z * 10, move.Acceleration.x * 10);
* Gyro.Set(move.Gyro.x, move.Gyro.z, move.Gyro.y);
* Magnetometer.Set(-move.Magnetometer.y / 350, -move.Magnetometer.z / 350, -move.Magnetometer.x / 350);*/
Accelerometer.Set(move.Acceleration.x * 10, move.Acceleration.y * 10, move.Acceleration.z * 10);
Gyro.Set(move.Gyro.x, move.Gyro.y, move.Gyro.z);
Magnetometer.Set(move.Magnetometer.x / 350, -move.Magnetometer.y / 350, move.Magnetometer.z / 350);
AHRS.Update(
Gyro.x, Gyro.y, Gyro.z,
Accelerometer.x, Accelerometer.y, Accelerometer.z,
Magnetometer.x, Magnetometer.y, Magnetometer.z);
MARGOrientation.Set(AHRS.Quaternion[1], AHRS.Quaternion[2], AHRS.Quaternion[3], AHRS.Quaternion[0]);
transform.rotation = MARGOrientation;
/*Debug.Log(
* "Move: " + Accelerometer + " - " + Gyro * 10 + " - " + Magnetometer +
* " DK2: " + dk2.Accelerometer + " - " + dk2.Gyro * 10 + " - " + dk2.Magnetometer * 100);
*
* Debug.Log(
* "Move: " + Accelerometer.sqrMagnitude + " - " + Gyro.sqrMagnitude + " - " + Magnetometer.sqrMagnitude +
* " DK2: " + dk2.Accelerometer.sqrMagnitude + " - " + dk2.Gyro.sqrMagnitude + " - " + dk2.Magnetometer.sqrMagnitude);*/
Debug.Log("Move: " + Accelerometer + " - " + Gyro * 10 + " - " + Magnetometer);
Debug.Log("DK2: " + dk2.Accelerometer + " - " + dk2.Gyro * 10 + " - " + dk2.Magnetometer);
Debug.Log(
(dk2.Accelerometer.sqrMagnitude / Accelerometer.sqrMagnitude) + " - " +
(dk2.Gyro.sqrMagnitude / Gyro.sqrMagnitude) + " - " +
(Magnetometer.sqrMagnitude / dk2.Magnetometer.sqrMagnitude));
//transform.rotation = move.Orientation;
transform.position = CorrectedPosition + DisplayedPosition;
if (move.GetButtonDown(PSMoveButton.Move))
{
move.ResetOrientation();
}
if (Registration.Correlations.Count == 300 && !Registration.IsRegistered)
{
Registration.Compute();
GlobalTransform = Registration.GetGlobalTransform();
LocalTransform = Registration.GetLocalTransform();
Debug.Log("GlobalTransform: " + GlobalTransform);
Debug.Log("LocalTransform: " + LocalTransform);
TransformMatrix = GlobalTransform;
}
Registration.AddCorrelation(dk2.Position, dk2.Orientation, move.Position);
}
}