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