Esempio n. 1
0
 /// <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]);
 }
Esempio n. 2
0
 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;
 }
Esempio n. 3
0
        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);
            }
        }
Esempio n. 4
0
 /// <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]);
 }
Esempio n. 5
0
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 static void xIMUserial_RotationMatrix(object sender, CalInertialAndMagneticData e)
 {
     form_3DcuboidB.RotationMatrix = (new QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix();
 }