/// <summary>
/// Updates the values saved for the IMU device during the calibration phase. Begins to call the
/// Madgwick algorithm once the threshold has been met.
/// </summary>
/// <param name="values"> Short values converted from the raw integer values.</param>
public void calibrate(short[] values)
{
calibration.countdown--;
if (calibration.countdown < calibration_trigger)
{
calibration.adjust("GYRO", "X", values[0]);
calibration.adjust("GYRO", "Y", values[1]);
calibration.adjust("GYRO", "Z", values[2]);
float AX = values[6] - calibration.get("ACCEL", "X");
float AY = values[7] - calibration.get("ACCEL", "Y");
float AZ = values[8] - calibration.get("ACCEL", "Z");
float MX = values[3] - calibration.get("MAG", "X");
float MY = values[4] - calibration.get("MAG", "Y");
float MZ = -(values[5] - calibration.get("MAG", "Z"));
update(0, 0, 0, AX, AY, AZ, MX, MY, MZ);
}
if (calibration.countdown == 0)
{
calibration.set("GYRO", "X", calibration.get("GYRO", "X") / calibration_trigger);
calibration.set("GYRO", "Y", calibration.get("GYRO", "Y") / calibration_trigger);
calibration.set("GYRO", "Z", calibration.get("GYRO", "Z") / calibration_trigger);
calibration.set_calibrated(true);
}
}
public int processData(List <int> message, int mode, bool docked)
{
// accel_data is array of the data from the accelerometer in the following format: G_X, G_Y, G_Z, M_X, M_Y, M_Z, A_X, A_Y, A_Z
int[] accel_data = new int[] {
message[0] + ((message[1]) << 8),
message[2] + ((message[3]) << 8),
(message[4] + ((message[5]) << 8)),
message[6] + ((message[7]) << 8),
message[8] + ((message[9]) << 8),
(message[10] + ((message[11]) << 8)),
message[12] + ((message[13]) << 8),
message[14] + ((message[15]) << 8),
(message[16] + ((message[17]) << 8))
};
long GyroValue = ((Math.Abs(accel_data[0]) + Math.Abs(accel_data[1]) + Math.Abs(accel_data[2])) / 3);
if (mode == 2)
{
short counter = 0;
short[] accel_data_short = new short[accel_data.Count()];
foreach (Int32 value in accel_data)
{
accel_data_short[counter] = Convert.ToInt16((accel_data[counter]) & 32767);
if (((accel_data[counter]) & 32768) != 0)
{
accel_data_short[counter] = Convert.ToInt16(-32768 + accel_data_short[counter]);
}
counter++;
}
try
{
GyroValue = ((Math.Abs(accel_data_short[0]) + Math.Abs(accel_data_short[1]) + Math.Abs(accel_data_short[2])) / 3);
}
catch (Exception)
{
}
if (cali == null)
{
sendData("M0dg");
return(0);
}
if (!cali.calibrated)
{
sample_period = samplePeriod;
samplePeriod = 0.2f;
cali.caliCountDown--;
if (cali.caliCountDown < 150)
{
cali.adjust("GYRO", "X", accel_data_short[0]);
cali.adjust("GYRO", "Y", accel_data_short[1]);
cali.adjust("GYRO", "Z", accel_data_short[2]);
float AX = accel_data_short[6] - cali.get("ACCEL", "X");
float AY = accel_data_short[7] - cali.get("ACCEL", "Y");
float AZ = accel_data_short[8] - cali.get("ACCEL", "Z");
float MX = accel_data_short[3] - cali.get("MAG", "X");
float MY = accel_data_short[4] - cali.get("MAG", "Y");
float MZ = -(accel_data_short[5] - cali.get("MAG", "Z"));
Update(0, 0, 0, AX, AY, AZ, MX, MY, MZ);
samplePeriod = sample_period;
}
// samplePeriod = sample_period;
cali.caliCountDown--;
cali.adjust("GYRO", "X", accel_data_short[0]);
cali.adjust("GYRO", "Y", accel_data_short[1]);
cali.adjust("GYRO", "Z", accel_data_short[2]);
if (cali.caliCountDown == 0)
{
cali.set("GYRO", "X", cali.get("GYRO", "X") / 100);
cali.set("GYRO", "Y", cali.get("GYRO", "Y") / 100);
cali.set("GYRO", "Z", cali.get("GYRO", "Z") / 100);
cali.set_calibrated(true);
}
}
else
{
cali.set("GYRO", "X", 0);
cali.set("GYRO", "Y", 0);
cali.set("GYRO", "Z", 0);
float GX = ((((float)(accel_data_short[0]) - (float)cali.get("GYRO", "X") * 500f * (float)Math.PI) /
(32758f * 180f)));
float GY = ((((float)(accel_data_short[1]) - (float)cali.get("GYRO", "Y") * 500f * (float)Math.PI) /
(32758f * 180f)));
float GZ = ((((float)(accel_data_short[2]) - (float)cali.get("GYRO", "Z")) * 500f * (float)Math.PI) /
(32758f * 180f));
float AX = accel_data_short[6] - cali.get("ACCEL", "X");
float AY = accel_data_short[7] - cali.get("ACCEL", "Y");
float AZ = accel_data_short[8] - cali.get("ACCEL", "Z");
float MX = accel_data_short[3] - cali.get("MAG", "X");
float MY = accel_data_short[4] - cali.get("MAG", "Y");
float MZ = -(accel_data_short[5] - cali.get("MAG", "Z"));
Update(GX, GY, GZ, AX, AY, AZ, MX, MY, MZ);
}
if (Kat) ///FIND
{
Quaternion quat2 = quat.inverse();
Quaternion quat3 = Quaternion.roatation_YPR(0, (((float)Math.PI) / 2), 0).product(quat2);
Quaternion quat4 = Quaternion.roatation_YPR(0, -(((float)Math.PI) / 2), 0).inverse();
Quaternion quat5 = quat3.product(quat4);
byte W = (byte)((quat5.W * 125) + 125);
byte X = (byte)((quat5.X * 125) + 125);
byte Y = (byte)((quat5.Y * 125) + 125);
byte Z = (byte)((quat5.Z * 125) + 125);
//Z = Z * -1;
byte M = 0;
unitydata = new byte[] { 255, (byte)(address), W, X, Y, Z, M, (byte)(address), };
}
if (docked)
{
try {
}
catch (Exception e)
{
if (e.HResult == -2147467261)
{
StopNU();
}
}
}
}
if (mode == 3)
{
short counter = 0;
short[] accel_data_short = new short[accel_data.Count()];
foreach (Int32 value in accel_data)
{
accel_data_short[counter] = Convert.ToInt16((accel_data[counter]) & 32767);
if (((accel_data[counter]) & 32768) != 0)
{
accel_data_short[counter] = Convert.ToInt16(-32768 + accel_data_short[counter]);
}
counter++;
}
GyroValue = ((Math.Abs(accel_data_short[0]) + Math.Abs(accel_data_short[1]) + Math.Abs(accel_data_short[2])) / 3);
float GX = ((((float)accel_data_short[0] - (float)cali.get("GYRO", "X")) * 500f * (float)Math.PI) / (32758f * 180f));
float GY = ((((float)accel_data_short[1] - (float)cali.get("GYRO", "Y")) * 500f * (float)Math.PI) / (32758f * 180f));
float GZ = ((((float)accel_data_short[2] - (float)cali.get("GYRO", "Z")) * 500f * (float)Math.PI) / (32758f * 180f));
int AX = (int)accel_data_short[6] - cali.get("ACCEL", "X");
int AY = (int)accel_data_short[7] - cali.get("ACCEL", "Y");
int AZ = accel_data_short[8] - cali.get("ACCEL", "Z");
int MX = ((int)accel_data_short[3] - (int)cali.get("MAG", "X"));
int MY = ((int)accel_data_short[4] - (int)cali.get("MAG", "Y"));
int MZ = -((int)accel_data_short[5] - (int)cali.get("MAG", "Z"));
Update(GX, GY, GZ, AX, AY, AZ, MX, MY, MZ);
Vector3D mag = new Vector3D(accel_data_short[3], accel_data_short[4], accel_data_short[5]);
Vector3D accel = new Vector3D(accel_data_short[6], accel_data_short[7], accel_data_short[8]);
//mag = Vector3.Normalize(mag);
accel.Normalize();
mag.Normalize();
float angle = (float)RadianToDegree(Vector3D.DotProduct(mag, accel));
byte[] byteArrayW = BitConverter.GetBytes(quat.W);
byte[] byteArrayX = BitConverter.GetBytes(quat.X);
byte[] byteArrayY = BitConverter.GetBytes(quat.Y);
byte[] byteArrayZ = BitConverter.GetBytes(quat.Z);
byte[] byteArrayMX = BitConverter.GetBytes(MX);
byte[] byteArrayMY = BitConverter.GetBytes(MY);
byte[] byteArrayMZ = BitConverter.GetBytes(MZ);
WriteDataToBox(string.Join(",", accel_data.ToArray()));
WriteDataToBox("\n");
DataReady(new byte[] { 6,
(byte)message[0],
(byte)message[1],
(byte)message[2],
(byte)message[3],
(byte)message[4],
(byte)message[5],
(byte)message[6],
(byte)message[7],
(byte)message[8],
(byte)message[9],
(byte)message[10],
(byte)message[11],
(byte)message[12],
(byte)message[13],
(byte)message[14],
(byte)message[15],
(byte)message[16],
(byte)message[17],
byteArrayW[0],
byteArrayW[1],
byteArrayW[2],
byteArrayW[3],
byteArrayX[0],
byteArrayX[1],
byteArrayX[2],
byteArrayX[3],
byteArrayY[0],
byteArrayY[1],
byteArrayY[2],
byteArrayY[3],
byteArrayZ[0],
byteArrayZ[1],
byteArrayZ[2],
byteArrayZ[3], });
}
return((int)GyroValue);
}