/// <summary>
/// Mahony AHRS
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
static void xIMUserial_Show_Oscilloscope(object sender, CalInertialAndMagneticData e)
{
oscilloscope123A.AddScopeData(e.Accelerometer[0] - weightCalibrated * biasValue[0],
e.Accelerometer[1] - weightCalibrated * biasValue[1],
e.Accelerometer[2]);
oscilloscope456A.AddScopeData(e.Gyroscope[0] - weightCalibrated * biasValue[3],
e.Gyroscope[1] - weightCalibrated * biasValue[4],
e.Gyroscope[2] - weightCalibrated * biasValue[5]);
oscilloscope789A.AddScopeData(e.Magnetometer[0] - 0f * weightCalibrated * biasValue[6],
e.Magnetometer[1] - 0f * weightCalibrated * biasValue[7],
e.Magnetometer[2] - 0f * weightCalibrated * biasValue[8]);
//oscilloscope789A.AddScopeData(e.Magnetometer[0],
//e.Magnetometer[1],
//e.Magnetometer[2]);
}
static void xIMUserial_RemoveBias(object sender, CalInertialAndMagneticData e)
{
// Recursive MEAN
// Acc Bias:
biasValue[0] = biasValue[0] - (biasValue[0] - e.Accelerometer[0]) / biasNum;
biasValue[1] = biasValue[1] - (biasValue[1] - e.Accelerometer[1]) / biasNum;
biasValue[2] = biasValue[2] - (biasValue[2] - e.Accelerometer[2]) / biasNum;
// Gyr Bias:
biasValue[3] = biasValue[3] - (biasValue[3] - e.Gyroscope[0]) / biasNum;
biasValue[4] = biasValue[4] - (biasValue[4] - e.Gyroscope[1]) / biasNum;
biasValue[5] = biasValue[5] - (biasValue[5] - e.Gyroscope[2]) / biasNum;
// Mag Bias:
biasValue[6] = biasValue[6] - (biasValue[6] - e.Magnetometer[0]) / biasNum;
biasValue[7] = biasValue[7] - (biasValue[7] - e.Magnetometer[1]) / biasNum;
biasValue[8] = biasValue[8] - (biasValue[8] - e.Magnetometer[2]) / biasNum;
biasNum += 1.0f;
}
static void xIMUserial_SaveFile(object sender, CalInertialAndMagneticData e)
{
channels[0] = e.Accelerometer[0];
channels[1] = e.Accelerometer[1];
channels[2] = e.Accelerometer[2];
channels[3] = e.Gyroscope[0];
channels[4] = e.Gyroscope[1];
channels[5] = e.Gyroscope[2];
channels[6] = e.Magnetometer[0];
channels[7] = e.Magnetometer[1];
channels[8] = e.Magnetometer[2];
if (csvFileWriter != null && isSaving)
{
csvFileWriter.WriteCSVline(channels);
}
}
/// <summary>
/// xIMUserial CalInertialAndMagneticDataReceived event to update algorithm in AHRS mode.
/// </summary>
static void xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS(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],
e.Magnetometer[0] - weightCalibrated * biasValue[6],
e.Magnetometer[1] - weightCalibrated * biasValue[7],
e.Magnetometer[2] - weightCalibrated * biasValue[8]);
}
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
static void xIMUserial_RotationMatrix(object sender, CalInertialAndMagneticData e)
{
form_3DcuboidB.RotationMatrix = (new QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix();
}