protected ObjectCluster BuildMsg(List<byte> packet)
{
ObjectCluster objectCluster = new ObjectCluster(GetShimmerAddress(), GetDeviceName());
byte[] newPacketByte = packet.ToArray();
long[] newPacket = ParseData(newPacketByte, SignalDataTypeArray);
int iTimeStamp = getSignalIndex("TimeStamp"); //find index
objectCluster.RawTimeStamp = (int)newPacket[iTimeStamp];
objectCluster.Add("Timestamp", "RAW", "no units", newPacket[iTimeStamp]);
double calibratedTS = CalibrateTimeStamp(newPacket[iTimeStamp]);
objectCluster.Add("Timestamp", "CAL", "mSecs", calibratedTS);
double time = (DateTime.UtcNow - UnixEpoch).TotalMilliseconds;
double[] accelerometer = new double[3];
double[] gyroscope = new double[3];
double[] magnetometer = new double[3];
if (HardwareVersion == (int)ShimmerVersion.SHIMMER3)
{
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_A_ACCEL) > 0))
{
int iAccelX = getSignalIndex("Low Noise Accelerometer X"); //find index
int iAccelY = getSignalIndex("Low Noise Accelerometer Y"); //find index
int iAccelZ = getSignalIndex("Low Noise Accelerometer Z"); //find index
double[] datatemp = new double[3] { newPacket[iAccelX], newPacket[iAccelY], newPacket[iAccelZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel);
string units;
if (DefaultAccelParams)
{
units = "m/(sec^2)*";
}
else
{
units = "m/(sec^2)";
}
objectCluster.Add("Low Noise Accelerometer X", "RAW", "no units", newPacket[iAccelX]);
objectCluster.Add("Low Noise Accelerometer X", "CAL", units, datatemp[0]);
objectCluster.Add("Low Noise Accelerometer Y", "RAW", "no units", newPacket[iAccelY]);
objectCluster.Add("Low Noise Accelerometer Y", "CAL", units, datatemp[1]);
objectCluster.Add("Low Noise Accelerometer Z", "RAW", "no units", newPacket[iAccelZ]);
objectCluster.Add("Low Noise Accelerometer Z", "CAL", units, datatemp[2]);
accelerometer[0] = datatemp[0];
accelerometer[1] = datatemp[1];
accelerometer[2] = datatemp[2];
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_D_ACCEL) > 0))
{
int iAccelX = getSignalIndex("Wide Range Accelerometer X"); //find index
int iAccelY = getSignalIndex("Wide Range Accelerometer Y"); //find index
int iAccelZ = getSignalIndex("Wide Range Accelerometer Z"); //find index
double[] datatemp = new double[3] { newPacket[iAccelX], newPacket[iAccelY], newPacket[iAccelZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixAccel2, SensitivityMatrixAccel2, OffsetVectorAccel2);
string units;
if (DefaultWRAccelParams)
{
units = "m/(sec^2)*";
}
else
{
units = "m/(sec^2)";
}
objectCluster.Add("Wide Range Accelerometer X", "RAW", "no units", newPacket[iAccelX]);
objectCluster.Add("Wide Range Accelerometer X", "CAL", units, datatemp[0]);
objectCluster.Add("Wide Range Accelerometer Y", "RAW", "no units", newPacket[iAccelY]);
objectCluster.Add("Wide Range Accelerometer Y", "CAL", units, datatemp[1]);
objectCluster.Add("Wide Range Accelerometer Z", "RAW", "no units", newPacket[iAccelZ]);
objectCluster.Add("Wide Range Accelerometer Z", "CAL", units, datatemp[2]);
accelerometer[0] = datatemp[0];
accelerometer[1] = datatemp[1];
accelerometer[2] = datatemp[2];
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_MPU9150_GYRO) > 0))
{
int iGyroX = getSignalIndex("Gyroscope X");
int iGyroY = getSignalIndex("Gyroscope Y");
int iGyroZ = getSignalIndex("Gyroscope Z");
double[] datatemp = new double[3] { newPacket[iGyroX], newPacket[iGyroY], newPacket[iGyroZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixGyro, SensitivityMatrixGyro, OffsetVectorGyro);
string units;
if (DefaultGyroParams)
{
units = "deg/sec*";
}
else
{
units = "deg/sec";
}
objectCluster.Add("Gyroscope X", "RAW", "no units", newPacket[iGyroX]);
objectCluster.Add("Gyroscope X", "CAL", units, datatemp[0]);
objectCluster.Add("Gyroscope Y", "RAW", "no units", newPacket[iGyroY]);
objectCluster.Add("Gyroscope Y", "CAL", units, datatemp[1]);
objectCluster.Add("Gyroscope Z", "RAW", "no units", newPacket[iGyroZ]);
objectCluster.Add("Gyroscope Z", "CAL", units, datatemp[2]);
gyroscope[0] = datatemp[0] * Math.PI / 180;
gyroscope[1] = datatemp[1] * Math.PI / 180;
gyroscope[2] = datatemp[2] * Math.PI / 180;
if (EnableGyroOnTheFlyCalibration)
{
GyroXRawList.Add(newPacket[iGyroX]);
GyroYRawList.Add(newPacket[iGyroY]);
GyroZRawList.Add(newPacket[iGyroZ]);
if (GyroXRawList.Count > ListSizeGyroOnTheFly)
{
GyroXRawList.RemoveAt(0);
GyroYRawList.RemoveAt(0);
GyroZRawList.RemoveAt(0);
}
GyroXCalList.Add(datatemp[0]);
GyroYCalList.Add(datatemp[1]);
GyroZCalList.Add(datatemp[2]);
if (GyroXCalList.Count > ListSizeGyroOnTheFly)
{
GyroXCalList.RemoveAt(0);
GyroYCalList.RemoveAt(0);
GyroZCalList.RemoveAt(0);
if (GetStandardDeviation(GyroXCalList) < ThresholdGyroOnTheFly && GetStandardDeviation(GyroYCalList) < ThresholdGyroOnTheFly && GetStandardDeviation(GyroZCalList) < ThresholdGyroOnTheFly)
{
OffsetVectorGyro[0, 0] = GyroXRawList.Average();
OffsetVectorGyro[1, 0] = GyroYRawList.Average();
OffsetVectorGyro[2, 0] = GyroZRawList.Average();
}
}
}
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_LSM303DLHC_MAG) > 0))
{
int iMagX = getSignalIndex("Magnetometer X");
int iMagY = getSignalIndex("Magnetometer Y");
int iMagZ = getSignalIndex("Magnetometer Z");
double[] datatemp = new double[3] { newPacket[iMagX], newPacket[iMagY], newPacket[iMagZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixMag, SensitivityMatrixMag, OffsetVectorMag);
string units;
if (DefaultMagParams)
{
units = "local*";
}
else
{
units = "local";
}
objectCluster.Add("Magnetometer X", "RAW", "no units", newPacket[iMagX]);
objectCluster.Add("Magnetometer X", "CAL", units, datatemp[0]);
objectCluster.Add("Magnetometer Y", "RAW", "no units", newPacket[iMagY]);
objectCluster.Add("Magnetometer Y", "CAL", units, datatemp[1]);
objectCluster.Add("Magnetometer Z", "RAW", "no units", newPacket[iMagZ]);
objectCluster.Add("Magnetometer Z", "CAL", units, datatemp[2]);
magnetometer[0] = datatemp[0];
magnetometer[1] = datatemp[1];
magnetometer[2] = datatemp[2];
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_VBATT) > 0))
{
int index = getSignalIndex("VSenseBatt");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1) * 1.988);
if (datatemp < 3400 && datatemp > 3000)
{
//System.Threading.Thread.Sleep(500);
if (CurrentLEDStatus != 1)
{
WriteBytes(new byte[2] { (byte)Shimmer.PacketTypeShimmer2.SET_BLINK_LED, (byte)1 }, 0, 2);
CurrentLEDStatus = 1;
}
}
else if (datatemp <= 3000)
{
//System.Threading.Thread.Sleep(500);
if (CurrentLEDStatus != 2)
{
WriteBytes(new byte[2] { (byte)Shimmer.PacketTypeShimmer2.SET_BLINK_LED, (byte)2 }, 0, 2);
CurrentLEDStatus = 2;
}
}
else
{
if (CurrentLEDStatus != 0)
{
WriteBytes(new byte[2] { (byte)Shimmer.PacketTypeShimmer2.SET_BLINK_LED, (byte)0 }, 0, 2);
CurrentLEDStatus = 0;
}
}
objectCluster.Add("VSenseBatt", "RAW", "no units", newPacket[index]);
objectCluster.Add("VSenseBatt", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXT_A7) > 0))
{
int index = getSignalIndex("External ADC A7");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("External ADC A7", "RAW", "no units", newPacket[index]);
objectCluster.Add("External ADC A7", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXT_A6) > 0))
{
int index = getSignalIndex("External ADC A6");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("External ADC A6", "RAW", "no units", newPacket[index]);
objectCluster.Add("External ADC A6", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXT_A15) > 0))
{
int index = getSignalIndex("External ADC A15");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("External ADC A15", "RAW", "no units", newPacket[index]);
objectCluster.Add("External ADC A15", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_INT_A1) > 0))
{
int index = getSignalIndex("Internal ADC A1");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("Internal ADC A1", "RAW", "no units", newPacket[index]);
objectCluster.Add("Internal ADC A1", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_INT_A12) > 0))
{
int index = getSignalIndex("Internal ADC A12");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("Internal ADC A12", "RAW", "no units", newPacket[index]);
objectCluster.Add("Internal ADC A12", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_INT_A13) > 0))
{
int index = getSignalIndex("Internal ADC A13");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("Internal ADC A13", "RAW", "no units", newPacket[index]);
objectCluster.Add("Internal ADC A13", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_INT_A14) > 0))
{
int index = getSignalIndex("Internal ADC A14");
double datatemp = newPacket[index];
datatemp = (CalibrateU12AdcValue(datatemp, 0, 3, 1));
objectCluster.Add("Internal ADC A14", "RAW", "no units", newPacket[index]);
objectCluster.Add("Internal ADC A14", "CAL", "mVolts", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_BMP180_PRESSURE) > 0))
{
int iUP = getSignalIndex("Pressure");
int iUT = getSignalIndex("Temperature");
double UT = (double)newPacket[iUT];
double UP = (double)newPacket[iUP];
UP = UP / Math.Pow(2, 8 - PressureResolution);
double[] datatemp = new double[2] { newPacket[iUP], newPacket[iUT] };
double[] bmp180caldata = CalibratePressureSensorData(UP, datatemp[1]);
objectCluster.Add("Pressure", "RAW", "no units", UP);
objectCluster.Add("Pressure", "CAL", "kPa", bmp180caldata[0] / 1000);
objectCluster.Add("Temperature", "RAW", "no units", newPacket[iUT]);
objectCluster.Add("Temperature", "CAL", "Celcius*", bmp180caldata[1]);
}
if (((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_GSR) > 0))
{
int iGSR = getSignalIndex("GSR Raw");
int newGSRRange = -1; // initialized to -1 so it will only come into play if mGSRRange = 4
double datatemp = newPacket[iGSR];
double p1 = 0, p2 = 0;
if (GSRRange == 4)
{
newGSRRange = (49152 & (int)datatemp) >> 14;
}
if (GSRRange == 0 || newGSRRange == 0)
{ //Note that from FW 1.0 onwards the MSB of the GSR data contains the range
// the polynomial function used for calibration has been deprecated, it is replaced with a linear function
//p1 = 0.0363;
//p2 = -24.8617;
p1 = 0.0373;
p2 = -24.9915;
}
else if (GSRRange == 1 || newGSRRange == 1)
{
//p1 = 0.0051;
//p2 = -3.8357;
p1 = 0.0054;
p2 = -3.5194;
}
else if (GSRRange == 2 || newGSRRange == 2)
{
//p1 = 0.0015;
//p2 = -1.0067;
p1 = 0.0015;
p2 = -1.0163;
}
else if (GSRRange == 3 || newGSRRange == 3)
{
//p1 = 4.4513e-04;
//p2 = -0.3193;
p1 = 4.5580e-04;
p2 = -0.3014;
}
datatemp = CalibrateGsrData(datatemp, p1, p2);
objectCluster.Add("GSR", "RAW", "no units", newPacket[iGSR]);
objectCluster.Add("GSR", "CAL", "kOhms", datatemp);
}
if ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXG1_24BIT) > 0)
{
int iStatus = getSignalIndex("EXG1 Sta");
int iCh1 = getSignalIndex("EXG1 CH1");
int iCh2 = getSignalIndex("EXG1 CH2");
double[] datatemp = new double[3] { newPacket[iStatus], newPacket[iCh1], newPacket[iCh2] };
int gain = ConvertEXGGainSettingToValue((Exg1RegArray[3] >> 4) & 7);
datatemp[1] = datatemp[1] * (((2.42 * 1000) / gain) / (Math.Pow(2, 23) - 1));
gain = ConvertEXGGainSettingToValue((Exg1RegArray[4] >> 4) & 7);
datatemp[2] = datatemp[2] * (((2.42 * 1000) / gain) / (Math.Pow(2, 23) - 1));
objectCluster.Add("EXG1 Sta", "RAW", "no units", newPacket[iStatus]);
if (IsDefaultECGConfigurationEnabled())
{
objectCluster.Add("ECG LL-RA", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("ECG LL-RA", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("ECG LA-RA", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("ECG LA-RA", "CAL", "mVolts", datatemp[2]);
}
else if (IsDefaultEMGConfigurationEnabled())
{
objectCluster.Add("EMG CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EMG CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EMG CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EMG CH2", "CAL", "mVolts", datatemp[2]);
}
else
{
objectCluster.Add("EXG1 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG1 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG1 CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG1 CH2", "CAL", "mVolts", datatemp[2]);
}
}
if ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXG2_24BIT) > 0)
{
int iStatus = getSignalIndex("EXG2 Sta");
int iCh1 = getSignalIndex("EXG2 CH1");
int iCh2 = getSignalIndex("EXG2 CH2");
double[] datatemp = new double[3] { newPacket[iStatus], newPacket[iCh1], newPacket[iCh2] };
int gain = ConvertEXGGainSettingToValue((Exg2RegArray[3] >> 4) & 7);
datatemp[1] = datatemp[1] * (((2.42 * 1000) / gain) / (Math.Pow(2, 23) - 1));
gain = ConvertEXGGainSettingToValue((Exg2RegArray[4] >> 4) & 7);
datatemp[2] = datatemp[2] * (((2.42 * 1000) / gain) / (Math.Pow(2, 23) - 1));
objectCluster.Add("EXG2 Sta", "RAW", "no units", newPacket[iStatus]);
if (IsDefaultECGConfigurationEnabled())
{
objectCluster.Add("EXG2 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("ECG Vx-RL", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("ECG Vx-RL", "CAL", "mVolts", datatemp[2]);
}
else if (IsDefaultEMGConfigurationEnabled())
{
objectCluster.Add("EXG2 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG2 CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG2 CH2", "CAL", "mVolts", datatemp[2]);
}
else
{
objectCluster.Add("EXG2 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG2 CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG2 CH2", "CAL", "mVolts", datatemp[2]);
}
}
if ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXG1_16BIT) > 0)
{
int iStatus = getSignalIndex("EXG1 Sta");
int iCh1 = getSignalIndex("EXG1 CH1 16Bit");
int iCh2 = getSignalIndex("EXG1 CH2 16Bit");
double[] datatemp = new double[3] { newPacket[iStatus], newPacket[iCh1], newPacket[iCh2] };
int gain = ConvertEXGGainSettingToValue((Exg1RegArray[3] >> 4) & 7);
datatemp[1] = datatemp[1] * (((2.42 * 1000) / (gain * 2)) / (Math.Pow(2, 15) - 1));
gain = ConvertEXGGainSettingToValue((Exg1RegArray[4] >> 4) & 7);
datatemp[2] = datatemp[2] * (((2.42 * 1000) / (gain * 2)) / (Math.Pow(2, 15) - 1));
objectCluster.Add("EXG1 Sta", "RAW", "no units", newPacket[iStatus]);
if (IsDefaultECGConfigurationEnabled())
{
objectCluster.Add("ECG LL-RA", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("ECG LL-RA", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("ECG LA-RA", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("ECG LA-RA", "CAL", "mVolts", datatemp[2]);
}
else if (IsDefaultEMGConfigurationEnabled())
{
objectCluster.Add("EMG CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EMG CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EMG CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EMG CH2", "CAL", "mVolts", datatemp[2]);
}
else
{
objectCluster.Add("EXG1 CH1 16Bit", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG1 CH1 16Bit", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG1 CH2 16Bit", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG1 CH2 16Bit", "CAL", "mVolts", datatemp[2]);
}
}
if ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_EXG2_16BIT) > 0)
{
int iStatus = getSignalIndex("EXG2 Sta");
int iCh1 = getSignalIndex("EXG2 CH1 16Bit");
int iCh2 = getSignalIndex("EXG2 CH2 16Bit");
double[] datatemp = new double[3] { newPacket[iStatus], newPacket[iCh1], newPacket[iCh2] };
int gain = ConvertEXGGainSettingToValue((Exg2RegArray[3] >> 4) & 7);
datatemp[1] = datatemp[1] * (((2.42 * 1000) / (gain * 2)) / (Math.Pow(2, 15) - 1));
gain = ConvertEXGGainSettingToValue((Exg2RegArray[4] >> 4) & 7);
datatemp[2] = datatemp[2] * (((2.42 * 1000) / (gain * 2)) / (Math.Pow(2, 15) - 1));
objectCluster.Add("EXG2 Sta", "RAW", "no units", newPacket[iStatus]);
if (IsDefaultECGConfigurationEnabled())
{
objectCluster.Add("EXG2 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("ECG Vx-RL", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("ECG Vx-RL", "CAL", "mVolts", datatemp[2]);
}
else if (IsDefaultEMGConfigurationEnabled())
{
objectCluster.Add("EXG2 CH1", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG2 CH2", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG2 CH2", "CAL", "mVolts", datatemp[2]);
}
else
{
objectCluster.Add("EXG2 CH1 16Bit", "RAW", "no units", newPacket[iCh1]);
objectCluster.Add("EXG2 CH1 16Bit", "CAL", "mVolts", datatemp[1]);
objectCluster.Add("EXG2 CH2 16Bit", "RAW", "no units", newPacket[iCh2]);
objectCluster.Add("EXG2 CH2 16Bit", "CAL", "mVolts", datatemp[2]);
}
}
if ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_BRIDGE_AMP) > 0)
{
int iSGHigh = getSignalIndex("Bridge Amplifier High");
int iSGLow = getSignalIndex("Bridge Amplifier Low");
double[] datatemp = new double[2] { newPacket[iSGHigh], newPacket[iSGLow] };
datatemp[0] = CalibrateU12AdcValue(datatemp[0], OffsetSGHigh, VRef, GainSGHigh);
datatemp[1] = CalibrateU12AdcValue(datatemp[1], OffsetSGLow, VRef, GainSGLow);
objectCluster.Add("Bridge Amplifier High", "RAW", "no units", newPacket[iSGHigh]);
objectCluster.Add("Bridge Amplifier High", "CAL", "mVolts", datatemp[0]);
objectCluster.Add("Bridge Amplifier Low", "RAW", "no units", newPacket[iSGLow]);
objectCluster.Add("Bridge Amplifier Low", "CAL", "mVolts", datatemp[1]);
}
if ((((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_A_ACCEL) > 0) || ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_D_ACCEL) > 0))
&& ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_MPU9150_GYRO) > 0) && ((EnabledSensors & (int)SensorBitmapShimmer3.SENSOR_LSM303DLHC_MAG) > 0)
&& Orientation3DEnabled)
{
if (OrientationAlgo == null)
{
OrientationAlgo = new GradDes3DOrientation(0.4, (1 / this.GetSamplingRate()), 1, 0, 0, 0);
}
Quaternion q = OrientationAlgo.update(accelerometer[0], accelerometer[1], accelerometer[2], gyroscope[0], gyroscope[1], gyroscope[2], magnetometer[0], magnetometer[1], magnetometer[2]);
double theta, Rx, Ry, Rz, rho;
rho = Math.Acos(q.q1);
theta = rho * 2;
Rx = q.q2 / Math.Sin(rho);
Ry = q.q3 / Math.Sin(rho);
Rz = q.q4 / Math.Sin(rho);
objectCluster.Add("Axis Angle A", "CAL", "local", theta);
objectCluster.Add("Axis Angle X", "CAL", "local", Rx);
objectCluster.Add("Axis Angle Y", "CAL", "local", Ry);
objectCluster.Add("Axis Angle Z", "CAL", "local", Rz);
objectCluster.Add("Quaternion 0", "CAL", "local", q.q1);
objectCluster.Add("Quaternion 1", "CAL", "local", q.q2);
objectCluster.Add("Quaternion 2", "CAL", "local", q.q3);
objectCluster.Add("Quaternion 3", "CAL", "local", q.q4);
}
}
else
{ //start of Shimmer2
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_ACCEL) > 0))
{
int iAccelX = getSignalIndex("Accelerometer X"); //find index
int iAccelY = getSignalIndex("Accelerometer Y"); //find index
int iAccelZ = getSignalIndex("Accelerometer Z"); //find index
double[] datatemp = new double[3] { newPacket[iAccelX], newPacket[iAccelY], newPacket[iAccelZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel);
string units;
if (DefaultAccelParams)
{
units = "m/(sec^2)*";
}
else
{
units = "m/(sec^2)";
}
objectCluster.Add("Accelerometer X", "RAW", "no units", newPacket[iAccelX]);
objectCluster.Add("Accelerometer X", "CAL", units, datatemp[0]);
objectCluster.Add("Accelerometer Y", "RAW", "no units", newPacket[iAccelY]);
objectCluster.Add("Accelerometer Y", "CAL", units, datatemp[1]);
objectCluster.Add("Accelerometer Z", "RAW", "no units", newPacket[iAccelZ]);
objectCluster.Add("Accelerometer Z", "CAL", units, datatemp[2]);
accelerometer[0] = datatemp[0];
accelerometer[1] = datatemp[1];
accelerometer[2] = datatemp[2];
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_GYRO) > 0))
{
int iGyroX = getSignalIndex("Gyroscope X");
int iGyroY = getSignalIndex("Gyroscope Y");
int iGyroZ = getSignalIndex("Gyroscope Z");
double[] datatemp = new double[3] { newPacket[iGyroX], newPacket[iGyroY], newPacket[iGyroZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixGyro, SensitivityMatrixGyro, OffsetVectorGyro);
string units;
if (DefaultGyroParams)
{
units = "deg/sec*";
}
else
{
units = "deg/sec";
}
objectCluster.Add("Gyroscope X", "RAW", "no units", newPacket[iGyroX]);
objectCluster.Add("Gyroscope X", "CAL", units, datatemp[0]);
objectCluster.Add("Gyroscope Y", "RAW", "no units", newPacket[iGyroY]);
objectCluster.Add("Gyroscope Y", "CAL", units, datatemp[1]);
objectCluster.Add("Gyroscope Z", "RAW", "no units", newPacket[iGyroZ]);
objectCluster.Add("Gyroscope Z", "CAL", units, datatemp[2]);
gyroscope[0] = datatemp[0] * Math.PI / 180;
gyroscope[1] = datatemp[1] * Math.PI / 180;
gyroscope[2] = datatemp[2] * Math.PI / 180;
if (EnableGyroOnTheFlyCalibration)
{
GyroXRawList.Add(newPacket[iGyroX]);
GyroYRawList.Add(newPacket[iGyroY]);
GyroZRawList.Add(newPacket[iGyroZ]);
if (GyroXRawList.Count > ListSizeGyroOnTheFly)
{
GyroXRawList.RemoveAt(0);
GyroYRawList.RemoveAt(0);
GyroZRawList.RemoveAt(0);
}
GyroXCalList.Add(datatemp[0]);
GyroYCalList.Add(datatemp[1]);
GyroZCalList.Add(datatemp[2]);
if (GyroXCalList.Count > ListSizeGyroOnTheFly)
{
GyroXCalList.RemoveAt(0);
GyroYCalList.RemoveAt(0);
GyroZCalList.RemoveAt(0);
if (GetStandardDeviation(GyroXCalList) < ThresholdGyroOnTheFly && GetStandardDeviation(GyroYCalList) < ThresholdGyroOnTheFly && GetStandardDeviation(GyroZCalList) < ThresholdGyroOnTheFly)
{
OffsetVectorGyro[0, 0] = GyroXRawList.Average();
OffsetVectorGyro[1, 0] = GyroYRawList.Average();
OffsetVectorGyro[2, 0] = GyroZRawList.Average();
}
}
}
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_MAG) > 0))
{
int iMagX = getSignalIndex("Magnetometer X");
int iMagY = getSignalIndex("Magnetometer Y");
int iMagZ = getSignalIndex("Magnetometer Z");
double[] datatemp = new double[3] { newPacket[iMagX], newPacket[iMagY], newPacket[iMagZ] };
datatemp = CalibrateInertialSensorData(datatemp, AlignmentMatrixMag, SensitivityMatrixMag, OffsetVectorMag);
string units;
if (DefaultMagParams)
{
units = "local*";
}
else
{
units = "local";
}
objectCluster.Add("Magnetometer X", "RAW", "no units", newPacket[iMagX]);
objectCluster.Add("Magnetometer X", "CAL", units, datatemp[0]);
objectCluster.Add("Magnetometer Y", "RAW", "no units", newPacket[iMagY]);
objectCluster.Add("Magnetometer Y", "CAL", units, datatemp[1]);
objectCluster.Add("Magnetometer Z", "RAW", "no units", newPacket[iMagZ]);
objectCluster.Add("Magnetometer Z", "CAL", units, datatemp[2]);
magnetometer[0] = datatemp[0];
magnetometer[1] = datatemp[1];
magnetometer[2] = datatemp[2];
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_GSR) > 0))
{
int iGSR = getSignalIndex("GSR Raw");
int newGSRRange = -1; // initialized to -1 so it will only come into play if mGSRRange = 4
double datatemp = newPacket[iGSR];
double p1 = 0, p2 = 0;
if (GSRRange == 4)
{
newGSRRange = (49152 & (int)datatemp) >> 14;
}
if (GSRRange == 0 || newGSRRange == 0)
{ //Note that from FW 1.0 onwards the MSB of the GSR data contains the range
// the polynomial function used for calibration has been deprecated, it is replaced with a linear function
p1 = 0.0373;
p2 = -24.9915;
}
else if (GSRRange == 1 || newGSRRange == 1)
{
p1 = 0.0054;
p2 = -3.5194;
}
else if (GSRRange == 2 || newGSRRange == 2)
{
p1 = 0.0015;
p2 = -1.0163;
}
else if (GSRRange == 3 || newGSRRange == 3)
{
p1 = 4.5580e-04;
p2 = -0.3014;
}
datatemp = CalibrateGsrData(datatemp, p1, p2);
objectCluster.Add("GSR", "RAW", "no units", newPacket[iGSR]);
objectCluster.Add("GSR", "CAL", "kOhms*", datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_ECG) > 0))
{
int iECGRALL = getSignalIndex("ECG RA LL");
int iECGLALL = getSignalIndex("ECG LA LL");
double[] datatemp = new double[2] { newPacket[iECGRALL], newPacket[iECGLALL] };
datatemp[0] = CalibrateU12AdcValue(datatemp[0], OffsetECGRALL, 3, GainECGRALL);
datatemp[1] = CalibrateU12AdcValue(datatemp[1], OffsetECGLALL, 3, GainECGLALL);
string units = "mVolts";
if (DefaultECGParams)
{
units = "mVolts*";
}
objectCluster.Add("ECG RA LL", "RAW", "no units", newPacket[iECGRALL]);
objectCluster.Add("ECG RA LL", "CAL", units, datatemp[0]);
objectCluster.Add("ECG LA LL", "RAW", "no units", newPacket[iECGLALL]);
objectCluster.Add("ECG LA LL", "CAL", units, datatemp[1]);
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_EMG) > 0))
{
int iEMG = getSignalIndex("EMG");
double datatemp = newPacket[iEMG];
datatemp = CalibrateU12AdcValue(datatemp, OffsetEMG, 3, GainEMG);
string units = "mVolts";
if (DefaultEMGParams)
{
units = "mVolts*";
}
objectCluster.Add("EMG", "RAW", "no units", newPacket[iEMG]);
objectCluster.Add("EMG", "CAL", units, datatemp);
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_STRAIN_GAUGE) > 0))
{
int iSGHigh = getSignalIndex("Strain Gauge High");
int iSGLow = getSignalIndex("Strain Gauge Low");
double[] datatemp = new double[2] { newPacket[iSGHigh], newPacket[iSGLow] };
datatemp[0] = CalibrateU12AdcValue(datatemp[0], OffsetSGHigh, VRef, GainSGHigh);
datatemp[1] = CalibrateU12AdcValue(datatemp[1], OffsetSGLow, VRef, GainSGLow);
objectCluster.Add("Strain Gauge High", "RAW", "no units", newPacket[iSGHigh]);
objectCluster.Add("Strain Gauge High", "CAL", "mVolts*", datatemp[0]);
objectCluster.Add("Strain Gauge Low", "RAW", "no units", newPacket[iSGLow]);
objectCluster.Add("Strain Gauge Low", "CAL", "mVolts*", datatemp[1]);
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_HEART) > 0))
{
int iHeartRate = getSignalIndex("Heart Rate");
double datatemp = newPacket[iHeartRate];
double cal = datatemp;
//if (FirmwareVersion == 0.1)
if (CompatibilityCode == 1)
{
}
else
{
if (datatemp == 0)
{
cal = LastKnownHeartRate;
}
else
{
cal = (int)(1024 / datatemp * 60);
LastKnownHeartRate = (int)cal;
}
}
objectCluster.Add("Heart Rate", "RAW", "no units", newPacket[iHeartRate]);
objectCluster.Add("Heart Rate", "CAL", "mVolts*", cal);
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_EXP_BOARD_A0) > 0))
{
int iA0 = getSignalIndex("Exp Board A0");
double datatemp = newPacket[iA0];
datatemp = CalibrateU12AdcValue(datatemp, 0, 3, 1) * 1.988;
if (GetPMux())
{
objectCluster.Add("VSenseReg", "RAW", "no units", newPacket[iA0]);
objectCluster.Add("VSenseReg", "CAL", "mVolts*", datatemp);
}
else
{
objectCluster.Add("Exp Board A0", "RAW", "no units", newPacket[iA0]);
objectCluster.Add("Exp Board A0", "CAL", "mVolts*", datatemp);
}
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_EXP_BOARD_A7) > 0))
{
int iA7 = getSignalIndex("Exp Board A7");
double datatemp = newPacket[iA7];
datatemp = CalibrateU12AdcValue(datatemp, 0, 3, 1) * 2;
if (GetPMux())
{
objectCluster.Add("VSenseBatt", "RAW", "no units", newPacket[iA7]);
objectCluster.Add("VSenseBatt", "CAL", "mVolts*", datatemp);
if (datatemp < 3400)
{
//System.Threading.Thread.Sleep(500);
if (CurrentLEDStatus == 0)
{
WriteBytes(new byte[2] { (byte)Shimmer.PacketTypeShimmer2.SET_BLINK_LED, (byte)1 }, 0, 2);
CurrentLEDStatus = 1;
}
else
{
//System.Threading.Thread.Sleep(500);
if (CurrentLEDStatus == 1)
{
WriteBytes(new byte[2] { (byte)Shimmer.PacketTypeShimmer2.SET_BLINK_LED, (byte)0 }, 0, 2);
CurrentLEDStatus = 0;
}
}
}
}
else
{
objectCluster.Add("Exp Board A7", "RAW", "no units", newPacket[iA7]);
objectCluster.Add("Exp Board A7", "CAL", "mVolts*", datatemp);
}
}
if (((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_ACCEL) > 0) && ((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_GYRO) > 0) && ((EnabledSensors & (int)SensorBitmapShimmer2.SENSOR_MAG) > 0) && Orientation3DEnabled)
{
if (OrientationAlgo == null)
{
OrientationAlgo = new GradDes3DOrientation(0.4, 1 / this.GetSamplingRate(), 1, 0, 0, 0);
}
Quaternion q = OrientationAlgo.update(accelerometer[0], accelerometer[1], accelerometer[2], gyroscope[0], gyroscope[1], gyroscope[2], magnetometer[0], magnetometer[1], magnetometer[2]);
double theta, Rx, Ry, Rz, rho;
rho = Math.Acos(q.q1);
theta = rho * 2;
Rx = q.q2 / Math.Sin(rho);
Ry = q.q3 / Math.Sin(rho);
Rz = q.q4 / Math.Sin(rho);
objectCluster.Add("Axis Angle A", "CAL", "local", theta);
objectCluster.Add("Axis Angle X", "CAL", "local", Rx);
objectCluster.Add("Axis Angle Y", "CAL", "local", Ry);
objectCluster.Add("Axis Angle Z", "CAL", "local", Rz);
objectCluster.Add("Quaternion 0", "CAL", "local", q.q1);
objectCluster.Add("Quaternion 1", "CAL", "local", q.q2);
objectCluster.Add("Quaternion 2", "CAL", "local", q.q3);
objectCluster.Add("Quaternion 3", "CAL", "local", q.q4);
}
}
return objectCluster;
}
public virtual void StartStreaming()
{
if (ShimmerState == SHIMMER_STATE_CONNECTED)
{
if (ShimmerState != SHIMMER_STATE_STREAMING)
{
StreamingACKReceived = false;
StreamTimeOutCount = 0;
LastReceivedTimeStamp = 0;
CurrentTimeStampCycle = 0;
LastReceivedCalibratedTimeStamp = -1;
FirstTimeCalTime = true;
PacketLossCount = 0;
PacketReceptionRate = 100;
KeepObjectCluster = null; //This is important and is required!
OrientationAlgo = null;
mWaitingForStartStreamingACK = true;
WriteBytes(new byte[1] { (byte)PacketTypeShimmer2.START_STREAMING_COMMAND }, 0, 1);
}
}
}