private void gyroscopeReceived(object sender, GyroscopeDataEventArgs e)
{
lock (_lock) {
gyroscopeEvents.Add(e);
//if (firstGyroMeasurement) {
// firstGyroMeasurement = false;
//} else {
// TimeSpan dt = e.Timestamp.Subtract(lastTimeStamp);
// Thalmic.Myo.Vector3 avg = (e.Gyroscope + lastGyroscope) * .5f;
// double dts = dt.TotalSeconds;
// double rx = avg.X * dts;
// double ry = avg.Y * dts;
// double rz = avg.Z * dts;
// double magnitude = Math.Sqrt(rx * rx + ry * ry + rz * rz);
// Vector3 myoAxis = new Vector3((float)(rx / magnitude), (float)(ry / magnitude), (float)(rz / magnitude));
// Vector3 axis = new Vector3(-myoAxis.y, myoAxis.z, myoAxis.x);
// float angle = (float)magnitude;
// Quaternion dq = Quaternion.AngleAxis(angle, axis);
// cumulativeDq = cumulativeDq * dq;
// cumulativeHasQ = true;
//}
//lastTimeStamp = e.Timestamp;
//lastGyroscope = e.Gyroscope;
//++gyroscopeDataCount;
}
}
void myo_OnGyroscopeData(object sender, Thalmic.Myo.GyroscopeDataEventArgs e)
{
lock (_lock)
{
_myoGyroscope = e.Gyroscope;
}
}
private Quaternion calculateChangeForGyro(GyroscopeDataEventArgs prev, GyroscopeDataEventArgs effective)
{
TimeSpan dt = effective.Timestamp.Subtract(prev.Timestamp);
Thalmic.Myo.Vector3 avg = (effective.Gyroscope + prev.Gyroscope) * .5f;
double dts = dt.TotalSeconds;
double rx = avg.X * dts;
double ry = avg.Y * dts;
double rz = avg.Z * dts;
double magnitude = Math.Sqrt(rx * rx + ry * ry + rz * rz);
Vector3 myoAxis = new Vector3((float)(rx / magnitude), (float)(ry / magnitude), (float)(rz / magnitude));
Vector3 axis = new Vector3(-myoAxis.y, myoAxis.z, myoAxis.x);
float angle = (float)magnitude;
// Unity is left handed. therefore the angle should be reversed.
Quaternion dq = Quaternion.AngleAxis(-angle, axis);
return(dq);
}
public void Update()
{
lock (_lock) {
// we sure can't do it online since we need to know the next gyroscope reading
// doing it precisely can be a bit inefficient (have to slice the gyroscope values with accel times)
// so, we'll have two modes:
// 1. precise mode
// 2. imprecise mode
// precise mode is confusing. may not even exist...
// make sure the values are ordered in time, so that we can merge them.
enforceOrder(gyroscopeEvents);
enforceOrder(accelerometerEvents);
var gyroEnum = gyroscopeEvents.GetEnumerator();
var accelEnum = accelerometerEvents.GetEnumerator();
bool gyroHasNext = gyroEnum.MoveNext();
bool accelHasNext = accelEnum.MoveNext();
while (gyroHasNext || accelHasNext)
{
bool useGyro = false;
bool useAccel = false;
if (gyroHasNext && accelHasNext)
{
int gyroIsLater = DateTime.Compare(gyroEnum.Current.Timestamp, accelEnum.Current.Timestamp);
if (gyroIsLater == 0)
{
// both are at the same time
useGyro = true;
useAccel = true;
}
else if (gyroIsLater > 0)
{
// accel is before
useAccel = true;
}
else
{
// gyro is before
useGyro = true;
}
}
else
{
useGyro = gyroHasNext;
useAccel = accelHasNext;
}
bool moveWithGyro = false;
bool moveWithAccel = false;
// by now, I should
// have the effective value for both
// have the necessary one(s) advance
if (useGyro)
{
prevGyro = effectiveGyro;
effectiveGyro = gyroEnum.Current;
gyroHasNext = gyroEnum.MoveNext();
//Debug.Log("1");
if (prevGyro != null)
{
//Debug.Log("2");
moveWithGyro = true;
}
}
if (useAccel)
{
prevAccel = effectiveAccel;
effectiveAccel = accelEnum.Current;
accelHasNext = accelEnum.MoveNext();
//Debug.Log("3");
if (prevAccel != null)
{
//Debug.Log("4");
moveWithAccel = true;
}
}
if (moveWithGyro && moveWithAccel)
{
// TODO think about averaging these two
if (accelerometerLowpass != null)
{
Quaternion accelCorrectionForAccelOnly = calculateCorrectionForAccel(prevAccel, effectiveAccel, accelerometerLowpass);
applyAccelCorrection(accelerometerLowpass, accelCorrectionForAccelOnly);
}
if (complementaryFilter != null)
{
Quaternion accelCorrectionForComplOnly = calculateCorrectionForAccel(prevAccel, effectiveAccel, complementaryFilter);
applyAccelCorrection(complementaryFilter, accelCorrectionForComplOnly);
Quaternion gyroChange = calculateChangeForGyro(prevGyro, effectiveGyro);
applyGyroChange(complementaryFilter, gyroChange);
}
if (gyroscopeIntegrated != null)
{
Quaternion gyroChange = calculateChangeForGyro(prevGyro, effectiveGyro);
applyGyroChange(gyroscopeIntegrated, gyroChange);
}
}
else
{
if (moveWithAccel)
{
// TODO calculate and apply (time will go in here, will scale the contrib.)
Debug.Log("move with accel");
// currentup should always be Vector3.up
// localaccelup should be the vector in local coords. this is what we read.
// accelup should be that vector in global space
// this does not require matrix inversion. good.
if (accelerometerLowpass != null)
{
Quaternion accelCorrectionForAccelOnly = calculateCorrectionForAccel(prevAccel, effectiveAccel, accelerometerLowpass);
//this is the actual one
applyAccelCorrection(accelerometerLowpass, accelCorrectionForAccelOnly);
// TODO test this accelerometer thing first. then, merge it with gyro.
}
if (complementaryFilter != null)
{
Quaternion accelCorrectionForComplOnly = calculateCorrectionForAccel(prevAccel, effectiveAccel, complementaryFilter);
applyAccelCorrection(complementaryFilter, accelCorrectionForComplOnly);
}
}
else if (moveWithGyro)
{
// TODO calculate and apply
Debug.Log("move with gyro");
if (gyroscopeIntegrated != null)
{
Quaternion gyroChange = calculateChangeForGyro(prevGyro, effectiveGyro);
applyGyroChange(gyroscopeIntegrated, gyroChange);
}
if (complementaryFilter != null)
{
//this is the actual one
// TODO test this accelerometer thing first. then, merge it with gyro.
Quaternion gyroChange = calculateChangeForGyro(prevGyro, effectiveGyro);
applyGyroChange(complementaryFilter, gyroChange);
}
}
}
} //this leaves effective gyro and accel set. what I should do is to initialize them in the very first use.
gyroscopeEvents.Clear();
accelerometerEvents.Clear();
// eat up the data.
// I have a couple of gyro and accel readings
// now order them
// for each time duration,
// integrate gyro (avg with last value for area)
// use
//Debug.Log(gyroscopeDataCount + " " + accelerometerDataCount);
//gyroscopeDataCount = accelerometerDataCount = 0;
//if (gyroscopeIntegrated != null) {
// if (cumulativeHasQ) {
// gyroscopeIntegrated.localRotation = gyroscopeIntegrated.localRotation * cumulativeDq;
// cumulativeDq = Quaternion.identity;
// cumulativeHasQ = false;
// }
//}
}
}
