private void WriteHeader(ObjectCluster obj)
{
ObjectCluster objectCluster = new ObjectCluster(obj);
List<String> names = objectCluster.GetNames();
List<String> formats = objectCluster.GetFormats();
List<String> units = objectCluster.GetUnits();
List<Double> data = objectCluster.GetData();
String deviceId = objectCluster.GetShimmerID();
for (int i = 0; i < data.Count; i++)
{
PCsvFile.Write(deviceId + Delimeter);
}
PCsvFile.WriteLine();
for (int i = 0; i < data.Count; i++)
{
PCsvFile.Write(names[i] + Delimeter);
}
PCsvFile.WriteLine();
for (int i = 0; i < data.Count; i++)
{
PCsvFile.Write(formats[i] + Delimeter);
}
PCsvFile.WriteLine();
for (int i = 0; i < data.Count; i++)
{
PCsvFile.Write(units[i] + Delimeter);
}
PCsvFile.WriteLine();
}
public void WriteData(ObjectCluster obj)
{
if (FirstWrite)
{
WriteHeader(obj);
FirstWrite = false;
}
Double[] data = obj.GetData().ToArray();
for (int i = 0; i < data.Length; i++)
{
PCsvFile.Write(data[i].ToString() + Delimeter);
}
PCsvFile.WriteLine();
}
public ObjectCluster(ObjectCluster obj)
{
double[] data = obj.GetData().ToArray();
string[] names = obj.GetNames().ToArray();
string[] formats = obj.GetFormats().ToArray();
string[] units = obj.GetUnits().ToArray();
Data = new List<double>();
SignalNames = new List<String>();
Format = new List<String>();
Units = new List<String>();
COMPort = obj.GetCOMPort();
ShimmerID = obj.GetShimmerID();
for (int count = 0; count < data.Length; count++)
{
Data.Add(obj.GetData()[count]);
SignalNames.Add(obj.GetNames()[count]);
Format.Add(obj.GetFormats()[count]);
Units.Add(obj.GetUnits()[count]);
}
}
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);
}
}
}
public void ReadData()
{
List<byte> buffer = new List<byte>();
int i;
byte[] bufferbyte;
List<byte> dataByte;
ObjectCluster objectCluster;
FlushInputConnection();
KeepObjectCluster = null;
while (!StopReading)
{
try
{
byte b = (byte)ReadByte();
if (ShimmerState == SHIMMER_STATE_STREAMING)
{
switch (b)
{
case (byte)PacketTypeShimmer2.DATA_PACKET: //Shimmer3 has the same value
if (IsFilled)
{
dataByte = new List<byte>();
for (i = 0; i < PacketSize; i++)
{
dataByte.Add((byte)ReadByte());
}
objectCluster = BuildMsg(dataByte);
if (KeepObjectCluster != null)
{
//ObjectClusterBuffer.Add(KeepObjectCluster); // dont need this if not storing packets in buffer
}
// check if there was a previously received packet, if there is send that, as it is a packet without error (zero-zero test), each packet starts with zero
if (KeepObjectCluster != null)
{
//Check the time stamp
if (mEnableTimeStampAlignmentCheck)
{
if (packetTimeStampChecker(objectCluster.RawTimeStamp, KeepObjectCluster.RawTimeStamp))
{
CustomEventArgs newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_DATA_PACKET, (object)KeepObjectCluster);
OnNewEvent(newEventArgs);
}
else
{
System.Console.WriteLine("Throwing Packet");
KeepObjectCluster = null;
ReadByte();
}
}
else
{
CustomEventArgs newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_DATA_PACKET, (object)KeepObjectCluster);
OnNewEvent(newEventArgs);
}
//
//packetTimeStampChecker(objectCluster.RawTimeStamp, KeepObjectCluster.RawTimeStamp);
//CustomEventArgs newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_DATA_PACKET, (object)KeepObjectCluster);
//OnNewEvent(newEventArgs);
}
KeepObjectCluster = objectCluster;
buffer.Clear();
}
break;
case (byte)PacketTypeShimmer2.ACK_COMMAND:
//Since the ack always proceeds the instreamcmd
if (StreamingACKReceived)
{
if (GetFirmwareIdentifier() == FW_IDENTIFIER_LOGANDSTREAM)
{
byte c = (byte)ReadByte(); // get the next byte and pass it the ShimmerSDBT, this should be instreamcmd
if (c != (byte)ShimmerSDBT.PacketTypeShimmer3SDBT.INSTREAM_CMD_RESPONSE)
{
KeepObjectCluster = null;
}
else
{
SDBT_switch(c);
}
}
else
{
System.Console.WriteLine("ACK for Command while Streaming Received");
}
}
else
{
System.Console.WriteLine("ACK for Streaming Command Received");
StreamingACKReceived = true;
}
break;
default:
System.Console.WriteLine("Misaligned ByteStream Detected");
// If it gets here means the previous packet is invalid so make it null so it wont be added to the buffer
KeepObjectCluster = null;
break;
}
}
else //connected but not streaming
{
switch (b)
{
case (byte)PacketTypeShimmer2.DATA_PACKET: //Read bytes but do nothing with them
if (IsFilled)
{
dataByte = new List<byte>();
for (i = 0; i < PacketSize; i++)
{
dataByte.Add((byte)ReadByte());
}
buffer.Clear();
}
break;
case (byte)PacketTypeShimmer2.INQUIRY_RESPONSE:
if (ShimmerState != SHIMMER_STATE_CONNECTED)
{
SetState(SHIMMER_STATE_CONNECTED);
}
if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER2 || HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER2R)
{
for (i = 0; i < 5; i++)
{
// get Sampling rate, accel range, config setup byte0, num chans and buffer size
buffer.Add((byte)ReadByte());
}
for (i = 0; i < (int)buffer[3]; i++)
{
// read each channel type for the num channels
buffer.Add((byte)ReadByte());
}
InterpretInquiryResponseShimmer2(buffer);
}
else if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3)
{
for (i = 0; i < 8; i++)
{
// get Sampling rate, accel range, config setup byte0, num chans and buffer size
buffer.Add((byte)ReadByte());
}
for (i = 0; i < (int)buffer[6]; i++)
{
// read each channel type for the num channels
buffer.Add((byte)ReadByte());
}
InterpretInquiryResponseShimmer3(buffer);
}
buffer.Clear();
break;
case (byte)PacketTypeShimmer2.SAMPLING_RATE_RESPONSE:
if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3)
{
int value = 0;
value = (int)ReadByte();
value += (((int)ReadByte() << 8) & 0xFF00);
ADCRawSamplingRateValue = value;
SamplingRate = (double)32768 / ADCRawSamplingRateValue;
}
else
{
ADCRawSamplingRateValue = ReadByte();
SamplingRate = (double)1024 / ADCRawSamplingRateValue;
}
break;
case (byte)PacketTypeShimmer2.ACCEL_RANGE_RESPONSE:
SetAccelRange(ReadByte());
break;
case (byte)PacketTypeShimmer3.ACCEL_SAMPLING_RATE_RESPONSE:
SetAccelSamplingRate(ReadByte());
break;
case (byte)PacketTypeShimmer3.MPU9150_GYRO_RANGE_RESPONSE:
SetGyroRange(ReadByte());
break;
case(byte) PacketTypeShimmer3.BAUD_RATE_COMMAND_RESPONSE:
SetBaudRate(ReadByte());
break;
case (byte)PacketTypeShimmer3.DETECT_EXPANSION_BOARD_RESPONSE:
// 2 byte response, only need byte 2nd byte
byte NumBytes;
NumBytes = (byte)ReadByte();
// size of byte array to read is NumBytes bytes
ExpansionDetectArray = new byte[NumBytes];
for (int p = 0; p < NumBytes; p++)
{
ExpansionDetectArray[p] = (byte)ReadByte();
}
if (ExpansionDetectArray[0] == (int)ExpansionBoardDetectShimmer3.EXPANSION_BRIDGE_AMPLIFIER_PLUS) ExpansionBoard = "Bridge Amplifier+";
else if (ExpansionDetectArray[0] == (int)ExpansionBoardDetectShimmer3.EXPANSION_GSR_PLUS) ExpansionBoard = "GSR+";
else if (ExpansionDetectArray[0] == (int)ExpansionBoardDetectShimmer3.EXPANSION_PROTO3_MINI) ExpansionBoard = "PROTO3 Mini";
else if (ExpansionDetectArray[0] == (int)ExpansionBoardDetectShimmer3.EXPANSION_EXG) ExpansionBoard = "ExG";
else if (ExpansionDetectArray[0] == (int)ExpansionBoardDetectShimmer3.EXPANSION_PROTO3_DELUXE) ExpansionBoard = "PROTO3 Deluxe";
else ExpansionBoard = "Unknown";
if (!ExpansionBoard.Equals("Unknown"))
{
ExpansionBoard = string.Format("{0} (SR{1}-{2}.{3})", ExpansionBoard, ExpansionDetectArray[0], ExpansionDetectArray[1], ExpansionDetectArray[2]);
}
break;
case (byte)PacketTypeShimmer2.MAG_GAIN_RESPONSE:
SetMagRange(ReadByte());
break;
case (byte)PacketTypeShimmer2.MAG_SAMPLING_RATE_RESPONSE:
SetMagSamplingRate(ReadByte());
break;
case (byte)PacketTypeShimmer2.CONFIG_SETUP_BYTE0_RESPONSE:
SetConfigSetupByte0(ReadByte());
break;
case (byte)PacketTypeShimmer2.GSR_RANGE_RESPONSE:
SetGSRRange(ReadByte());
break;
case (byte)PacketTypeShimmer3.INTERNAL_EXP_POWER_ENABLE_RESPONSE:
SetInternalExpPower(ReadByte());
break;
case (byte)PacketTypeShimmer2.ACK_COMMAND:
if (mWaitingForStartStreamingACK)
{
SetState(SHIMMER_STATE_STREAMING);
mWaitingForStartStreamingACK = false;
}
break;
case (byte)PacketTypeShimmer2.ACCEL_CALIBRATION_RESPONSE:
// size is 21 bytes
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.ACCEL_CALIBRATION_RESPONSE);
break;
case (byte)PacketTypeShimmer2.GYRO_CALIBRATION_RESPONSE:
// size is 21 bytes
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.GYRO_CALIBRATION_RESPONSE);
break;
case (byte)PacketTypeShimmer2.MAG_CALIBRATION_RESPONSE:
// size is 21 bytes
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.MAG_CALIBRATION_RESPONSE);
break;
case (byte)PacketTypeShimmer2.ALL_CALIBRATION_RESPONSE:
//Retrieve Accel
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.ACCEL_CALIBRATION_RESPONSE);
//Retrieve Gyro
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.GYRO_CALIBRATION_RESPONSE);
//Retrieve Mag
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer2.MAG_CALIBRATION_RESPONSE);
if (HardwareVersion != (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3)
{
//Retrieve EMG n ECG
bufferbyte = new byte[12];
for (int p = 0; p < 12; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
if (bufferbyte[0] == 255 && bufferbyte[1] == 255 && bufferbyte[2] == 255 && bufferbyte[3] == 255)
{
DefaultEMGParams = true;
}
else
{
OffsetEMG = (double)((bufferbyte[0] & 0xFF) << 8) + (bufferbyte[1] & 0xFF);
GainEMG = (double)((bufferbyte[2] & 0xFF) << 8) + (bufferbyte[3] & 0xFF);
DefaultEMGParams = false;
}
if (bufferbyte[4] == 255 && bufferbyte[5] == 255 && bufferbyte[6] == 255 && bufferbyte[7] == 255)
{
DefaultECGParams = true;
}
else
{
OffsetECGLALL = (double)((bufferbyte[4] & 0xFF) << 8) + (bufferbyte[5] & 0xFF);
GainECGLALL = (double)((bufferbyte[6] & 0xFF) << 8) + (bufferbyte[7] & 0xFF);
OffsetECGRALL = (double)((bufferbyte[8] & 0xFF) << 8) + (bufferbyte[9] & 0xFF);
GainECGRALL = (double)((bufferbyte[10] & 0xFF) << 8) + (bufferbyte[11] & 0xFF);
DefaultECGParams = false;
}
}
else
{
//Retrieve Digital Accel Cal Paramters if Shimmer 3
bufferbyte = new byte[21];
for (int p = 0; p < 21; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
RetrieveKinematicCalibrationParametersFromPacket(bufferbyte, (byte)PacketTypeShimmer3.WR_ACCEL_CALIBRATION_RESPONSE);
}
break;
case (byte)PacketTypeShimmer3.BMP180_CALIBRATION_COEFFICIENTS_RESPONSE:
bufferbyte = new byte[22];
for (int p = 0; p < 22; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
AC1 = Calculatetwoscomplement((int)((int)(bufferbyte[1] & 0xFF) + ((int)(bufferbyte[0] & 0xFF) << 8)), 16);
AC2 = Calculatetwoscomplement((int)((int)(bufferbyte[3] & 0xFF) + ((int)(bufferbyte[2] & 0xFF) << 8)), 16);
AC3 = Calculatetwoscomplement((int)((int)(bufferbyte[5] & 0xFF) + ((int)(bufferbyte[4] & 0xFF) << 8)), 16);
AC4 = (int)((int)(bufferbyte[7] & 0xFF) + ((int)(bufferbyte[6] & 0xFF) << 8));
AC5 = (int)((int)(bufferbyte[9] & 0xFF) + ((int)(bufferbyte[8] & 0xFF) << 8));
AC6 = (int)((int)(bufferbyte[11] & 0xFF) + ((int)(bufferbyte[10] & 0xFF) << 8));
B1 = Calculatetwoscomplement((int)((int)(bufferbyte[13] & 0xFF) + ((int)(bufferbyte[12] & 0xFF) << 8)), 16);
B2 = Calculatetwoscomplement((int)((int)(bufferbyte[15] & 0xFF) + ((int)(bufferbyte[14] & 0xFF) << 8)), 16);
MB = Calculatetwoscomplement((int)((int)(bufferbyte[17] & 0xFF) + ((int)(bufferbyte[16] & 0xFF) << 8)), 16);
MC = Calculatetwoscomplement((int)((int)(bufferbyte[19] & 0xFF) + ((int)(bufferbyte[18] & 0xFF) << 8)), 16);
MD = Calculatetwoscomplement((int)((int)(bufferbyte[21] & 0xFF) + ((int)(bufferbyte[20] & 0xFF) << 8)), 16);
break;
case (byte)PacketTypeShimmer2.BLINK_LED_RESPONSE:
bufferbyte = new byte[1];
bufferbyte[0] = (byte)ReadByte();
CurrentLEDStatus = bufferbyte[0];
break;
case (byte)PacketTypeShimmer2.FW_VERSION_RESPONSE:
// size is 21 bytes
bufferbyte = new byte[6];
for (int p = 0; p < 6; p++)
{
bufferbyte[p] = (byte)ReadByte();
}
FirmwareIdentifier = ((double)((bufferbyte[1] & 0xFF) << 8) + (double)(bufferbyte[0] & 0xFF));
FirmwareVersion = ((double)((bufferbyte[3] & 0xFF) << 8) + (double)(bufferbyte[2] & 0xFF) + ((double)((bufferbyte[4] & 0xFF)) / 10));
FirmwareInternal = ((int)(bufferbyte[5] & 0xFF));
FirmwareMajor = (int)((bufferbyte[3] & 0xFF) << 8) + (int)(bufferbyte[2] & 0xFF);
FirmwareMinor = (int)(bufferbyte[4] & 0xFF);
string fw_id = "";
if (FirmwareIdentifier == 1)
fw_id = "BtStream ";
else if (FirmwareIdentifier == 3)
fw_id = "LogAndStream ";
else
fw_id = "Unknown ";
string temp = fw_id + FirmwareVersion.ToString("0.0") + "." + FirmwareInternal.ToString();
FirmwareVersionFullName = temp;
SetCompatibilityCode();
UpdateBasedOnCompatibilityCode();
break;
case (byte)PacketTypeShimmer2.GET_SHIMMER_VERSION_RESPONSE:
bufferbyte = new byte[1];
bufferbyte[0] = (byte)ReadByte();
HardwareVersion = bufferbyte[0];
// set default calibration parameters
SetCompatibilityCode();
UpdateBasedOnCompatibilityCode();
break;
case (byte)PacketTypeShimmer3.EXG_REGS_RESPONSE:
System.Console.WriteLine("EXG r r" + ChipID);
if (ChipID == 1)
{
ReadByte();
Exg1RegArray[0] = (byte)ReadByte();
Exg1RegArray[1] = (byte)ReadByte();
Exg1RegArray[2] = (byte)ReadByte();
Exg1RegArray[3] = (byte)ReadByte();
Exg1RegArray[4] = (byte)ReadByte();
Exg1RegArray[5] = (byte)ReadByte();
Exg1RegArray[6] = (byte)ReadByte();
Exg1RegArray[7] = (byte)ReadByte();
Exg1RegArray[8] = (byte)ReadByte();
Exg1RegArray[9] = (byte)ReadByte();
}
else
{
ReadByte();
Exg2RegArray[0] = (byte)ReadByte();
Exg2RegArray[1] = (byte)ReadByte();
Exg2RegArray[2] = (byte)ReadByte();
Exg2RegArray[3] = (byte)ReadByte();
Exg2RegArray[4] = (byte)ReadByte();
Exg2RegArray[5] = (byte)ReadByte();
Exg2RegArray[6] = (byte)ReadByte();
Exg2RegArray[7] = (byte)ReadByte();
Exg2RegArray[8] = (byte)ReadByte();
Exg2RegArray[9] = (byte)ReadByte();
}
break;
default:
// This is to extend log and stream functionality
if (GetFirmwareIdentifier() == FW_IDENTIFIER_LOGANDSTREAM)
{
SDBT_switch(b);
}
else
{
System.Console.WriteLine("?");
}
break;
}
}
}
catch (System.TimeoutException)
{
//
if (ShimmerState == SHIMMER_STATE_STREAMING)
{
System.Console.WriteLine("Timeout Streaming");
StreamTimeOutCount++;
if (StreamTimeOutCount == 10)
{
CustomEventArgs newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, "Connection lost" );
OnNewEvent(newEventArgs);
Disconnect();
}
}
}
catch (System.InvalidOperationException)
{
}
catch (System.IO.IOException)
{
}
}
// only stop reading when disconnecting, so disconnect serial port here too
CloseConnection();
}
