Exemplo n.º 1
0
        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;
        }
Exemplo n.º 2
0
 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);
         }
     }
 }