void myo_OnAccelerometerData(object sender, Thalmic.Myo.AccelerometerDataEventArgs e) { lock (_lock) { _myoAccelerometer = e.Accelerometer; } }
//gyroscop and accelerometer arrive together. nice. private void accelerometerReceived(object sender, AccelerometerDataEventArgs e) { lock (_lock) { accelerometerEvents.Add(e); ++accelerometerDataCount; } }
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; // } //} } }
private Quaternion calculateCorrectionForAccel(AccelerometerDataEventArgs prev, AccelerometerDataEventArgs effective, Transform currentTransform) { TimeSpan dts = effective.Timestamp.Subtract(prev.Timestamp); double dt = dts.TotalSeconds; // make this local // we have a local up vector. draw it. // TODO this needs to be a rotation taht will be applied after the current orientation. right now it isn't. fix it. // this is more like desiredLocalAccelUp. so, turn this to become the actual local up Vector3 localAccelUp = myoToUnity(effective.Accelerometer); Debug.DrawRay(currentTransform.position, localAccelUp, Color.red, Time.deltaTime, false); localAccelUp.Normalize(); //// these are the same. proof. //Debug.Log(currentTransform.position); //Debug.Log(currentTransform.localToWorldMatrix.GetColumn(3)); //Debug.Log(currentTransform.forward); //Debug.Log(currentTransform.localToWorldMatrix.GetColumn(2)); //yvec = M . loccurrup. so, the middle row of rot mat //this is the current actual up, in local coordinates Vector3 localCurrentWorldUp = currentTransform.localToWorldMatrix.GetRow(1); //Vector3 localCurrentUp = currentTransform.localToWorldMatrix.GetRow(1); Vector3 actualCurrentWorldUp = currentTransform.TransformDirection(localCurrentWorldUp); //Debug.DrawRay(currentTransform.position, localCurrentUp, Color.cyan, Time.deltaTime, false); //Debug.DrawRay(currentTransform.position, actualCurrentUp, Color.white, Time.deltaTime, false); // so, I'll turn so that localAccelUp becomes localCurrentWorldUp. this is a rotation in local coords. // I can find that axis easily in world coords, since localCurrentWorldUp is world's Vector3.up Vector3 worldAccelUp = currentTransform.TransformDirection(localAccelUp); // turn this to be y // like LaValle says. same because left hand rule AND left handed coord frame. Vector3 axis = new Vector3(-worldAccelUp.z, 0, worldAccelUp.x); // TODO this axis may not be correct // TODO the only uncertainty that I have is this angle calculation. maybe I should calculate it myself? since its wrt Vector3.up? float angle = Vector3.Angle(worldAccelUp, Vector3.up); // now, transform this axis into local coords, and apply the rotation there Vector3 localAxis = currentTransform.InverseTransformDirection(axis); //Debug.DrawRay(currentTransform.position, worldAccelUp, Color.blue, Time.deltaTime, false); Debug.DrawRay(currentTransform.position, localCurrentWorldUp, Color.white, Time.deltaTime, false); Debug.DrawRay(currentTransform.position, localAxis, Color.green, Time.deltaTime, false); //Debug.Log(currentTransform.name); //Debug.Log("angle " + angle); // now do the rotation in local coords return(Quaternion.AngleAxis((float)(angle * lowpassNewContribution * dt), localAxis.normalized)); //return Quaternion.identity; //Vector3 currentUp = Vector3.up; //Vector3 accelup = currentTransform.TransformDirection(localAccelUp); //float angle = Vector3.Angle(currentUp, accelup); //Vector3 axis = new Vector3(-accelup.z, 0, accelup.x); // TODO this axis may not be correct //axis.Normalize(); ////TODO normalize axis? ////Debug.DrawRay(currentTransform.position, currentUp, Color.white, Time.deltaTime, false); ////Debug.DrawRay(currentTransform.position, accelup, Color.red, Time.deltaTime, false); ////Debug.DrawRay(currentTransform.position, axis, Color.blue, Time.deltaTime, false); ////TODO this quaternion may be flipped. check it. //Quaternion correction = Quaternion.AngleAxis((float)(angle * lowpassNewContribution * dt), axis); ////here I will set it immediately to debug it ////accelerometerLowpass.localRotation = Quaternion.AngleAxis(angle, axis); //correction = Quaternion.identity; //return correction; }