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