void UpdateSD()
{
sendClearData(7);
sendClearData(67);
sendClearData(12);
sendClearData((byte)(cali.get("MAG", "X") >> 8));
sendClearData((byte)(cali.get("MAG", "X")));
sendClearData(11);
System.Threading.Thread.Sleep(100);
sendClearData(7);
sendClearData(67);
sendClearData(13);
sendClearData((byte)(cali.get("MAG", "Y") >> 8));
sendClearData((byte)(cali.get("MAG", "Y")));
sendClearData(11);
System.Threading.Thread.Sleep(100);
sendClearData(7);
sendClearData(67);
sendClearData(14);
sendClearData((byte)(cali.get("MAG", "Z") >> 8));
sendClearData((byte)(cali.get("MAG", "Z")));
sendClearData(11);
System.Threading.Thread.Sleep(100);
sendClearData(7);
sendClearData(67);
sendClearData(11);
sendClearData(0);
sendClearData(0);
sendClearData(11);
}
/// <summary>
/// Gets the memory / calibration of the IMU values after calibration is complete and invokes the
/// main Madwick algorithm.
/// </summary>
/// <param name="values"> Short values converted from the raw integer values.</param>
public void adjust(short[] values)
{
float GX = ((((float)(values[0]) - (float)calibration.get("GYRO", "X")) * 500f * (float)Math.PI) / (32758f * 180f));
float GY = ((((float)(values[1]) - (float)calibration.get("GYRO", "Y")) * 500f * (float)Math.PI) / (32758f * 180f));
float GZ = ((((float)(values[2]) - (float)calibration.get("GYRO", "Z")) * 500f * (float)Math.PI) / (32758f * 180f));
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(GX, GY, GZ, AX, AY, AZ, MX, MY, MZ);
}