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