public void OnBroadcast(object sender, MesgBroadcastEventArgs e)
{
float[] calibratedXYZ;
int count;
List <Mesg> mesgs = e.mesgs;
int[] rawXYZ = new int[NUM_AXIS];
foreach (Mesg mesg in mesgs)
{
switch (mesg.Num)
{
case MesgNum.AccelerometerData:
if (haveAccelCal)
{
AccelerometerDataMesg accelData = new AccelerometerDataMesg(mesg);
count = accelData.GetNumAccelX();
for (int i = 0; i < count; i++)
{
//Extract the uncalibrated accel data from incoming message
rawXYZ[X_AXIS_OFFSET] = Convert.ToInt32(accelData.GetAccelX(i));
rawXYZ[Y_AXIS_OFFSET] = Convert.ToInt32(accelData.GetAccelY(i));
rawXYZ[Z_AXIS_OFFSET] = Convert.ToInt32(accelData.GetAccelZ(i));
// Apply calibration to the values
calibratedXYZ = AdjustSensorData(rawXYZ, accelCalParams);
// Update the message
ProcessCalibrationFactor(mesg, accelDataFieldNameXYZ, calibratedXYZ, accelDataFieldNum);
}
}
break;
case MesgNum.GyroscopeData:
if (haveGyroCal)
{
GyroscopeDataMesg gyroData = new GyroscopeDataMesg(mesg);
count = gyroData.GetNumGyroX();
for (int i = 0; i < count; i++)
{
//Extract the uncalibrated gyro data from incoming message
rawXYZ[X_AXIS_OFFSET] = Convert.ToInt32(gyroData.GetGyroX(i));
rawXYZ[Y_AXIS_OFFSET] = Convert.ToInt32(gyroData.GetGyroY(i));
rawXYZ[Z_AXIS_OFFSET] = Convert.ToInt32(gyroData.GetGyroZ(i));
// Apply calibration to the values
calibratedXYZ = AdjustSensorData(rawXYZ, gyroCalParams);
// Update the message
ProcessCalibrationFactor(mesg, gyroDataFieldNameXYZ, calibratedXYZ, gyroDataFieldNum);
}
}
break;
case MesgNum.MagnetometerData:
if (haveMagCal)
{
MagnetometerDataMesg magData = new MagnetometerDataMesg(mesg);
count = magData.GetNumMagX();
for (int i = 0; i < count; i++)
{
//Extract the uncalibrated mag data from incoming message
rawXYZ[X_AXIS_OFFSET] = Convert.ToInt32(magData.GetMagX(i));
rawXYZ[Y_AXIS_OFFSET] = Convert.ToInt32(magData.GetMagY(i));
rawXYZ[Z_AXIS_OFFSET] = Convert.ToInt32(magData.GetMagZ(i));
// Apply calibration to the values
calibratedXYZ = AdjustSensorData(rawXYZ, magCalParams);
// Update the message
ProcessCalibrationFactor(mesg, magDataFieldNameXYZ, calibratedXYZ, magDataFieldNum);
}
}
break;
default:
break;
} // switch
} // foreach
}
