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