public void AddData(MainWindow.MpuData mpuData) { _bound = 0; var mpuDataArray = new double[6]; mpuDataArray[0] = mpuData.AccX; mpuDataArray[1] = mpuData.AccY; mpuDataArray[2] = mpuData.AccZ; mpuDataArray[3] = mpuData.GyrX; mpuDataArray[4] = mpuData.GyrY; mpuDataArray[5] = mpuData.GyrZ; for (var i = 0; i < 6; ++i) { if (mpuDataArray[i] > 2000) { return; } if (double.IsNaN(mpuDataArray[i])) { return; } if (double.IsInfinity(mpuDataArray[i])) { return; } } for (var i = 0; i < 6; ++i) { _data[i].Add(mpuDataArray[i]); while (_data[i].Count > DataLength) { _data[i].RemoveAt(0); } if (!_show[i]) { continue; } for (var j = 0; j < _data[i].Count; ++j) { var abs = Math.Abs((double)_data[i][j]); if (abs > _bound) { _bound = abs; } } } _eularAngle = _calculator.UpdateData(mpuData); }
public Vector3 UpdateData(MainWindow.MpuData mpuData) { var a = new Vector3(mpuData.AccX, mpuData.AccY, mpuData.AccZ); var g = new Vector3(mpuData.GyrX, mpuData.GyrY, mpuData.GyrZ); Vector3 v; Vector3 e; var q0 = Q.X; var q1 = Q.Y; var q2 = Q.Z; var q3 = Q.W; var q0Q0 = q0 * q0; var q0Q1 = q0 * q1; var q0Q2 = q0 * q2; var q0Q3 = q0 * q3; var q1Q1 = q1 * q1; var q1Q2 = q1 * q2; var q1Q3 = q1 * q3; var q2Q2 = q2 * q2; var q2Q3 = q2 * q3; var q3Q3 = q3 * q3; a = Vector3.Normalize(a); v = new Vector3(2 * (q1Q3 - q0Q2), 2 * (q0Q1 + q2Q3), q0Q0 - q1Q1 - q2Q2 + q3Q3); e = new Vector3(a.Y * v.Z - a.Z * v.Y, a.Z * v.X - a.X * v.Z, a.X * v.Y - a.Y * v.X); _intError += e * Ki; g += Kp * e + _intError; q0 = q0 + (-q1 * g.X - q2 * g.Y - q3 * g.Z) * _halfT; q1 = q1 + (q0 * g.X + q2 * g.Z - q3 * g.Y) * _halfT; q2 = q2 + (q0 * g.Y - q1 * g.Z + q3 * g.X) * _halfT; q3 = q3 + (q0 * g.Z + q1 * g.Y - q2 * g.X) * _halfT; Q.X = q0; Q.Y = q1; Q.Z = q2; Q.W = q3; Q = Quaternion.Normalize(Q); var eax = Math.Atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.296; var eay = Math.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.296; var eaz = Math.Atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.296; var eularAngle = new Vector3((float)eax, (float)eay, (float)eaz); return(eularAngle); }