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