public RotationAction UndoRotation()
{
if (RotationHistory.Count < 1)
{
return(null);
}
RotationAction rotAct = RotationHistory[RotationHistory.Count - 1];
RotationHistory.RemoveAt(RotationHistory.Count - 1);
hudManager.EnableUndo(RotationHistory.Count > 0);
return(rotAct);
}
private void SaveHistory()
{
double currentTimeMillis = System.TimeSpan.FromTicks(System.DateTime.Now.Ticks).TotalMilliseconds;
History.Enqueue(new RotationHistory()
{
Milliseconds = currentTimeMillis, Rotation = actualRotation
});
//Removes the elements in the history Queue that are older than the set window
RotationHistory his = History.Peek();
while (his.Milliseconds < (currentTimeMillis - MillisecondsOfHistory))
{
History.Dequeue();
his = History.Peek();
}
}
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();
}
}
public Quaternion GetHistoricValue(double millisIntoThePast)
{
if (History.Count == 0)
{
return(Quaternion.identity);
}
RotationHistory[] history = History.ToArray();
double limit = System.TimeSpan.FromTicks(System.DateTime.Now.Ticks).TotalMilliseconds - millisIntoThePast;
int counter = -1;
RotationHistory pastValue = history[++counter];
while (pastValue.Milliseconds < limit && counter < (history.Length - 2))
{
pastValue = history[++counter];
}
return(history[counter].Rotation);
}
public float[] GetHistoricAngles(double millisIntoThePast)
{
float[] empty = { 0, 0, 0 };
if (History.Count == 0)
{
return(empty);
}
RotationHistory[] history = History.ToArray();
double limit = System.TimeSpan.FromTicks(System.DateTime.Now.Ticks).TotalMilliseconds - millisIntoThePast;
int counter = -1;
RotationHistory pastValue = history[++counter];
while (pastValue.Milliseconds < limit && counter < (history.Length - 2))
{
pastValue = history[++counter];
}
return(history[counter].Angles);
}
public void RecordAction(Axis rotAx, int coord, bool cw)
{
RotationHistory.Add(new RotationAction(rotAx, coord, cw));
hudManager.EnableUndo(RotationHistory.Count > 0);
}
public void CleanHistory()
{
RotationHistory.Clear();
hudManager.EnableUndo(false);
}