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);
// Assuming madgwick quaternion returns in [x,y,z,w] for [0,1,2,3]
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.
// Use this for when the IMU is face up
rotation = new Quaternion(-q1, -q0, q2, q3);
// Use this when IMU is upside down
//rotation = 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);
}
static void xIMUserial_CalInertialAndMagneticDataReceived_updateIMU(object sender, CalInertialAndMagneticData e)
{
AHRS.Update(deg2rad(e.Gyroscope[0] - weightCalibrated * biasValue[3]),
deg2rad(e.Gyroscope[1] - weightCalibrated * biasValue[4]),
deg2rad(e.Gyroscope[2] - weightCalibrated * biasValue[5]),
e.Accelerometer[0] - weightCalibrated * biasValue[0],
e.Accelerometer[1] - weightCalibrated * biasValue[1],
e.Accelerometer[2]);
}
/// <summary>
/// xIMUserial CalInertialAndMagneticDataReceived event to update algorithm in AHRS mode.
/// </summary>
static void xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS(object sender, x_IMU_API.CalInertialAndMagneticData e)
{
AHRS.Update(Deg2Rad(e.Gyroscope[0]), Deg2Rad(e.Gyroscope[1]), Deg2Rad(e.Gyroscope[2]), e.Accelerometer[0],
e.Accelerometer[1], e.Accelerometer[2], e.Magnetometer[0], e.Magnetometer[1], e.Magnetometer[2]);
var quaternion = new[] { AHRS.Quaternion[0], -AHRS.Quaternion[1], -AHRS.Quaternion[2], 0 };
var quaternionData = new QuaternionData(quaternion);
var conjugate = quaternionData.ConvertToConjugate();
var rotationMatrix = conjugate.ConvertToRotationMatrix();
_form3DcuboidB.RotationMatrix = rotationMatrix;
_stopWatch.Stop();
_stopWatch.Reset();
}
static void f()
{
List <double> t = new List <double>();
List <double> AccelX = new List <double>();
List <double> AccelY = new List <double>();
List <double> AccelZ = new List <double>();
List <double> GyroX = new List <double>();
List <double> GyroY = new List <double>();
List <double> GyroZ = new List <double>();
List <double> MagX = new List <double>();
List <double> MagY = new List <double>();
List <double> MagZ = new List <double>();
double speed = 0;
string pwd = @"..\..\";
string filename = @"201907141651.txt";
if (File.Exists(pwd + filename))
{
StreamReader sr = new StreamReader(pwd + filename, Encoding.UTF8);
String text = sr.ReadToEnd();
foreach (String line in text.Split('\n'))
{
String[] StrArray;
if (line[0] == '\0') //单片机保存数据时候每一行结尾多了一个\0，处理数据时需要去掉
{
StrArray = line.Remove(0, 1).Split(new char[4] {
':', ';', ',', '\r'
});
}
else
{
StrArray = line.Split(new char[4] {
':', ';', ',', '\r'
});
}
if (StrArray.Length >= 13)
{
t.Add(Convert.ToDouble(StrArray[0]));
AccelX.Add(Convert.ToDouble(StrArray[2]));
AccelY.Add(Convert.ToDouble(StrArray[3]));
AccelZ.Add(Convert.ToDouble(StrArray[4]));
GyroX.Add(Convert.ToDouble(StrArray[6]));
GyroY.Add(Convert.ToDouble(StrArray[7]));
GyroZ.Add(Convert.ToDouble(StrArray[8]));
MagX.Add(Convert.ToDouble(StrArray[10]));
MagY.Add(Convert.ToDouble(StrArray[11]));
MagZ.Add(Convert.ToDouble(StrArray[12]));
}
}
filename = filename.Split('.')[0];
StreamWriter SW = new StreamWriter(filename + "_c.txt");
for (int i = 0; i < t.Count; i++)
{
AHRS.Update((float)GyroX[i], (float)GyroY[i], (float)GyroZ[i], (float)AccelX[i],
(float)AccelY[i], (float)AccelZ[i], (float)MagX[i], (float)MagY[i], (float)MagZ[i]);
float[] acc = { (float)AccelX[i], (float)AccelY[i], (float)AccelZ[i] };
float[] compensate_acc = gravity_compensate(AHRS.Quaternion, acc);
speed += compensate_acc[2] * 9.8 * 0.2; //重力加速度g，采样周期0.2s
//System.Diagnostics.Debug.WriteLine(t[i].ToString((".00")) + ": speed:" + speed.ToString(".00") + " theta:" +
// Math.Acos(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[1] - AHRS.Quaternion[2] * AHRS.Quaternion[3])).ToString(".###") + "°");
SW.WriteLine(t[i].ToString(".00") + ":\t theta:" +
Math.Acos(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[1] - AHRS.Quaternion[2] * AHRS.Quaternion[3])).ToString(".000") + "°");
}
} // end of fileexist
} //end of f()
/// <summary>
/// xIMUserial CalInertialAndMagneticDataReceived event to update algorithm in IMU mode.
/// </summary>
static void xIMUserial_CalInertialAndMagneticDataReceived_updateIMU(object sender, x_IMU_API.CalInertialAndMagneticData e)
{
AHRS.Update(deg2rad(e.Gyroscope[0]), deg2rad(e.Gyroscope[1]), deg2rad(e.Gyroscope[2]), e.Accelerometer[0], e.Accelerometer[1], e.Accelerometer[2]);
}
void CallbackGearVR(GattCharacteristic sender, GattValueChangedEventArgs eventArgs)
{
var buf = eventArgs.CharacteristicValue.ToArray();
if (buf.Length < 3)
{
return;
}
// The data array consist of multiple data which size is 16bytes and footer 12bytes
// On current version, the data array is 60 bytes and dataCount = 3
const int DATA_SIZE = 16;
const int FOOTER_SIZE = 6 + 3 + 1 + 1 + 1; // 12 bytes = 6(mag) + 3(touch) + 1(temperature) + 1(button) + 1(unknown)
int dataCount = (buf.Length - FOOTER_SIZE) / DATA_SIZE;
var mscale = 0.06 * 0.06;
magnetometer[0] = Get16(DATA_SIZE * dataCount + 0, buf) * mscale; // 48 ~ 54
magnetometer[1] = Get16(DATA_SIZE * dataCount + 2, buf) * mscale;
magnetometer[2] = Get16(DATA_SIZE * dataCount + 4, buf) * mscale;
var mmag = Math.Sqrt(magnetometer[0] * magnetometer[0]
+ magnetometer[1] * magnetometer[1] + magnetometer[2] * magnetometer[2]);
for (int i = 0; i < dataCount; i++)
{
var time2 = Get32(DATA_SIZE * i, buf);
double ascale = 9.80665 / 2048.0;
var ax = Get16(DATA_SIZE * i + 4, buf) * ascale;
var ay = Get16(DATA_SIZE * i + 6, buf) * ascale;
var az = Get16(DATA_SIZE * i + 8, buf) * ascale;
var amag = Math.Sqrt(ax * ax + ay * ay + az * az);
acceleration[0] = ax;
acceleration[1] = ay;
acceleration[2] = az;
double gscale = 0.017453292 / 14.285;
var gx = Get16(DATA_SIZE * i + 10, buf) * gscale;
var gy = Get16(DATA_SIZE * i + 12, buf) * gscale;
var gz = Get16(DATA_SIZE * i + 14, buf) * gscale;
var gmag = Math.Sqrt(gx * gx + gy * gy + gz * gz);
gyroscope[0] = gx;
gyroscope[1] = gy;
gyroscope[2] = gz;
AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)magnetometer[0], (float)magnetometer[1], (float)magnetometer[2]);
}
// We need to fix the difference of coordinate system between AHRS and VR app.
// I don't know how to introduce correct conversion, but it seems roughly right (working).
quaternion[0] = AHRS.Quaternion[1];
quaternion[1] = AHRS.Quaternion[3];
quaternion[2] = -AHRS.Quaternion[2];
quaternion[3] = AHRS.Quaternion[0];
position = ArmModel.CalculateModel(quaternion);
temperature = buf[DATA_SIZE * dataCount + 6 + 3]; // 57
int touchPos = DATA_SIZE * dataCount + 6; // 54
var touchFlag = ParseInt(touchPos * 8, touchPos * 8 + 4, false, buf);
var tx = ParseInt(touchPos * 8 + 4, touchPos * 8 + 14, false, buf);
var ty = ParseInt(touchPos * 8 + 14, touchPos * 8 + 24, false, buf);
double tscale = 1.0 / 320.0;
// from -1.0(left) to +1.0(right)
trackpad[0] = tx * tscale * 2.0 - 1.0;
// from -1.0(bottom) to +1.0(top)
trackpad[1] = 1.0 - ty * tscale * 2.0;
button[TOUCH] = touchFlag == 1;
// trigger, home, back, touch click, vol up, vol down
int buttonPos = DATA_SIZE * dataCount + 6 + 3 + 1; // 58;
button[TRIGGER] = (buf[buttonPos] & (1 << 0)) != 0;
button[HOME] = (buf[buttonPos] & (1 << 1)) != 0;
button[APP] = (buf[buttonPos] & (1 << 2)) != 0;
button[CLICK] = (buf[buttonPos] & (1 << 3)) != 0;
button[VOLUP] = (buf[buttonPos] & (1 << 4)) != 0;
button[VOLDOWN] = (buf[buttonPos] & (1 << 5)) != 0;
version = buf[DATA_SIZE * dataCount + 6 + 3 + 1 + 1]; // 59
}
private static void Processing1()
{
while (true)
{
lock (data1)
{
while (data1.Count < 50)
{
Monitor.Wait(data1);
}
int i = 1;
while (i == 1)
{
RxPkt1[0] = data1.Dequeue();
if (RxPkt1[0] == 16)
{
RxPkt1[1] = data1.Dequeue();
if (RxPkt1[1] == 1)
{
int j = 1;
int k = 2;
while (j == 1)
{
RxPkt1[k] = data1.Dequeue();
if (RxPkt1[k] == 16)
{
RxPkt1[k + 1] = data1.Dequeue();
if (RxPkt1[k + 1] == 4)
{
j = 0;
i = 0;
}
}
k++;
}
}
}
}
byte[] convert = new byte[26];
for (i = 0; i < 26; i++)
{
convert[i] = RxPkt1[i];
}
Array.Reverse(convert);//operating system is little endians while the package is big endians, reverse the array.
float[] Accelerometer = new float[3];
float[] Gyroscope = new float[3];
float[] Mag = new float[3];
Accelerometer[0] = (float)(BitConverter.ToInt16(convert, 22)) / 4096;
Accelerometer[1] = (float)(BitConverter.ToInt16(convert, 20)) / 4096;
Accelerometer[2] = (float)(BitConverter.ToInt16(convert, 18)) / 4096;
Gyroscope[0] = (float)(BitConverter.ToInt16(convert, 16)) / (float)32.75;
Gyroscope[1] = (float)(BitConverter.ToInt16(convert, 14)) / (float)32.75;
Gyroscope[2] = (float)(BitConverter.ToInt16(convert, 12)) / (float)32.75;
if (Gyroscope[0] < 3 && Gyroscope[0] > -3)
{
Gyroscope[0] = 0;
}
if (Gyroscope[1] < 3 && Gyroscope[1] > -3)
{
Gyroscope[1] = 0;
}
if (Gyroscope[2] < 3 && Gyroscope[2] > -3)
{
Gyroscope[2] = 0;
}
thisRTime = toMs(RxPkt1[23], RxPkt1[24], timerOffsetFromOverflow);
if (thisRTime < lastRTime)
{
overflowsTime += lastRTime;
timerOffsetFromOverflow = 65536 - lastTimerValue;
offsetCombined = (uint)((RxPkt1[23] << 8) + RxPkt1[24] + timerOffsetFromOverflow);
thisRTime = toMs(RxPkt1[23], RxPkt1[24], timerOffsetFromOverflow);
}
lastRTime = thisRTime;
lastTimerValue = (uint)((RxPkt1[23] << 8) + RxPkt1[24]);
time1 = (thisRTime + overflowsTime - firstRTime) / 1000;
form_3DcuboidA.RotationMatrix = ConvertToRotateMatrix(AHRS1.Quaternion);
form_3DcuboidB.RotationMatrix = ConvertToRotateMatrix(AHRS.Quaternion);
// these two lines call the filtering algorithm.
AHRS1.Update(-deg2rad(Gyroscope[0]), deg2rad(Gyroscope[1]), -deg2rad(Gyroscope[2]), -Accelerometer[0], Accelerometer[1], -Accelerometer[2], time1);
AHRS.Update(-deg2rad(Gyroscope[0]), deg2rad(Gyroscope[1]), -deg2rad(Gyroscope[2]), -Accelerometer[0], Accelerometer[1], -Accelerometer[2], time1);
}
}
}
static void f(string FullPath)
{
List <double> t = new List <double>();
List <double> AccelX = new List <double>();
List <double> AccelY = new List <double>();
List <double> AccelZ = new List <double>();
List <double> GyroX = new List <double>();
List <double> GyroY = new List <double>();
List <double> GyroZ = new List <double>();
List <double> MagX = new List <double>();
List <double> MagY = new List <double>();
List <double> MagZ = new List <double>();
double speed = 0;
string pwd = @"..\..\";
string filename = @"201907260808.txt";
string Path = System.IO.Path.GetDirectoryName(FullPath); //获取文件路径
string Name = System.IO.Path.GetFileName(FullPath); //获取文件名
//System.Diagnostics.Debug.WriteLine(Directory.GetCurrentDirectory());
if (File.Exists(Path + Name))
{
StreamReader sr = new StreamReader(Path + Name, Encoding.UTF8);
String text = sr.ReadToEnd();
foreach (String line in text.Split('\n'))
{
String[] StrArray;
if (line[0] == '\0') //单片机保存数据时候每一行结尾多了一个\0，处理数据时需要去掉
{
StrArray = line.Remove(0, 1).Split(new char[4] {
':', ';', ',', '\r'
});
}
else
{
StrArray = line.Split(new char[4] {
':', ';', ',', '\r'
});
}
if (StrArray.Length >= 10)
{
//t.Add(Convert.ToDouble(StrArray[0]));
//AccelX.Add(Convert.ToDouble(StrArray[2]));
//AccelY.Add(Convert.ToDouble(StrArray[3]));
//AccelZ.Add(Convert.ToDouble(StrArray[4]));
//GyroX.Add(Convert.ToDouble(StrArray[6]));
//GyroY.Add(Convert.ToDouble(StrArray[7]));
//GyroZ.Add(Convert.ToDouble(StrArray[8]));
//MagX.Add(Convert.ToDouble(StrArray[10]));
//MagY.Add(Convert.ToDouble(StrArray[11]));
//MagZ.Add(Convert.ToDouble(StrArray[12]));
t.Add(Convert.ToDouble(StrArray[0]));
AccelX.Add(Convert.ToDouble(StrArray[1]));
AccelY.Add(Convert.ToDouble(StrArray[2]));
AccelZ.Add(Convert.ToDouble(StrArray[3]));
GyroX.Add(Convert.ToDouble(StrArray[4]));
GyroY.Add(Convert.ToDouble(StrArray[5]));
GyroZ.Add(Convert.ToDouble(StrArray[6]));
MagX.Add(Convert.ToDouble(StrArray[7]));
MagY.Add(Convert.ToDouble(StrArray[8]));
MagZ.Add(Convert.ToDouble(StrArray[9]));
}
}
Name = Name.Split('.')[0]; //去掉文件后缀
//StreamWriter SW = new StreamWriter(Name + "_c.txt");
double compAngleX = 0;
double compAngleY = 0;
for (int i = 0; i < t.Count; i++)
{
//AHRS.Update(0, 0, 0, 0, 0.5f, 0.5f, 0.3f, 0.2f, 0.1f);
AHRS.Update(deg2rad(GyroX[i]), deg2rad(GyroY[i]), deg2rad(GyroZ[i]), AccelX[i],
AccelY[i], AccelZ[i], MagX[i], MagY[i], MagZ[i]);
//AHRS.Update((double)deg2rad(GyroX[i]), (double)deg2rad(GyroY[i]), (double)deg2rad(GyroZ[i]), (double)AccelX[i],
// (double)AccelY[i], (double)AccelZ[i]);
//double[] acc = { (double)AccelX[i], (double)AccelY[i], (double)AccelZ[i] };
//double[] compensate_acc = gravity_compensate(AHRS.Quaternion, acc);
//speed += compensate_acc[2] * 9.8 * 0.2; //重力加速度g，采样周期0.2s
//Console.Write(i.ToString() + AHRS.Quaternion[0].ToString(" 0.0000000 ")+ AHRS.Quaternion[1].ToString("0.0000000 ")+AHRS.Quaternion[2].ToString("0.0000000 ")+ AHRS.Quaternion[3].ToString("0.0000000 "));
double[] q = { AHRS.Quaternion[0], AHRS.Quaternion[1], AHRS.Quaternion[2], AHRS.Quaternion[3] };
double[] Angle = quatern2euler(quaternConj(q));
double roll = Math.Atan2(AccelY[i], AccelZ[i]) / Math.PI * 180;
double pitch = Math.Atan(-AccelX[i] / Math.Sqrt(AccelY[i] * AccelY[i] + AccelZ[i] * AccelZ[i])) / Math.PI * 180;
compAngleX = roll;
compAngleY = pitch;
double gyroXrate = deg2rad(GyroX[i]);
double gyroYrate = deg2rad(GyroY[i]);
compAngleX = 0.93 * (compAngleX + gyroXrate / 256) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate / 256) + 0.07 * pitch;
Console.Write(" roll: " + compAngleX.ToString("0.000") + ", pitch: " + compAngleX.ToString("0.000 "));
//System.Diagnostics.Debug.WriteLine(t[i].ToString((".00")) + ": speed:" + speed.ToString(".00") + " theta:" +
// Math.Acos(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[1] - AHRS.Quaternion[2] * AHRS.Quaternion[3])).ToString(".###") + "°");
//SW.WriteLine(t[i].ToString(".00") + ":\t theta:" +Math.Tan(
// Math.Asin(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[1] - AHRS.Quaternion[2] * AHRS.Quaternion[3]))).ToString(".000") + "%");
//四元数转换成欧拉角
//double phi = Math.Atan2((2 * (AHRS.Quaternion[0] * AHRS.Quaternion[3])), (1 - 2 * (AHRS.Quaternion[3] * AHRS.Quaternion[3] + AHRS.Quaternion[1] * AHRS.Quaternion[1]))) / Math.PI * 180;
//double theta = Math.Asin(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[1] - AHRS.Quaternion[2] * AHRS.Quaternion[3])) / Math.PI * 180;
//double psi = Math.Atan2(2 * (AHRS.Quaternion[0] * AHRS.Quaternion[2]), (1 - 2 * (AHRS.Quaternion[1] * AHRS.Quaternion[1] + AHRS.Quaternion[2] * AHRS.Quaternion[2]))) / Math.PI * 180;
//double phi = Math.Atan2((2 * (AHRS.Quaternion[1] * AHRS.Quaternion[2] - AHRS.Quaternion[0] * AHRS.Quaternion[3])), 2 * (AHRS.Quaternion[0] * AHRS.Quaternion[0] + AHRS.Quaternion[1] * AHRS.Quaternion[1]) - 1) / Math.PI * 180;
//double theta = -Math.Asin(2 * (AHRS.Quaternion[1] * AHRS.Quaternion[3] + AHRS.Quaternion[0] * AHRS.Quaternion[2])) / Math.PI * 180;
//double psi = Math.Atan2(2 * (AHRS.Quaternion[2] * AHRS.Quaternion[3] + AHRS.Quaternion[0] * AHRS.Quaternion[1]), 2 * (AHRS.Quaternion[0] * AHRS.Quaternion[0] + AHRS.Quaternion[3] * AHRS.Quaternion[3] )- 1) / Math.PI * 180;
Console.WriteLine(" phi: " + Angle[0].ToString("0.000") + ", theta: " + Angle[1].ToString("0.000") + ", psi: " + Angle[2].ToString("0.000"));
//SW.WriteLine("phi: " + phi.ToString(".000") + ", theta: " + theta.ToString(".000") + ", psi: " + psi.ToString(".000"));
}
//SW.Close();
} // end of fileexist
else
{
Console.WriteLine("文件不存在");
}
} //end of f()