From b7ea2e989ecdd27d17af458f9626b9f19b97a291 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 14 Jan 2025 17:07:35 +0800 Subject: [PATCH 01/20] some update on simulator unit test --- .../ShimmerAPI/LiteProtocolInstructionSet.cs | 3 + ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 4 +- .../ShimmerLogAndStreamS3RSimulator.cs | 29 ++ .../ShimmerLogAndStreamS3Simulator.cs | 33 +- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 300 +++++++++++++++++- .../ShimmerLogAndStreamS3SimulatorTest.cs | 77 ++++- 6 files changed, 430 insertions(+), 16 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs index 4f1d3227..449899d4 100644 --- a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs +++ b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs @@ -375,6 +375,9 @@ public enum InstructionsGet { [pbr::OriginalName("GET_CALIB_DUMP_COMMAND")] GetCalibDumpCommand = 154, [pbr::OriginalName("GET_I2C_BATT_STATUS_COMMAND_VALUE")] GetI2CBattStatusCommandValue = 158, [pbr::OriginalName("GET_BMP280_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp280CalibrationCoefficientsCommand = 159, + [pbr::OriginalName("GET_BMP390_PRESS_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp390CalibrationCoefficientsCommand = 166, + [pbr::OriginalName("BMP390_CALIBRATION_COEFFICIENTS_RESPONSE")] Bmp390CalibrationCoefficientsResponse = 165, + } public enum InstructionsResponse { diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 40db4fca..0c42212d 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -118,7 +118,7 @@ public abstract class ShimmerBluetooth : ShimmerDevice protected double FirmwareIdentifier; protected int FirmwareInternal; protected String FirmwareVersionFullName; - protected List ListofSensorChannels = new List(); + public List ListofSensorChannels = new List(); protected Boolean Orientation3DEnabled = false; protected Boolean EnableGyroOnTheFlyCalibration = false; protected int ListSizeGyroOnTheFly = 100; @@ -7199,7 +7199,7 @@ protected double[] CalibratePressure280SensorData(double UP, double UT) caldata[1] = T;///10; // TODO divided by 10 in BMP180, needed here? return caldata; } - protected double[] CalibratePressure390SensorData(double UP, double UT) + public double[] CalibratePressure390SensorData(double UP, double UT) { double uncompTemp = UT; double partialDataT1; diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3RSimulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3RSimulator.cs index a19d94b5..4cb7e8d2 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3RSimulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3RSimulator.cs @@ -29,6 +29,35 @@ protected override void TxFirmwareVersion() mBuffer.Add((byte)0x00); mBuffer.Add((byte)0x01); } + + public byte[] GetPressureResoTest() + { + byte[] pressureResoResTest = + { + (byte) 0xE7, (byte) 0x6B, (byte) 0xF0, (byte) 0x4A, (byte) 0xF9, + (byte) 0xAB, (byte) 0x1C, (byte) 0x9B, (byte) 0x15, (byte) 0x06, + (byte) 0x01, (byte) 0xD2, (byte) 0x49, (byte) 0x18, (byte) 0x5F, + (byte) 0x03, (byte) 0xFA, (byte) 0x3A, (byte) 0x0F, (byte) 0x07, + (byte) 0xF5 + }; + return pressureResoResTest; + } + + public byte[] GetTestDataPacket() + { + byte[] newPacket = { + 188, 19, 112, 203, 7, 9, 8, 190, 4, 24, 251, 7, 2, 240, 186, + (byte)0x00, (byte)0xCF, (byte)0x7F, (byte)0x00, (byte)0x17, (byte)0x64, + 128, 186, 181, 80, 4, 169, 40, 128, 127, 255, 255, 253, 80, 71 + }; + return newPacket; + } + + public String[] GetTestDataType() + { + String[] dataType = { "u24", "i16", "i16", "i16", "i16", "i16", "i16", "u24", "u24", "u8", "i24r", "i24r", "u8", "i24r", "i24r", null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null }; + return dataType; + } } } diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs index 24bf1395..a73943e5 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs @@ -15,6 +15,9 @@ public class ShimmerLogAndStreamS3Simulator : ShimmerLogAndStream public bool isConnectionOpen = false; protected BlockingCollection mBuffer = new BlockingCollection(1000); // Fixed size 1000 + public bool isGetBmp390CalibrationCoefficientsCommand = false; + public bool isGetPressureCalibrationCoefficientsCommand = false; + public bool mIsNewBMPSupported; public ShimmerLogAndStreamS3Simulator(string devID, string bComPort) : base(devID) @@ -44,6 +47,10 @@ public ShimmerLogAndStreamS3Simulator(string devID, string bComPort, double samp { ComPort = bComPort; } + public void SetIsNewBMPSupported(bool isNewBMPSupported) + { + mIsNewBMPSupported = isNewBMPSupported; + } public override string GetShimmerAddress() { @@ -181,8 +188,9 @@ protected override void WriteBytes(byte[] buffer, int index, int length) mBuffer.Add((byte)0x9B); } } - else if (buffer[0] == (byte)InstructionsGet.GetBmp280CalibrationCoefficientsCommand) + else if (buffer[0] == (byte)InstructionsGet.GetBmp390CalibrationCoefficientsCommand) { + isGetBmp390CalibrationCoefficientsCommand = true; mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x9f); byte[] bytes = UtilShimmer.HexStringToByteArray("7A6A0D6632007F9016D7D00BBC1B2AFFF9FF8C3CF8C67017"); @@ -192,6 +200,18 @@ protected override void WriteBytes(byte[] buffer, int index, int length) } mBuffer.Add((byte)0xDF); } + else if (buffer[0] == (byte)InstructionsGet.Bmp390CalibrationCoefficientsResponse) + { + isGetPressureCalibrationCoefficientsCommand = true; + mBuffer.Add((byte)0xff); + mBuffer.Add((byte)0xa6); + byte[] bytes = UtilShimmer.HexStringToByteArray("19011D6BBA643200859289D6D00BC918CBFFF9FF7B1A1FEE4DFC"); + foreach (byte byteValue in bytes) + { + mBuffer.Add(byteValue); + } + mBuffer.Add((byte)0xDF); + } else if (buffer[0] == (byte)PacketTypeShimmer2.GET_BLINK_LED) { mBuffer.Add((byte)0xff); @@ -211,7 +231,16 @@ protected override void WriteBytes(byte[] buffer, int index, int length) { mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x02); - byte[] bytes = UtilShimmer.HexStringToByteArray("800202FF01080001"); + byte[] bytes; + if (GetShimmerVersion() == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) + { + bytes = UtilShimmer.HexStringToByteArray("800202FF01080000000001"); + } + else + { + bytes = UtilShimmer.HexStringToByteArray("800202FF01080001"); + } + foreach (byte byteValue in bytes) { mBuffer.Add(byteValue); diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index c675f76a..28102f8e 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -1,8 +1,14 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI; using ShimmerAPI.Simulators; +using ShimmerAPI.Utilities; using System; +using System.Collections.Concurrent; +using System.Diagnostics; +using System.Linq; using System.Threading; using System.Threading.Tasks; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerBluetoothTests { @@ -11,6 +17,7 @@ public class ShimmerLogAndStreamS3RSimulatorTest { ShimmerLogAndStreamS3RSimulator mDevice; String ComPort = "COM99"; + ConcurrentQueue cq = new ConcurrentQueue(); [TestInitialize] public void SetUp() @@ -22,27 +29,191 @@ public void SetUp() public void Test001_testConnectandDisconnect() { //Comment out test as it is not completed - /* + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + // if (!mDevice.GetFirmwareVersionFullName().Equals("LogAndStream 0.0.1")) + // { + // Assert.Fail(); + // } + + // if (!mDevice.GetShimmerVersion().Equals(10)) //Shimmer3R + // { + // Assert.Fail(); + // } + + // if (!mDevice.isGetPressureCalibrationCoefficientsCommand) + // { + // Assert.Fail(); + // } + + // if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) + // { + // Assert.Fail(); + // } + + // foreach (byte b in mDevice.ListofSensorChannels) + // { + // Debug.WriteLine(b + " ; "); + // } + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + + [TestMethod] + public void Test002_ConnectandTestBMP390() + { + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + // try + // { + // mDevice.CalculateBMP390PressureCalibrationCoefficientsResponse(mDevice.GetPressureResoTest()); + + // string[] sensorDataType = { "u24" }; + + // byte[] sensorDataP2 = { 0x00, 0x17, 0x64 }; + // byte[] sensorDataT2 = { 0x00, 0xCF, 0x7F }; + + // long[] uncalibResultP2 = ProgrammerUtilities.ParseData(sensorDataP2, sensorDataType); + // long[] uncalibResultT2 = ProgrammerUtilities.ParseData(sensorDataT2, sensorDataType); + // double[] bmpX80caldata2 = new double[2]; + // bmpX80caldata2 = mDevice.CalibratePressure390SensorData(uncalibResultP2[0], uncalibResultT2[0]); + // Bmp3QuantizedCalibData.TLin = bmpX80caldata2[1]; + + // //Assert.AreEqual(resultP2, 100912.81758676282); + // //Assert.AreEqual(resultT2, 23.26587201654911); + // Assert.AreEqual(Math.Round(bmpX80caldata2[0], 4), 100912.8176); + // Assert.AreEqual(Math.Round(bmpX80caldata2[1], 4), 23.2659); + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + + [TestMethod] + public void Test003_ConnectandTestCalibParamRead() + { + //need to work on calib dump + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + + [TestMethod] + public void Test004_ConnectandTestDefaultLNAccelAndGyroCalibParam() + { if (mDevice != null) { try { mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); Thread.Sleep(30000); if (!mDevice.IsConnected()) { Assert.Fail(); } - if (!mDevice.GetFirmwareVersionFullName().Equals("LogAndStream 0.0.1")) - { - Assert.Fail(); - } + //mDevice.ReadCalibrationParameters("All"); + //mDevice.RetrieveKinematicCalibrationParametersFromPacket(); - if (!mDevice.GetShimmerVersion().Equals(10)) //Shimmer3R - { - Assert.Fail(); - } + mDevice.WriteAccelRange(0); + mDevice.WriteSensors((int)ShimmerBluetooth.SensorBitmapShimmer3.SENSOR_A_ACCEL); + double[,] lnAccelOffset = mDevice.OffsetVectorAccel; + Assert.IsTrue(lnAccelOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); + + double[,] lnAccelAlignment = mDevice.AlignmentMatrixAccel; + Assert.IsTrue(lnAccelAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, -1 } }.Cast())); + + double[,] lnAccelSensitivity0 = mDevice.SensitivityMatrixAccel; + Assert.IsTrue(lnAccelSensitivity0.Cast().SequenceEqual(new double[,] { { 1672, 0, 0 }, { 0, 1672, 0 }, { 0, 0, 1672 } }.Cast())); + + mDevice.WriteAccelRange(1); + double[,] lnAccelSensitivity1 = mDevice.SensitivityMatrixAccel; + Assert.IsTrue(lnAccelSensitivity1.Cast().SequenceEqual(new double[,] { { 836, 0, 0 }, { 0, 836, 0 }, { 0, 0, 836 } }.Cast())); + + mDevice.WriteAccelRange(2); + double[,] lnAccelSensitivity2 = mDevice.SensitivityMatrixAccel; + Assert.IsTrue(lnAccelSensitivity2.Cast().SequenceEqual(new double[,] { { 418, 0, 0 }, { 0, 418, 0 }, { 0, 0, 418 } }.Cast())); + + mDevice.WriteAccelRange(3); + double[,] lnAccelSensitivity3 = mDevice.SensitivityMatrixAccel; + Assert.IsTrue(lnAccelSensitivity3.Cast().SequenceEqual(new double[,] { { 209, 0, 0 }, { 0, 209, 0 }, { 0, 0, 209 } }.Cast())); + + + mDevice.WriteGyroRange(0); + double[,] gyroOffset = mDevice.OffsetVectorGyro; + Assert.IsTrue(gyroOffset.Cast().SequenceEqual(new double[,] { { 1843 }, { 1843 }, { 1843 } }.Cast())); + + double[,] gyroAlignment = mDevice.AlignmentMatrixGyro; + Assert.IsTrue(gyroAlignment.Cast().SequenceEqual(new double[,] { { 0, -1, 0 }, { -1, 0, 0 }, { 0, 0, -1 } }.Cast())); + + double[,] gyroSensitivity0 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity0.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); + + mDevice.WriteGyroRange(1); + double[,] gyroSensitivity1 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity1.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); + + mDevice.WriteGyroRange(2); + double[,] gyroSensitivity2 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity2.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); } catch (Exception ex) { @@ -53,7 +224,114 @@ public void Test001_testConnectandDisconnect() { Assert.Fail("mDevice is null"); } - */ + } + [TestMethod] + public void Test005_ConnectandTestDefaultWRAccelCalibParam() + { + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + [TestMethod] + public void Test006_ConnectandTestDefaultMagCalibParam() + { + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + [TestMethod] + public void Test007_ConnectandTestDefaultHighGAccelCalibParam() + { + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} + } + [TestMethod] + public void Test008_ConnectandTestDefaultWRMagCalibParam() + { + //if (mDevice != null) + //{ + // try + // { + // mDevice.StartConnectThread(); + // mDevice.SetIsNewBMPSupported(true); + // Thread.Sleep(30000); + // if (!mDevice.IsConnected()) + // { + // Assert.Fail(); + // } + + + // } + // catch (Exception ex) + // { + // Assert.Fail($"Test aborted due to exception: {ex.Message}"); + // } + //} + //else + //{ + // Assert.Fail("mDevice is null"); + //} } } -} +} \ No newline at end of file diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index b9b2d6d3..e4c26d13 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -1,4 +1,5 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI; using ShimmerAPI.Simulators; using System; using System.Collections.Concurrent; @@ -6,6 +7,7 @@ using System.Text; using System.Threading; using System.Threading.Tasks; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerBluetoothTests { @@ -15,6 +17,7 @@ public class ShimmerLogAndStreamS3SimulatorTest ShimmerLogAndStreamS3Simulator mDevice; String ComPort = "COM99"; ConcurrentQueue cq = new ConcurrentQueue(); + protected int SetEnabledSensors = (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE; [TestInitialize] public void SetUp() @@ -29,6 +32,7 @@ public void Test001_testConnectandDisconnect() { try { + mDevice.SetIsNewBMPSupported(false); mDevice.StartConnectThread(); Thread.Sleep(30000); if (!mDevice.IsConnected()) @@ -45,6 +49,78 @@ public void Test001_testConnectandDisconnect() { Assert.Fail(); } + + //if (!mDevice.isGetBmp390CalibrationCoefficientsCommand) + //{ + // Assert.Fail(); + //} + + //if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) + //{ + // Assert.Fail(); + //} + + try + { + mDevice.Disconnect(); + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } + + } + + [TestMethod] + public void Test002_testConnectandDisconnect_NewBMPSupported() + { + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(false); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + if (!mDevice.GetFirmwareVersionFullName().Equals("LogAndStream 0.16.9")) + { + Assert.Fail(); + } + + if (!mDevice.GetShimmerVersion().Equals(3)) //Shimmer3 + { + Assert.Fail(); + } + + //if (!mDevice.isGetBmp390CalibrationCoefficientsCommand) + //{ + // Assert.Fail(); + //} + + //if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) + //{ + // Assert.Fail(); + //} + + //foreach (byte b in mDevice.ListofSensorChannels) + //{ + // Console.WriteLine(b + " ; "); + //} + } catch (Exception ex) { @@ -55,7 +131,6 @@ public void Test001_testConnectandDisconnect() { Assert.Fail("mDevice is null"); } - } } From 485d56bbeb35eee99f26f977c8e3ed8658233122 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Thu, 16 Jan 2025 09:45:51 +0800 Subject: [PATCH 02/20] updating sensor class and calibration dump method --- ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 28 +++--- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 90 ++++++++++++++++++- .../ShimmerLogAndStreamS3Simulator.cs | 10 ++- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 27 +++++- .../ShimmerLogAndStreamS3SimulatorTest.cs | 20 +++++ 5 files changed, 157 insertions(+), 18 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index 5257d476..f3b8029a 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -1,6 +1,7 @@ using ShimmerAPI.Utilities; using System; using System.Collections.Generic; +using System.Linq; using System.Text; namespace ShimmerAPI.Sensors @@ -13,19 +14,24 @@ public class LNAccel public double[,] OffsetVectorAccel = new double[3, 1]; public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { - - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + byte[] remainingBytes = sensorcalibrationdump.Skip(10).ToArray(); + while (remainingBytes.Length > 12) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("LN Accel calibration parameters"); - } + byte[] packetType = remainingBytes.Take(2).ToArray(); + //int sensorId = ((sensorIdBytes[1] << 8) | sensorIdBytes[0]) & 0xFFFF; + //var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); + //var sensorID = (((int)packetType[0]) + ((int)packetType[1] << 8)); + var sensorID = ((packetType[1] << 8) | packetType[0]) & 0xFFFF; + if (sensorID == CALIBRATION_ID) + { + var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); + var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); + var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); + (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("LN Accel calibration parameters"); + } + } } - } } diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 3c0ca735..5b828106 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -8,6 +8,7 @@ using System.ComponentModel; using System.Diagnostics; using ShimmerAPI.Utilities; +using ShimmerAPI.Sensors; namespace ShimmerAPI { @@ -243,6 +244,8 @@ public enum ChannelContents private long configtime = 0; public byte[] storedConfig = new byte[118]; + private List calibDumpResponse = new List(); + // btsd changes private bool dataReceived; // btsd changes @@ -704,12 +707,12 @@ private void readInfoMem() //worker.ReportProgress(40, status_text); if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - //ReadCalibDump(); + ReadCalibDump(); ReadCalibrationParameters("All"); } else { - //ReadCalibDump(); + ReadCalibDump(); ReadCalibrationParameters("All"); } @@ -854,6 +857,8 @@ public override void WriteShimmerName() int InfoMemIndex = 0; byte[] InfoMem = new byte[128*3]; byte[] CalibDump = new byte[128]; + private Dictionary mCalibBytesDescriptions; + private byte[] mCalibBytes; [Obsolete("This method is unfinished and should not be used.")] public void WriteShimmer3Infomem(byte[] infoMem) @@ -931,8 +936,6 @@ public void ReadShimmer3InfoMem() [Obsolete("This method is unfinished and should not be used.")] public void ReadCalibDump() { - - List calibDumpResponse = new List(); byte[] byteResult = SendGetMemoryCommand((byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_CALIB_DUMP_COMMAND, 0x80, 0x00, 0x00); @@ -964,6 +967,10 @@ public void ReadCalibDump() } } + CalibByteDumpParse(calibDumpResponse.ToArray()); + + Console.WriteLine(calibDumpResponse); + /* WriteBytes(new byte[4] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_CALIB_DUMP_COMMAND, 0x80, 0x00, 0x00 }, 0, 4); @@ -993,6 +1000,81 @@ public void ReadCalibDump() */ } + public void CalibByteDumpParse(byte[] calibBytesAll) + { + mCalibBytesDescriptions = new Dictionary(); + mCalibBytes = calibBytesAll; + + if (calibBytesAll.Length > 2) + { + mCalibBytesDescriptions[0] = "PacketLength_LSB"; + mCalibBytesDescriptions[1] = "PacketLength_MSB"; + + var packetLength = calibBytesAll.Take(2).ToArray(); + var svoBytes = calibBytesAll.Skip(2).Take(8).ToArray(); + //var svo = new ShimmerVerObject(svoBytes); + + //mCalibBytesDescriptions[2] = $"HwID_LSB ({svo.GetHardwareVersionParsed()})"; + mCalibBytesDescriptions[3] = "HwID_MSB"; + //mCalibBytesDescriptions[4] = $"FwID_LSB ({svo.mFirmwareIdentifierParsed})"; + mCalibBytesDescriptions[5] = "FwID_MSB"; + mCalibBytesDescriptions[6] = "FWVerMjr_LSB"; + mCalibBytesDescriptions[7] = "FWVerMjr_MSB"; + mCalibBytesDescriptions[8] = "FWVerMinor"; + mCalibBytesDescriptions[9] = "FWVerInternal"; + + int currentOffset = 10; + var remainingBytes = calibBytesAll.Skip(10).ToArray(); + + while (remainingBytes.Length > 12) + { + var sensorIdBytes = remainingBytes.Take(2).ToArray(); + int sensorId = ((sensorIdBytes[1] << 8) | sensorIdBytes[0]) & 0xFFFF; + + //var sensorDetails = GetSensorDetails(sensorId); + //var sensorName = sensorDetails?.mSensorDetailsRef?.mGuiFriendlyLabel ?? ""; + + mCalibBytesDescriptions[currentOffset] = $"SensorID_LSB ({sensorId})"; + mCalibBytesDescriptions[currentOffset + 1] = "SensorID_MSB"; + + mCalibBytesDescriptions[currentOffset + 2] = "SensorRange"; + int rangeValue = remainingBytes[2] & 0xFF; + + mCalibBytesDescriptions[currentOffset + 3] = "CalibByteLength"; + int calibLength = remainingBytes[3] & 0xFF; + + mCalibBytesDescriptions[currentOffset + 4] = "CalibTimeTicks_LSB"; + mCalibBytesDescriptions[currentOffset + 11] = "CalibTimeTicks_MSB"; + + var calibTimeBytesTicks = remainingBytes.Skip(4).Take(8).ToArray(); + + int endIndex = 12 + calibLength; + if (remainingBytes.Length >= endIndex) + { + mCalibBytesDescriptions[currentOffset + 12] = "Calib_Start"; + mCalibBytesDescriptions[currentOffset + endIndex - 1] = "Calib_End"; + + var calibBytes = remainingBytes.Skip(12).Take(calibLength).ToArray(); + //CalibByteDumpParsePerSensor(sensorId, rangeValue, calibTimeBytesTicks, calibBytes, calibReadSource); + LNAccel lnAccel = new LNAccel(); + lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(mCalibBytes); + + remainingBytes = remainingBytes.Skip(endIndex).ToArray(); + currentOffset += endIndex; + } + else + { + break; + } + } + } + } + + public List GetCalibrationDump() + { + return calibDumpResponse; + } + protected byte[] SendGetMemoryCommand(byte cmd, byte val0, byte val1, byte val2) { diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs index 24bf1395..f5140a4d 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs @@ -211,7 +211,15 @@ protected override void WriteBytes(byte[] buffer, int index, int length) { mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x02); - byte[] bytes = UtilShimmer.HexStringToByteArray("800202FF01080001"); + byte[] bytes; + if (GetShimmerVersion() == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) + { + bytes = UtilShimmer.HexStringToByteArray("800202FF01080000000001"); + } + else + { + bytes = UtilShimmer.HexStringToByteArray("800202FF01080001"); + } foreach (byte byteValue in bytes) { mBuffer.Add(byteValue); diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index c675f76a..1a9c2e0c 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -1,6 +1,10 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI.Sensors; using ShimmerAPI.Simulators; +using ShimmerAPI.Utilities; using System; +using System.Collections.Generic; +using System.Diagnostics; using System.Threading; using System.Threading.Tasks; @@ -22,13 +26,14 @@ public void SetUp() public void Test001_testConnectandDisconnect() { //Comment out test as it is not completed - /* + if (mDevice != null) { try { mDevice.StartConnectThread(); Thread.Sleep(30000); + if (!mDevice.IsConnected()) { Assert.Fail(); @@ -43,6 +48,7 @@ public void Test001_testConnectandDisconnect() { Assert.Fail(); } + //ProcessCalibrationData(); } catch (Exception ex) { @@ -53,7 +59,24 @@ public void Test001_testConnectandDisconnect() { Assert.Fail("mDevice is null"); } - */ + } + public void ProcessCalibrationData() + { + byte[] calibDump = mDevice.GetCalibrationDump().ToArray(); + + if (calibDump == null || calibDump.Length < 2) + { + throw new ArgumentException("Invalid calibDump: must contain at least 2 bytes."); + } + + //mDevice.WriteAccelRange(0); + LNAccel lnAccel = new LNAccel(); + lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDump); + //mDevice.WriteGyroRange(0); + //GyroSensor gyro = new GyroSensor(); + //gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDumpResponse.ToArray()); + } + } } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index b9b2d6d3..39cb624f 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -1,4 +1,5 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI.Sensors; using ShimmerAPI.Simulators; using System; using System.Collections.Concurrent; @@ -45,6 +46,8 @@ public void Test001_testConnectandDisconnect() { Assert.Fail(); } + + //ProcessCalibrationData(); } catch (Exception ex) { @@ -58,5 +61,22 @@ public void Test001_testConnectandDisconnect() } + public void ProcessCalibrationData() + { + byte[] calibDump = mDevice.GetCalibrationDump().ToArray(); + + if (calibDump == null || calibDump.Length < 2) + { + throw new ArgumentException("Invalid calibDump: must contain at least 2 bytes."); + } + + //mDevice.WriteAccelRange(0); + LNAccel lnAccel = new LNAccel(); + lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDump); + //mDevice.WriteGyroRange(0); + //GyroSensor gyro = new GyroSensor(); + //gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDumpResponse.ToArray()); + } + } } From 513f0ce4e16edc09274687d5bbc113573342ffd0 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 21 Jan 2025 13:53:11 +0800 Subject: [PATCH 03/20] more update on the calib dump parsing for sensor class --- ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 102 ++++++- ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs | 2 +- ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 10 + ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 261 +++++++++++++++++- .../ShimmerAPI/Utilities/UtilShimmer.cs | 38 +++ .../ShimmerLogAndStreamS3RSimulatorTest.cs | 50 +++- 6 files changed, 426 insertions(+), 37 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index f3b8029a..4cbb7f8a 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -3,34 +3,106 @@ using System.Collections.Generic; using System.Linq; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { public class LNAccel { public readonly int CALIBRATION_ID = 2; + public readonly int SENSOR_ID = 37; public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + + public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 1672, 0, 0 }, { 0, 1672, 0 }, { 0, 0, 1672 } }; + public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 836, 0, 0 }, { 0, 836, 0 }, { 0, 0, 836 } }; + public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 418, 0, 0 }, { 0, 418, 0 }, { 0, 0, 418 } }; + public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 209, 0, 0 }, { 0, 209, 0 }, { 0, 0, 209 } }; + public static readonly double[,] ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV = new double[3, 3] { { -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, -1 } }; + public static readonly double[,] OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV = new double[3, 1] { { 0 }, { 0 }, { 0 } }; + + public static List ListofLNAccelRange = new List { 0, 1, 2, 3 }; + + public static List sensitivityMatrixMap = new List { - byte[] remainingBytes = sensorcalibrationdump.Skip(10).ToArray(); - while (remainingBytes.Length > 12) - { - byte[] packetType = remainingBytes.Take(2).ToArray(); - //int sensorId = ((sensorIdBytes[1] << 8) | sensorIdBytes[0]) & 0xFFFF; + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV + }; + + public static List alignmentMatrixMap = new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV + }; + + public static List offsetVectorMap = new List + { + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV + }; - //var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - //var sensorID = (((int)packetType[0]) + ((int)packetType[1] << 8)); - var sensorID = ((packetType[1] << 8) | packetType[0]) & 0xFFFF; - if (sensorID == CALIBRATION_ID) + public static Dictionary> calibDetailsAccelLN = new Dictionary>() + { + { + ListofLNAccelRange[0], + new List + { + alignmentMatrixMap[0], + sensitivityMatrixMap[0], + offsetVectorMap[0] + } + }, + { + ListofLNAccelRange[1], + new List { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("LN Accel calibration parameters"); + alignmentMatrixMap[0], + sensitivityMatrixMap[1], + offsetVectorMap[0] } + }, + { + ListofLNAccelRange[2], + new List + { + alignmentMatrixMap[0], + sensitivityMatrixMap[2], + offsetVectorMap[0] + } + }, + { + ListofLNAccelRange[3], + new List + { + alignmentMatrixMap[0], + sensitivityMatrixMap[3], + offsetVectorMap[0] + } + } + }; + + public Dictionary> GetCalibDetails() + { + return calibDetailsAccelLN; + } + + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); + var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); + + if (sensorID == CALIBRATION_ID) + { + var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); + var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); + var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); + (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + Console.WriteLine("LN Accel calibration parameters retrieved successfully."); + } + else + { + Console.WriteLine("Invalid calibration ID."); } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs index f3f74627..0b727e27 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs @@ -5,7 +5,7 @@ namespace ShimmerAPI.Sensors { - class MagSensor + public class MagSensor { public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixMag = new double[3, 3]; diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 40db4fca..26c35af5 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -75,6 +75,7 @@ using static com.shimmerresearch.radioprotocol.LiteProtocolInstructionSet.Types; using ShimmerAPI.Utilities; using static ShimmerAPI.ShimmerBluetooth; +using ShimmerAPI.Sensors; namespace ShimmerAPI { @@ -256,6 +257,15 @@ public abstract class ShimmerBluetooth : ShimmerDevice protected long ShimmerRealWorldClock = 0; protected List UnalignedBytesReceived = new List(); + protected List mMapOfSensorClasses = new List(); + + protected LNAccel lnAccel = new LNAccel(); + protected WRAccel wrAccel = new WRAccel(); + protected HighGAccel highgAccel = new HighGAccel(); + protected GyroSensor gyro = new GyroSensor(); + protected MagSensor mag = new MagSensor(); + protected WRMag wrMag = new WRMag(); + public enum BTCRCMode { OFF = 0, diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 5b828106..292fdb02 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -9,6 +9,9 @@ using System.Diagnostics; using ShimmerAPI.Utilities; using ShimmerAPI.Sensors; +using NUnit.Framework.Internal; +using System.Collections; +using static ShimmerAPI.ShimmerLogAndStream; namespace ShimmerAPI { @@ -188,6 +191,30 @@ public enum ChannelContents STRAIN_LOW = 0x28, GsrRaw = 0x1C } + + public enum CalibrationScaleFactor + { + NONE = 1, + TEN = 10, + ONE_HUNDRED = 100 + } + + public class Calibration + { + // TODO: offset scale factor currently not used + public static CalibrationScaleFactor mOffsetScaleFactor { get; set; } = CalibrationScaleFactor.NONE; + public static CalibrationScaleFactor mSensitivityScaleFactor { get; set; } = CalibrationScaleFactor.NONE; + public static CalibrationScaleFactor mAlignmentScaleFactor { get; set; } = CalibrationScaleFactor.ONE_HUNDRED; + public static double GetScaleFactor(CalibrationScaleFactor factor) + { + return 1.0; // (double)factor + } + } + + public double[][] mOffsetVector = null; + public double[][] mSensitivityMatrix = null; + public double[][] mAlignmentMatrix = null; + // btsd changes private int trialConfig; private int interval; @@ -707,11 +734,13 @@ private void readInfoMem() //worker.ReportProgress(40, status_text); if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { + CreateMapOfSensorClasses(); ReadCalibDump(); ReadCalibrationParameters("All"); } else { + CreateMapOfSensorClasses(); ReadCalibDump(); ReadCalibrationParameters("All"); } @@ -859,6 +888,11 @@ public override void WriteShimmerName() byte[] CalibDump = new byte[128]; private Dictionary mCalibBytesDescriptions; private byte[] mCalibBytes; + private int mHardwareVersion; + private int mFirmwareIdentifier; + private int mFirmwareVersionMajor; + private int mFirmwareVersionMinor; + private int mFirmwareVersionInternal; [Obsolete("This method is unfinished and should not be used.")] public void WriteShimmer3Infomem(byte[] infoMem) @@ -1000,6 +1034,174 @@ public void ReadCalibDump() */ } + private void CreateMapOfSensorClasses() + { + + //lnAccel = new LNAccel(); + mMapOfSensorClasses.Add(lnAccel); //need to implement default/ current calibration matrix + + } + + public Dictionary>> GetMapOfSensorCalibrationAll() + { + Dictionary>> mapOfSensorCalibration = new Dictionary>>(); + + foreach (object sensorObj in mMapOfSensorClasses) + { + Dictionary> returnVal = lnAccel.GetCalibDetails(); //LNACCEL as placeholder, will update with appropriate getcalib for each sensor classes + if(returnVal != null) + { + mapOfSensorCalibration.Add(lnAccel.SENSOR_ID, returnVal); + } + + } + + return mapOfSensorCalibration; + } + + public byte[] CalibByteDumpGenerate() + { + byte[] calibBytesAll = new byte[] { }; + Dictionary>> mapOfAllCalib = GetMapOfSensorCalibrationAll(); + + foreach (var sensorEntry in mapOfAllCalib) + { + int sensorId = sensorEntry.Key; + var calibMapPerSensor = sensorEntry.Value; + + foreach (var calibEntry in calibMapPerSensor) + { + byte[] calibBytesPerSensor = GenerateCalibDump(calibEntry); + + if (calibBytesPerSensor != null) + { + byte[] calibSensorKeyBytes = new byte[2]; + calibSensorKeyBytes[0] = (byte)((sensorId >> 0) & 0xFF); + calibSensorKeyBytes[1] = (byte)((sensorId >> 8) & 0xFF); + + calibBytesPerSensor = calibSensorKeyBytes.Concat(calibBytesPerSensor).ToArray(); + calibBytesAll = calibBytesAll.Concat(calibBytesPerSensor).ToArray(); + } + } + } + + byte[] svoBytes = GenerateVersionByteArrayNew(); + byte[] concatBytes = svoBytes.Concat(calibBytesAll).ToArray(); + + byte[] packetLength = new byte[2]; + packetLength[0] = (byte)(concatBytes.Length & 0xFF); + packetLength[1] = (byte)((concatBytes.Length >> 8) & 0xFF); + + concatBytes = packetLength.Concat(concatBytes).ToArray(); + + return concatBytes; + } + + public byte[] GenerateCalibDump(KeyValuePair> calibEntry) + { + byte[] rangeBytes = new byte[1]; + int rangeValue = (int)calibEntry.Key; + rangeBytes[0] = (byte)(rangeValue & 0xFF); + + byte[] timestamp = UtilShimmer.ConvertMilliSecondsToShimmerRtcDataBytesLSB(0); //usually used getCalibTimeMs() + byte[] bufferCalibParam = GenerateCalParamByteArray(); + byte[] calibLength = new byte[] { (byte)bufferCalibParam.Length }; + + byte[] returnArray = rangeBytes.Concat(calibLength).ToArray(); + returnArray = returnArray.Concat(timestamp).ToArray(); + returnArray = returnArray.Concat(bufferCalibParam).ToArray(); + + return returnArray; + } + public byte[] GenerateCalParamByteArray() + { + //will continue adding this after setting up current/ default calibration + if (IsCurrentValuesSet()) + { + return GenerateCalParamByteArray(LNAccel.offsetVectorMap[0], LNAccel.sensitivityMatrixMap[0], LNAccel.alignmentMatrixMap[0]); + } + else + { + return GenerateCalParamByteArray(LNAccel.offsetVectorMap[0], LNAccel.sensitivityMatrixMap[0], LNAccel.alignmentMatrixMap[0]); + } + } + + public bool IsCurrentValuesSet() + { + if (mAlignmentMatrix != null && mSensitivityMatrix != null && mOffsetVector != null) + { + return true; + } + return false; + } + + public byte[] GenerateCalParamByteArray(double[,] offsetVector, double[,] sensitivityMatrix, double[,] alignmentMatrix) + { + // Scale the sensitivity if needed + double[,] sensitivityMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(sensitivityMatrix); + for (int i = 0; i <= 2; i++) + { + sensitivityMatrixToUse[i,i] = Math.Round(sensitivityMatrixToUse[i,i] * Calibration.GetScaleFactor(Calibration.mSensitivityScaleFactor)); + } + + // Scale the alignment by 100 + double[,] alignmentMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(alignmentMatrix); + for (int i = 0; i <= 2; i++) + { + alignmentMatrixToUse[i,0] = Math.Round(alignmentMatrixToUse[i,0] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); + alignmentMatrixToUse[i,1] = Math.Round(alignmentMatrixToUse[i,1] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); + alignmentMatrixToUse[i,2] = Math.Round(alignmentMatrixToUse[i,2] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); + } + + // Generate the calibration bytes + byte[] bufferCalibParam = new byte[21]; + + // offsetVector -> buffer offset = 0 + for (int i = 0; i < 3; i++) + { + bufferCalibParam[0 + (i * 2)] = (byte)((((int)offsetVector[i,0]) >> 8) & 0xFF); + bufferCalibParam[0 + (i * 2) + 1] = (byte)((((int)offsetVector[i,0]) >> 0) & 0xFF); + } + + // sensitivityMatrix -> buffer offset = 6 + for (int i = 0; i < 3; i++) + { + bufferCalibParam[6 + (i * 2)] = (byte)((((int)sensitivityMatrixToUse[i,i]) >> 8) & 0xFF); + bufferCalibParam[6 + (i * 2) + 1] = (byte)((((int)sensitivityMatrixToUse[i,i]) >> 0) & 0xFF); + } + + // alignmentMatrix -> buffer offset = 12 + for (int i = 0; i < 3; i++) + { + bufferCalibParam[12 + (i * 3)] = (byte)(((int)(alignmentMatrixToUse[i,0])) & 0xFF); + bufferCalibParam[12 + (i * 3) + 1] = (byte)(((int)(alignmentMatrixToUse[i,1])) & 0xFF); + bufferCalibParam[12 + (i * 3) + 2] = (byte)(((int)(alignmentMatrixToUse[i,2])) & 0xFF); + } + + return bufferCalibParam; + } + + + public byte[] GenerateVersionByteArrayNew() + { + byte[] byteArray = new byte[8]; + + int index = 0; + byteArray[index++] = (byte)(mHardwareVersion & 0xFF); + byteArray[index++] = (byte)((mHardwareVersion >> 8) & 0xFF); + + byteArray[index++] = (byte)(mFirmwareIdentifier & 0xFF); + byteArray[index++] = (byte)((mFirmwareIdentifier >> 8) & 0xFF); + + byteArray[index++] = (byte)(mFirmwareVersionMajor & 0xFF); + byteArray[index++] = (byte)((mFirmwareVersionMajor >> 8) & 0xFF); + + byteArray[index++] = (byte)(mFirmwareVersionMinor & 0xFF); + byteArray[index++] = (byte)(mFirmwareVersionInternal & 0xFF); + + return byteArray; + } + public void CalibByteDumpParse(byte[] calibBytesAll) { mCalibBytesDescriptions = new Dictionary(); @@ -1012,11 +1214,11 @@ public void CalibByteDumpParse(byte[] calibBytesAll) var packetLength = calibBytesAll.Take(2).ToArray(); var svoBytes = calibBytesAll.Skip(2).Take(8).ToArray(); - //var svo = new ShimmerVerObject(svoBytes); + parseVersionByteArray(svoBytes); - //mCalibBytesDescriptions[2] = $"HwID_LSB ({svo.GetHardwareVersionParsed()})"; + mCalibBytesDescriptions[2] = $"HwID_LSB (" + mHardwareVersion + ")"; mCalibBytesDescriptions[3] = "HwID_MSB"; - //mCalibBytesDescriptions[4] = $"FwID_LSB ({svo.mFirmwareIdentifierParsed})"; + mCalibBytesDescriptions[4] = $"FwID_LSB (" + mFirmwareIdentifier + ")"; mCalibBytesDescriptions[5] = "FwID_MSB"; mCalibBytesDescriptions[6] = "FWVerMjr_LSB"; mCalibBytesDescriptions[7] = "FWVerMjr_MSB"; @@ -1055,9 +1257,33 @@ public void CalibByteDumpParse(byte[] calibBytesAll) mCalibBytesDescriptions[currentOffset + endIndex - 1] = "Calib_End"; var calibBytes = remainingBytes.Skip(12).Take(calibLength).ToArray(); - //CalibByteDumpParsePerSensor(sensorId, rangeValue, calibTimeBytesTicks, calibBytes, calibReadSource); - LNAccel lnAccel = new LNAccel(); - lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(mCalibBytes); + + if (sensorId == 37) + { + RetrieveKinematicCalibrationParametersFromPacket(remainingBytes, (byte)PacketTypeShimmer2.ACCEL_CALIBRATION_RESPONSE); + //lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + else if (sensorId == 30 || sensorId == 38) + { + gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + else if (sensorId == 31 || sensorId == 39) + { + wrAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + else if (sensorId == 32 || sensorId == 42) + { + mag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + else if (sensorId == 33 || sensorId == 40) + { + highgAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + else if (sensorId == 34 || sensorId == 41) + { + wrMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + } + remainingBytes = remainingBytes.Skip(endIndex).ToArray(); currentOffset += endIndex; @@ -1070,6 +1296,29 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } } + public void parseVersionByteArray(byte[] byteArray) + { + if ((byteArray.Length == 7) || (byteArray.Length == 8)) + { + + int index = 0; + if (byteArray.Length == 7) + { + mHardwareVersion = byteArray[index++] & 0xFF; + } + else if (byteArray.Length == 8) + { + mHardwareVersion = (byteArray[index++] | (byteArray[index++] << 8)) & 0xFFFF; + } + + mFirmwareIdentifier = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; + mFirmwareVersionMajor = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; + mFirmwareVersionMinor = byteArray[index++] & 0xFF; + mFirmwareVersionInternal = byteArray[index++] & 0xFF; + + } + } + public List GetCalibrationDump() { return calibDumpResponse; diff --git a/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs b/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs index f46afa61..a52a945d 100644 --- a/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs +++ b/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs @@ -65,5 +65,43 @@ public static String BytesToHexString(byte[] bytes) return null; } } + + public static byte[] ConvertMilliSecondsToShimmerRtcDataBytesLSB(long milliseconds) + { + byte[] rtcTimeArray = ConvertMilliSecondsToShimmerRtcDataBytesMSB(milliseconds); + Array.Reverse(rtcTimeArray); + return rtcTimeArray; + } + + public static byte[] ConvertMilliSecondsToShimmerRtcDataBytesMSB(long milliseconds) + { + long milisecondTicks = (long)(milliseconds * 32.768); + byte[] rtcTimeArray = BitConverter.GetBytes(milisecondTicks); + + Array.Reverse(rtcTimeArray); + + return rtcTimeArray; + } + + public static double[,] DeepCopyDoubleMatrix(double[,] input) + { + if (input == null) + return null; + + int rows = input.GetLength(0); + int cols = input.GetLength(1); + + double[,] result = new double[rows, cols]; + + for (int r = 0; r < rows; r++) + { + for (int c = 0; c < cols; c++) + { + result[r, c] = input[r, c]; + } + } + return result; + } + } } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index 1a9c2e0c..1efa2a1e 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -1,4 +1,5 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI; using ShimmerAPI.Sensors; using ShimmerAPI.Simulators; using ShimmerAPI.Utilities; @@ -26,14 +27,14 @@ public void SetUp() public void Test001_testConnectandDisconnect() { //Comment out test as it is not completed - + if (mDevice != null) { try { mDevice.StartConnectThread(); Thread.Sleep(30000); - + if (!mDevice.IsConnected()) { Assert.Fail(); @@ -59,24 +60,43 @@ public void Test001_testConnectandDisconnect() { Assert.Fail("mDevice is null"); } - + } - public void ProcessCalibrationData() + + + [TestMethod] + public void Test003_ConnectandTestCalibParamRead() { - byte[] calibDump = mDevice.GetCalibrationDump().ToArray(); + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + Thread.Sleep(30000); - if (calibDump == null || calibDump.Length < 2) + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); //need to check this, calibration not updating the byte + mDevice.CalibByteDumpParse(deviceCalBytes); + mDevice.WriteAccelRange(2); + mDevice.WriteSensors((int)ShimmerBluetooth.SensorBitmapShimmer3.SENSOR_A_ACCEL); + + System.Console.WriteLine("done ..."); + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else { - throw new ArgumentException("Invalid calibDump: must contain at least 2 bytes."); + Assert.Fail("mDevice is null"); } - - //mDevice.WriteAccelRange(0); - LNAccel lnAccel = new LNAccel(); - lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDump); - //mDevice.WriteGyroRange(0); - //GyroSensor gyro = new GyroSensor(); - //gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDumpResponse.ToArray()); } - } } From 61092d2736659e6790faa2cbf94bb1ab53a27e50 Mon Sep 17 00:00:00 2001 From: JongChern Date: Tue, 21 Jan 2025 15:01:25 +0800 Subject: [PATCH 04/20] minor refactoring --- ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs | 7 +++++ ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 7 +++++ ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 6 +++++ ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs | 7 +++++ ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs | 7 +++++ ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 6 +++++ ShimmerAPI/ShimmerAPI/Shimmer4LogAndStream.cs | 2 +- ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 26 ++++++++++++++----- ShimmerAPI/ShimmerAPI/ShimmerDevice.cs | 3 ++- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 16 ++++++------ .../ShimmerLogAndStreamS3SimulatorTest.cs | 3 ++- 11 files changed, 73 insertions(+), 17 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs index 767a3157..3864ca92 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs @@ -7,10 +7,17 @@ namespace ShimmerAPI.Sensors { public class GyroSensor { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; + + public GyroSensor(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index 0dcb8569..eb9ef7d1 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -7,10 +7,17 @@ namespace ShimmerAPI.Sensors { public class HighGAccel { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; + + public HighGAccel(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index 4cbb7f8a..af8438bf 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -9,6 +9,7 @@ namespace ShimmerAPI.Sensors { public class LNAccel { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int SENSOR_ID = 37; public double[,] AlignmentMatrixAccel = new double[3, 3]; @@ -82,6 +83,11 @@ public class LNAccel } }; + public LNAccel(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public Dictionary> GetCalibDetails() { return calibDetailsAccelLN; diff --git a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs index 0b727e27..002443f2 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs @@ -7,10 +7,17 @@ namespace ShimmerAPI.Sensors { public class MagSensor { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixMag = new double[3, 3]; public double[,] SensitivityMatrixMag = new double[3, 3]; public double[,] OffsetVectorMag = new double[3, 1]; + + public MagSensor(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs index 4b26deb3..63b1e6b4 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs @@ -7,10 +7,17 @@ namespace ShimmerAPI.Sensors { public class WRAccel { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; + + public WRAccel(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index d5d58d91..255b6515 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -7,10 +7,16 @@ namespace ShimmerAPI.Sensors { public class WRMag { + protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public double[,] AlignmentMatrixMag = new double[3, 3]; public double[,] SensitivityMatrixMag = new double[3, 3]; public double[,] OffsetVectorMag = new double[3, 1]; + public WRMag(int hardwareVersion) + { + ShimmerHardwareVersion = hardwareVersion; + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { diff --git a/ShimmerAPI/ShimmerAPI/Shimmer4LogAndStream.cs b/ShimmerAPI/ShimmerAPI/Shimmer4LogAndStream.cs index 4edea793..292f8be8 100644 --- a/ShimmerAPI/ShimmerAPI/Shimmer4LogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/Shimmer4LogAndStream.cs @@ -9,7 +9,7 @@ namespace ShimmerAPI [Obsolete("This class is unfinished and should not be used.")] public abstract class Shimmer4LogAndStream : ShimmerLogAndStream { - LNAccel LowNoiseAccel = new LNAccel(); + LNAccel LowNoiseAccel = new LNAccel((int)ShimmerBluetooth.ShimmerVersion.SHIMMER4SDK); public Shimmer4LogAndStream(String devID) : base(devID) { diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 26c35af5..9dcfa485 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -259,12 +259,12 @@ public abstract class ShimmerBluetooth : ShimmerDevice protected List mMapOfSensorClasses = new List(); - protected LNAccel lnAccel = new LNAccel(); - protected WRAccel wrAccel = new WRAccel(); - protected HighGAccel highgAccel = new HighGAccel(); - protected GyroSensor gyro = new GyroSensor(); - protected MagSensor mag = new MagSensor(); - protected WRMag wrMag = new WRMag(); + protected LNAccel SensorLNAccel; + protected WRAccel SensorWRAccel; + protected HighGAccel SensorHighGAccel; + protected GyroSensor SensorGyro; + protected MagSensor SensorMag; + protected WRMag SensorWRMag; public enum BTCRCMode { @@ -922,6 +922,19 @@ public void StartConnectThread() protected int ConnectWaitDurationinmS = 500; + protected void InitializeSensors() + { + if (HardwareVersion == (int)ShimmerVersion.SHIMMER3R) + { + SensorLNAccel = new LNAccel(HardwareVersion); + SensorWRAccel = new WRAccel(HardwareVersion); + SensorHighGAccel = new HighGAccel(HardwareVersion); + SensorGyro = new GyroSensor(HardwareVersion); + SensorMag = new MagSensor(HardwareVersion); + SensorWRMag = new WRMag(HardwareVersion); + } + } + public void Connect() { if (!IsConnectionOpen()) @@ -999,6 +1012,7 @@ public void Connect() if (GetFirmwareIdentifier() == FW_IDENTIFIER_LOGANDSTREAM) { //WriteBatteryFrequency(0); + InitializeSensors(); ReadExpansionBoard(); InitializeShimmer3SDBT(); } diff --git a/ShimmerAPI/ShimmerAPI/ShimmerDevice.cs b/ShimmerAPI/ShimmerAPI/ShimmerDevice.cs index 4f7519f7..47570b6c 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerDevice.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerDevice.cs @@ -28,7 +28,8 @@ public enum ShimmerVersion SHIMMER2 = 1, SHIMMER2R = 2, SHIMMER3 = 3, - SHIMMER3R = 10 + SHIMMER3R = 10, + SHIMMER4SDK = 58 } protected double CalibrateTimeStamp(double timeStamp) { diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 292fdb02..b8bb6003 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -1038,7 +1038,7 @@ private void CreateMapOfSensorClasses() { //lnAccel = new LNAccel(); - mMapOfSensorClasses.Add(lnAccel); //need to implement default/ current calibration matrix + mMapOfSensorClasses.Add(SensorLNAccel); //need to implement default/ current calibration matrix } @@ -1048,10 +1048,10 @@ public Dictionary>> GetMapOfSensorCalibrati foreach (object sensorObj in mMapOfSensorClasses) { - Dictionary> returnVal = lnAccel.GetCalibDetails(); //LNACCEL as placeholder, will update with appropriate getcalib for each sensor classes + Dictionary> returnVal = SensorLNAccel.GetCalibDetails(); //LNACCEL as placeholder, will update with appropriate getcalib for each sensor classes if(returnVal != null) { - mapOfSensorCalibration.Add(lnAccel.SENSOR_ID, returnVal); + mapOfSensorCalibration.Add(SensorLNAccel.SENSOR_ID, returnVal); } } @@ -1265,23 +1265,23 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == 30 || sensorId == 38) { - gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorGyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); } else if (sensorId == 31 || sensorId == 39) { - wrAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorWRAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); } else if (sensorId == 32 || sensorId == 42) { - mag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); } else if (sensorId == 33 || sensorId == 40) { - highgAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorHighGAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); } else if (sensorId == 34 || sensorId == 41) { - wrMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorWRMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index 39cb624f..137606b0 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -1,4 +1,5 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using ShimmerAPI; using ShimmerAPI.Sensors; using ShimmerAPI.Simulators; using System; @@ -71,7 +72,7 @@ public void ProcessCalibrationData() } //mDevice.WriteAccelRange(0); - LNAccel lnAccel = new LNAccel(); + LNAccel lnAccel = new LNAccel((int)ShimmerBluetooth.ShimmerVersion.SHIMMER3); lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDump); //mDevice.WriteGyroRange(0); //GyroSensor gyro = new GyroSensor(); From 824557cf4806f0f9e9be913ddd878d975275b647 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Wed, 22 Jan 2025 13:59:25 +0800 Subject: [PATCH 05/20] some more updates on calib dump parsing and sensor classes map --- .../ShimmerAPI/Sensors/AbstractSensor.cs | 13 ++ ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs | 138 ++++++++++++++++- ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 43 +++++- ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 142 +++++++++-------- ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs | 83 +++++++++- ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs | 120 ++++++++++++++- ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 43 +++++- ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 29 +++- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 145 +++++++++++++----- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 11 +- 10 files changed, 624 insertions(+), 143 deletions(-) create mode 100644 ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs diff --git a/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs new file mode 100644 index 00000000..f2464022 --- /dev/null +++ b/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace ShimmerAPI.Sensors +{ + public interface AbstractSensor + { + int SENSOR_ID { get; } + Dictionary> GetCalibDetails(); + + } +} diff --git a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs index 3864ca92..2ab4c1fa 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs @@ -2,20 +2,146 @@ using System; using System.Collections.Generic; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { - public class GyroSensor + public class GyroSensor : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; - public double[,] AlignmentMatrixAccel = new double[3, 3]; - public double[,] SensitivityMatrixAccel = new double[3, 3]; - public double[,] OffsetVectorAccel = new double[3, 1]; + public int SENSOR_ID { get; private set; } + public double[,] AlignmentMatrixGyro = new double[3, 3]; + public double[,] SensitivityMatrixGyro = new double[3, 3]; + public double[,] OffsetVectorGyro = new double[3, 1]; + public Dictionary> calibDetailsGyro; public GyroSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 38; + + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV; + AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV; + OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV; + } + else + { + SENSOR_ID = 30; + + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3; + AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3; + OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3; + } + } + + public Dictionary> GetCalibDetails() + { + if (ShimmerHardwareVersion == 10) + { + calibDetailsGyro = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + }, + { + 4, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + }, + { + 5, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV, + SENSITIVITIY_MATRIX_GYRO_4000DPS_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV + } + } + }; + } + else + { + calibDetailsGyro = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3, + SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3, + OFFSET_VECTOR_GYRO_SHIMMER3 + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3, + SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3, + OFFSET_VECTOR_GYRO_SHIMMER3 + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3, + SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3, + OFFSET_VECTOR_GYRO_SHIMMER3 + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_GYRO_SHIMMER3, + SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3, + OFFSET_VECTOR_GYRO_SHIMMER3 + } + } + }; + } + return calibDetailsGyro; } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) @@ -23,12 +149,12 @@ public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sen var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (sensorID == SENSOR_ID) { var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + (AlignmentMatrixGyro, SensitivityMatrixGyro, OffsetVectorGyro) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); System.Console.WriteLine("Gyro calibration parameters"); } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index eb9ef7d1..707921f0 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -2,20 +2,51 @@ using System; using System.Collections.Generic; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { - public class HighGAccel + public class HighGAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; - public double[,] AlignmentMatrixAccel = new double[3, 3]; - public double[,] SensitivityMatrixAccel = new double[3, 3]; - public double[,] OffsetVectorAccel = new double[3, 1]; + public int SENSOR_ID { get; private set; } + public double[,] AlignmentMatrixAltAccel = new double[3, 3]; + public double[,] SensitivityMatrixAltAccel = new double[3, 3]; + public double[,] OffsetVectorAltAccel = new double[3, 1]; + public Dictionary> calibDetailsAltAccel; public HighGAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 40; + + SensitivityMatrixAltAccel = SENSITIVITY_MATRIX_HIGH_G_ACCEL_200G_SHIMMER3R_ADXL371; + AlignmentMatrixAltAccel = ALIGNMENT_MATRIX_HIGH_G_ACCEL_SHIMMER3R_ADXL371; + OffsetVectorAltAccel = OFFSET_VECTOR_ACCEL_HIGH_G_SHIMMER3R_ADXL371; + } + } + + public Dictionary> GetCalibDetails() + { + if (ShimmerHardwareVersion == 10) + { + calibDetailsAltAccel = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_HIGH_G_ACCEL_SHIMMER3R_ADXL371, + SENSITIVITY_MATRIX_HIGH_G_ACCEL_200G_SHIMMER3R_ADXL371, + OFFSET_VECTOR_ACCEL_HIGH_G_SHIMMER3R_ADXL371 + } + } + }; + } + return calibDetailsAltAccel; } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) @@ -23,12 +54,12 @@ public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sen var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (sensorID == SENSOR_ID) { var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + (AlignmentMatrixAltAccel, SensitivityMatrixAltAccel, OffsetVectorAltAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); System.Console.WriteLine("High-G Accel calibration parameters"); } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index af8438bf..d276f3e3 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -7,89 +7,97 @@ namespace ShimmerAPI.Sensors { - public class LNAccel + public class LNAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; - public readonly int SENSOR_ID = 37; + public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; + public Dictionary> calibDetailsAccelLN; - public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 1672, 0, 0 }, { 0, 1672, 0 }, { 0, 0, 1672 } }; - public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 836, 0, 0 }, { 0, 836, 0 }, { 0, 0, 836 } }; - public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 418, 0, 0 }, { 0, 418, 0 }, { 0, 0, 418 } }; - public static readonly double[,] SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV = new double[3, 3] { { 209, 0, 0 }, { 0, 209, 0 }, { 0, 0, 209 } }; - public static readonly double[,] ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV = new double[3, 3] { { -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, -1 } }; - public static readonly double[,] OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV = new double[3, 1] { { 0 }, { 0 }, { 0 } }; - - public static List ListofLNAccelRange = new List { 0, 1, 2, 3 }; - - public static List sensitivityMatrixMap = new List + public LNAccel(int hardwareVersion) { - SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV, - SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV, - SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV, - SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV - }; + ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 37; - public static List alignmentMatrixMap = new List - { - ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV - }; + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV; + AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV; + OffsetVectorAccel = OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV; + } + else + { + SENSOR_ID = 2; - public static List offsetVectorMap = new List - { - OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV - }; + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; + AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; + OffsetVectorAccel = OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3_KXTC9_2050; + } + } - public static Dictionary> calibDetailsAccelLN = new Dictionary>() + public Dictionary> GetCalibDetails() { - { - ListofLNAccelRange[0], - new List + if(ShimmerHardwareVersion == 10) + { + calibDetailsAccelLN = new Dictionary>() { - alignmentMatrixMap[0], - sensitivityMatrixMap[0], - offsetVectorMap[0] - } - }, - { - ListofLNAccelRange[1], - new List - { - alignmentMatrixMap[0], - sensitivityMatrixMap[1], - offsetVectorMap[0] - } - }, - { - ListofLNAccelRange[2], - new List - { - alignmentMatrixMap[0], - sensitivityMatrixMap[2], - offsetVectorMap[0] - } - }, + { + 0, + new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV, + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV + } + } + }; + } + else { - ListofLNAccelRange[3], - new List + calibDetailsAccelLN = new Dictionary>() { - alignmentMatrixMap[0], - sensitivityMatrixMap[3], - offsetVectorMap[0] - } + { + 0, + new List + { + ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050, + SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050, + OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3_KXTC9_2050 + } + } + }; } - }; - public LNAccel(int hardwareVersion) - { - ShimmerHardwareVersion = hardwareVersion; - } - - public Dictionary> GetCalibDetails() - { return calibDetailsAccelLN; } @@ -98,7 +106,7 @@ public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sen var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (sensorID == SENSOR_ID) { var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); diff --git a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs index 002443f2..ce7ecbb5 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs @@ -2,20 +2,101 @@ using System; using System.Collections.Generic; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { - public class MagSensor + public class MagSensor : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixMag = new double[3, 3]; public double[,] SensitivityMatrixMag = new double[3, 3]; public double[,] OffsetVectorMag = new double[3, 1]; + public Dictionary> calibDetailsMag; public MagSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 42; + + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; + AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL; + OffsetVectorMag = OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL; + } + else + { + SENSOR_ID = 32; + + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3_LSM303AH; + AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3_LSM303AH; + OffsetVectorMag = OFFSET_VECTOR_MAG_SHIMMER3_LSM303AH; + } + } + + public Dictionary> GetCalibDetails() + { + if (ShimmerHardwareVersion == 10) + { + calibDetailsMag = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL, + SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL, + OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL, + SENSITIVITY_MATRIX_MAG_8GA_SHIMMER3R_LIS3MDL, + OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL, + SENSITIVITY_MATRIX_MAG_12GA_SHIMMER3R_LIS3MDL, + OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL, + SENSITIVITY_MATRIX_MAG_16GA_SHIMMER3R_LIS3MDL, + OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL + } + }, + }; + } + else + { + calibDetailsMag = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3_LSM303AH, + SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3_LSM303AH, + OFFSET_VECTOR_MAG_SHIMMER3_LSM303AH + } + } + }; + } + return calibDetailsMag; } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs index 63b1e6b4..00e11806 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs @@ -2,20 +2,128 @@ using System; using System.Collections.Generic; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { - public class WRAccel + public class WRAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; - public double[,] AlignmentMatrixAccel = new double[3, 3]; - public double[,] SensitivityMatrixAccel = new double[3, 3]; - public double[,] OffsetVectorAccel = new double[3, 1]; + public int SENSOR_ID { get; private set; } + public double[,] AlignmentMatrixAccel2 = new double[3, 3]; + public double[,] SensitivityMatrixAccel2 = new double[3, 3]; + public double[,] OffsetVectorAccel2 = new double[3, 1]; + public Dictionary> calibDetailsAccel2; public WRAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 39; + + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12; + AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12; + OffsetVectorAccel2 = OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12; + } + else + { + SENSOR_ID = 31; + + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3_LSM303AH; + AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH; + OffsetVectorAccel2 = OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH; + } + } + + public Dictionary> GetCalibDetails() + { + if (ShimmerHardwareVersion == 10) + { + calibDetailsAccel2 = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12 + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_4G_SHIMMER3R_LIS2DW12, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12 + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_8G_SHIMMER3R_LIS2DW12, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12 + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_16G_SHIMMER3R_LIS2DW12, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12 + } + } + }; + } + else + { + calibDetailsAccel2 = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3_LSM303AH, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH + } + }, + { + 2, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_4G_SHIMMER3_LSM303AH, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH + } + }, + { + 3, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_8G_SHIMMER3_LSM303AH, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH + } + }, + { + 1, + new List + { + ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH, + SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_16G_SHIMMER3_LSM303AH, + OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH + } + } + }; + } + return calibDetailsAccel2; } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) @@ -23,12 +131,12 @@ public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sen var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (sensorID == SENSOR_ID) { var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + (AlignmentMatrixAccel2, SensitivityMatrixAccel2, OffsetVectorAccel2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); System.Console.WriteLine("WR Accel calibration parameters"); } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index 255b6515..8e4ed1fd 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -2,19 +2,50 @@ using System; using System.Collections.Generic; using System.Text; +using static ShimmerAPI.ShimmerBluetooth; namespace ShimmerAPI.Sensors { - public class WRMag + public class WRMag : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; - public double[,] AlignmentMatrixMag = new double[3, 3]; - public double[,] SensitivityMatrixMag = new double[3, 3]; - public double[,] OffsetVectorMag = new double[3, 1]; + public int SENSOR_ID { get; private set; } + public double[,] AlignmentMatrixMag2 = new double[3, 3]; + public double[,] SensitivityMatrixMag2 = new double[3, 3]; + public double[,] OffsetVectorMag2 = new double[3, 1]; + public Dictionary> calibDetailsMag; + public WRMag(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; + if (ShimmerHardwareVersion == 10) + { + SENSOR_ID = 41; + SensitivityMatrixMag2 = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3R_LIS2MDL; + AlignmentMatrixMag2 = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS2MDL; + OffsetVectorMag2 = OFFSET_VECTOR_MAG_SHIMMER3R_LIS2MDL; + } + } + + public Dictionary> GetCalibDetails() + { + if (ShimmerHardwareVersion == 10) + { + calibDetailsMag = new Dictionary>() + { + { + 0, + new List + { + ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS2MDL, + SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3R_LIS2MDL, + OFFSET_VECTOR_MAG_SHIMMER3R_LIS2MDL + } + } + }; + } + return calibDetailsMag; } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) @@ -22,12 +53,12 @@ public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sen var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (sensorID == SENSOR_ID) { var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixMag, SensitivityMatrixMag, OffsetVectorMag) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + (AlignmentMatrixMag2, SensitivityMatrixMag2, OffsetVectorMag2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); System.Console.WriteLine("WR Mag calibration parameters"); } diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 9dcfa485..07f5db9c 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -708,7 +708,7 @@ public enum ExpansionBoardDetectShimmer3 public static readonly double[,] SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3R_LSM6DSV = new double[3, 3] { { 57, 0, 0 }, { 0, 57, 0 }, { 0, 0, 57 } }; //Default Values for Gyroscope Calibration public static readonly double[,] SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3R_LSM6DSV = new double[3, 3] { { 29, 0, 0 }, { 0, 29, 0 }, { 0, 0, 29 } }; //Default Values for Gyroscope Calibration public static readonly double[,] SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3R_LSM6DSV = new double[3, 3] { { 14, 0, 0 }, { 0, 14, 0 }, { 0, 0, 14 } }; //Default Values for Gyroscope Calibration - public static readonly double[,] SENSITIVITIY_MATRIX_GYRO_40000DPS_SHIMMER3R_LSM6DSV = new double[3, 3] { { 7, 0, 0 }, { 0, 7, 0 }, { 0, 0, 7 } }; //Default Values for Gyroscope Calibration + public static readonly double[,] SENSITIVITIY_MATRIX_GYRO_4000DPS_SHIMMER3R_LSM6DSV = new double[3, 3] { { 7, 0, 0 }, { 0, 7, 0 }, { 0, 0, 7 } }; //Default Values for Gyroscope Calibration public static readonly double[,] OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV = new double[3, 1] { { 0 }, { 0 }, { 0 } }; //Default Values for Gyroscope Calibration // Shimmer3r LN Accel @@ -927,11 +927,28 @@ protected void InitializeSensors() if (HardwareVersion == (int)ShimmerVersion.SHIMMER3R) { SensorLNAccel = new LNAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorLNAccel); SensorWRAccel = new WRAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRAccel); SensorHighGAccel = new HighGAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorHighGAccel); SensorGyro = new GyroSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorGyro); SensorMag = new MagSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorMag); SensorWRMag = new WRMag(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRMag); + } + else + { + SensorLNAccel = new LNAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorLNAccel); + SensorWRAccel = new WRAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRAccel); + SensorGyro = new GyroSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorGyro); + SensorMag = new MagSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorMag); } } @@ -2678,7 +2695,7 @@ protected void RetrieveKinematicCalibrationParametersFromPacket(byte[] bufferCal } else if (GyroRange == 5) { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_40000DPS_SHIMMER3R_LSM6DSV; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_4000DPS_SHIMMER3R_LSM6DSV; } AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV; OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV; @@ -2789,19 +2806,19 @@ protected void RetrieveKinematicCalibrationParametersFromPacket(byte[] bufferCal AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL; OffsetVectorMag = OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL; - if (GetMagRange() == 1) + if (GetMagRange() == 0) { SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; } - else if (GetMagRange() == 2) + else if (GetMagRange() == 1) { SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_8GA_SHIMMER3R_LIS3MDL; } - else if (GetMagRange() == 3) + else if (GetMagRange() == 2) { SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_12GA_SHIMMER3R_LIS3MDL; } - else if (GetMagRange() == 4) + else if (GetMagRange() == 3) { SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_16GA_SHIMMER3R_LIS3MDL; } diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index b8bb6003..00b7ff5b 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -211,9 +211,9 @@ public static double GetScaleFactor(CalibrationScaleFactor factor) } } - public double[][] mOffsetVector = null; - public double[][] mSensitivityMatrix = null; - public double[][] mAlignmentMatrix = null; + private double[,] mAlignmentMatrix; + private double[,] mSensitivityMatrix; + private double[,] mVectorOffset; // btsd changes private int trialConfig; @@ -734,13 +734,11 @@ private void readInfoMem() //worker.ReportProgress(40, status_text); if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - CreateMapOfSensorClasses(); ReadCalibDump(); ReadCalibrationParameters("All"); } else { - CreateMapOfSensorClasses(); ReadCalibDump(); ReadCalibrationParameters("All"); } @@ -1034,24 +1032,16 @@ public void ReadCalibDump() */ } - private void CreateMapOfSensorClasses() - { - - //lnAccel = new LNAccel(); - mMapOfSensorClasses.Add(SensorLNAccel); //need to implement default/ current calibration matrix - - } - public Dictionary>> GetMapOfSensorCalibrationAll() { Dictionary>> mapOfSensorCalibration = new Dictionary>>(); - foreach (object sensorObj in mMapOfSensorClasses) + foreach (AbstractSensor sensorClass in mMapOfSensorClasses) { - Dictionary> returnVal = SensorLNAccel.GetCalibDetails(); //LNACCEL as placeholder, will update with appropriate getcalib for each sensor classes + Dictionary> returnVal = sensorClass.GetCalibDetails(); if(returnVal != null) { - mapOfSensorCalibration.Add(SensorLNAccel.SENSOR_ID, returnVal); + mapOfSensorCalibration.Add(sensorClass.SENSOR_ID, returnVal); } } @@ -1103,6 +1093,7 @@ public byte[] GenerateCalibDump(KeyValuePair> calibEntry) int rangeValue = (int)calibEntry.Key; rangeBytes[0] = (byte)(rangeValue & 0xFF); + (mAlignmentMatrix, mSensitivityMatrix, mVectorOffset) = (calibEntry.Value[0], calibEntry.Value[1], calibEntry.Value[2]); byte[] timestamp = UtilShimmer.ConvertMilliSecondsToShimmerRtcDataBytesLSB(0); //usually used getCalibTimeMs() byte[] bufferCalibParam = GenerateCalParamByteArray(); byte[] calibLength = new byte[] { (byte)bufferCalibParam.Length }; @@ -1113,26 +1104,10 @@ public byte[] GenerateCalibDump(KeyValuePair> calibEntry) return returnArray; } - public byte[] GenerateCalParamByteArray() - { - //will continue adding this after setting up current/ default calibration - if (IsCurrentValuesSet()) - { - return GenerateCalParamByteArray(LNAccel.offsetVectorMap[0], LNAccel.sensitivityMatrixMap[0], LNAccel.alignmentMatrixMap[0]); - } - else - { - return GenerateCalParamByteArray(LNAccel.offsetVectorMap[0], LNAccel.sensitivityMatrixMap[0], LNAccel.alignmentMatrixMap[0]); - } - } - public bool IsCurrentValuesSet() + public byte[] GenerateCalParamByteArray() { - if (mAlignmentMatrix != null && mSensitivityMatrix != null && mOffsetVector != null) - { - return true; - } - return false; + return GenerateCalParamByteArray(mVectorOffset, mSensitivityMatrix, mAlignmentMatrix); } public byte[] GenerateCalParamByteArray(double[,] offsetVector, double[,] sensitivityMatrix, double[,] alignmentMatrix) @@ -1258,30 +1233,116 @@ public void CalibByteDumpParse(byte[] calibBytesAll) var calibBytes = remainingBytes.Skip(12).Take(calibLength).ToArray(); - if (sensorId == 37) + //need to update this to parse correct sensor id, sensor range for correct sensitivity + if (sensorId == SensorLNAccel.SENSOR_ID) { - RetrieveKinematicCalibrationParametersFromPacket(remainingBytes, (byte)PacketTypeShimmer2.ACCEL_CALIBRATION_RESPONSE); - //lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorLNAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + if (LNAccelRange == 0) + { + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV; + } + else if (LNAccelRange == 1) + { + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV; + } + else if (LNAccelRange == 2) + { + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV; + } + else if (LNAccelRange == 3) + { + SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV; + } + + AlignmentMatrixAccel = SensorLNAccel.AlignmentMatrixAccel; + OffsetVectorAccel = SensorLNAccel.OffsetVectorAccel; } - else if (sensorId == 30 || sensorId == 38) + else if (sensorId == SensorGyro.SENSOR_ID) { SensorGyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + if (GyroRange == 0) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3; + } + else if (GyroRange == 1) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3; + } + else if (GyroRange == 2) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3; + } + else if (GyroRange == 3) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3; + } + + AlignmentMatrixGyro = SensorGyro.AlignmentMatrixGyro; + OffsetVectorGyro = SensorGyro.OffsetVectorGyro; } - else if (sensorId == 31 || sensorId == 39) + else if (sensorId == SensorWRAccel.SENSOR_ID) { SensorWRAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + if (AccelRange == 0) + { + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12; + } + else if (AccelRange == 1) + { + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_4G_SHIMMER3R_LIS2DW12; + } + else if (AccelRange == 2) + { + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_8G_SHIMMER3R_LIS2DW12; + } + else if (AccelRange == 3) + { + SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_16G_SHIMMER3R_LIS2DW12; + } + AlignmentMatrixAccel2 = SensorWRAccel.AlignmentMatrixAccel2; + OffsetVectorAccel2 = SensorWRAccel.OffsetVectorAccel2; } - else if (sensorId == 32 || sensorId == 42) + else if (sensorId == SensorMag.SENSOR_ID) { SensorMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + if (GetMagRange() == 0) + { + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; + } + else if (GetMagRange() == 1) + { + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_8GA_SHIMMER3R_LIS3MDL; + } + else if (GetMagRange() == 2) + { + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_12GA_SHIMMER3R_LIS3MDL; + } + else if (GetMagRange() == 3) + { + SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_16GA_SHIMMER3R_LIS3MDL; + } + AlignmentMatrixMag = SensorMag.AlignmentMatrixMag; + OffsetVectorMag = SensorMag.OffsetVectorMag; } - else if (sensorId == 33 || sensorId == 40) + else if (sensorId == SensorHighGAccel.SENSOR_ID) { SensorHighGAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + SensitivityMatrixAltAccel = SensorHighGAccel.SensitivityMatrixAltAccel; + AlignmentMatrixAltAccel = SensorHighGAccel.AlignmentMatrixAltAccel; + OffsetVectorAltAccel = SensorHighGAccel.OffsetVectorAltAccel; } - else if (sensorId == 34 || sensorId == 41) + else if (sensorId == SensorWRMag.SENSOR_ID) { SensorWRMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + + AlignmentMatrixMag2 = SensorWRMag.AlignmentMatrixMag2; + OffsetVectorMag2 = SensorWRMag.OffsetVectorMag2; + SensitivityMatrixMag2 = SensorWRMag.SensitivityMatrixMag2; } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index 1efa2a1e..525834b7 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -80,10 +80,15 @@ public void Test003_ConnectandTestCalibParamRead() } byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); - System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); //need to check this, calibration not updating the byte + System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); + mDevice.CalibByteDumpParse(deviceCalBytes); + + System.Console.WriteLine("done ..."); + + mDevice.WriteLNAccelRange(3); + deviceCalBytes = mDevice.CalibByteDumpGenerate(); + System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); mDevice.CalibByteDumpParse(deviceCalBytes); - mDevice.WriteAccelRange(2); - mDevice.WriteSensors((int)ShimmerBluetooth.SensorBitmapShimmer3.SENSOR_A_ACCEL); System.Console.WriteLine("done ..."); From f930a2d41ac607a6af18872c5ffdc45dc2eb0aed Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Wed, 22 Jan 2025 16:04:47 +0800 Subject: [PATCH 06/20] update the gyro sensitivity matrix for shimmer3r sensor --- ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 8 ++++---- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 16 ++++++++++++---- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 07f5db9c..9bc60c11 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -928,12 +928,12 @@ protected void InitializeSensors() { SensorLNAccel = new LNAccel(HardwareVersion); mMapOfSensorClasses.Add(SensorLNAccel); - SensorWRAccel = new WRAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorWRAccel); - SensorHighGAccel = new HighGAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorHighGAccel); SensorGyro = new GyroSensor(HardwareVersion); mMapOfSensorClasses.Add(SensorGyro); + SensorHighGAccel = new HighGAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorHighGAccel); + SensorWRAccel = new WRAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRAccel); SensorMag = new MagSensor(HardwareVersion); mMapOfSensorClasses.Add(SensorMag); SensorWRMag = new WRMag(HardwareVersion); diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 00b7ff5b..61e941b0 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -1264,19 +1264,27 @@ public void CalibByteDumpParse(byte[] calibBytesAll) if (GyroRange == 0) { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV; } else if (GyroRange == 1) { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3R_LSM6DSV; } else if (GyroRange == 2) { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3R_LSM6DSV; } else if (GyroRange == 3) { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3R_LSM6DSV; + } + else if (GyroRange == 4) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3R_LSM6DSV; + } + else if (GyroRange == 5) + { + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_4000DPS_SHIMMER3R_LSM6DSV; } AlignmentMatrixGyro = SensorGyro.AlignmentMatrixGyro; From 5fe670d595fea3fd6f68a49c09fed167bcd13bf0 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Thu, 23 Jan 2025 13:25:19 +0800 Subject: [PATCH 07/20] updates the calibbyte dump that being parsed --- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 61e941b0..0b46df92 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -1236,7 +1236,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) //need to update this to parse correct sensor id, sensor range for correct sensitivity if (sensorId == SensorLNAccel.SENSOR_ID) { - SensorLNAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorLNAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); if (LNAccelRange == 0) { @@ -1260,7 +1260,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == SensorGyro.SENSOR_ID) { - SensorGyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorGyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); if (GyroRange == 0) { @@ -1292,7 +1292,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == SensorWRAccel.SENSOR_ID) { - SensorWRAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorWRAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); if (AccelRange == 0) { @@ -1315,7 +1315,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == SensorMag.SENSOR_ID) { - SensorMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); if (GetMagRange() == 0) { @@ -1338,7 +1338,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == SensorHighGAccel.SENSOR_ID) { - SensorHighGAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorHighGAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); SensitivityMatrixAltAccel = SensorHighGAccel.SensitivityMatrixAltAccel; AlignmentMatrixAltAccel = SensorHighGAccel.AlignmentMatrixAltAccel; @@ -1346,7 +1346,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } else if (sensorId == SensorWRMag.SENSOR_ID) { - SensorWRMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(remainingBytes); + SensorWRMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); AlignmentMatrixMag2 = SensorWRMag.AlignmentMatrixMag2; OffsetVectorMag2 = SensorWRMag.OffsetVectorMag2; From 6c212b6703cbbda457383337c6321254122e73c4 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Thu, 23 Jan 2025 15:03:37 +0800 Subject: [PATCH 08/20] updating sensor class to include respective readonly sensor id --- ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs | 12 +++++++----- ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 7 ++++--- ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 10 ++++++---- ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs | 10 ++++++---- ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs | 10 ++++++---- ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 7 ++++--- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 2 ++ 7 files changed, 35 insertions(+), 23 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs index 2ab4c1fa..d55cb4c7 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs @@ -10,6 +10,8 @@ public class GyroSensor : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_MPU9X50_GYRO = 30; + public readonly int SHIMMER_LSM6DSV_GYRO = 38; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixGyro = new double[3, 3]; public double[,] SensitivityMatrixGyro = new double[3, 3]; @@ -19,17 +21,17 @@ public class GyroSensor : AbstractSensor public GyroSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 38; - + SENSOR_ID = SHIMMER_LSM6DSV_GYRO; + SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV; AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV; OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV; } else { - SENSOR_ID = 30; + SENSOR_ID = SHIMMER_MPU9X50_GYRO; SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3; AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3; @@ -39,7 +41,7 @@ public GyroSensor(int hardwareVersion) public Dictionary> GetCalibDetails() { - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsGyro = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index 707921f0..47c960b9 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -10,6 +10,7 @@ public class HighGAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_ADXL371_ACCEL_HIGHG = 40; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixAltAccel = new double[3, 3]; public double[,] SensitivityMatrixAltAccel = new double[3, 3]; @@ -19,9 +20,9 @@ public class HighGAccel : AbstractSensor public HighGAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 40; + SENSOR_ID = SHIMMER_ADXL371_ACCEL_HIGHG; SensitivityMatrixAltAccel = SENSITIVITY_MATRIX_HIGH_G_ACCEL_200G_SHIMMER3R_ADXL371; AlignmentMatrixAltAccel = ALIGNMENT_MATRIX_HIGH_G_ACCEL_SHIMMER3R_ADXL371; @@ -31,7 +32,7 @@ public HighGAccel(int hardwareVersion) public Dictionary> GetCalibDetails() { - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsAltAccel = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index d276f3e3..0625b411 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -11,6 +11,8 @@ public class LNAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_ANALOG_ACCEL = 2; + public readonly int SHIMMER_LSM6DSV_ACCEL_LN = 37; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; @@ -20,9 +22,9 @@ public class LNAccel : AbstractSensor public LNAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 37; + SENSOR_ID = SHIMMER_LSM6DSV_ACCEL_LN; SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV; AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV; @@ -30,7 +32,7 @@ public LNAccel(int hardwareVersion) } else { - SENSOR_ID = 2; + SENSOR_ID = SHIMMER_ANALOG_ACCEL; SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; @@ -40,7 +42,7 @@ public LNAccel(int hardwareVersion) public Dictionary> GetCalibDetails() { - if(ShimmerHardwareVersion == 10) + if(ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsAccelLN = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs index ce7ecbb5..e15480bd 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs @@ -10,6 +10,8 @@ public class MagSensor : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_LSM303_MAG = 32; + public readonly int SHIMMER_LIS2MDL_MAG = 42; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixMag = new double[3, 3]; public double[,] SensitivityMatrixMag = new double[3, 3]; @@ -19,9 +21,9 @@ public class MagSensor : AbstractSensor public MagSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 42; + SENSOR_ID = SHIMMER_LIS2MDL_MAG; SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL; @@ -29,7 +31,7 @@ public MagSensor(int hardwareVersion) } else { - SENSOR_ID = 32; + SENSOR_ID = SHIMMER_LSM303_MAG; SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3_LSM303AH; AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3_LSM303AH; @@ -39,7 +41,7 @@ public MagSensor(int hardwareVersion) public Dictionary> GetCalibDetails() { - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsMag = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs index 00e11806..d8877ef8 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs @@ -10,6 +10,8 @@ public class WRAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_LSM303_ACCEL = 31; + public readonly int SHIMMER_LIS2DW12_ACCEL_WR = 39; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixAccel2 = new double[3, 3]; public double[,] SensitivityMatrixAccel2 = new double[3, 3]; @@ -19,9 +21,9 @@ public class WRAccel : AbstractSensor public WRAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 39; + SENSOR_ID = SHIMMER_LIS2DW12_ACCEL_WR; SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12; AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12; @@ -29,7 +31,7 @@ public WRAccel(int hardwareVersion) } else { - SENSOR_ID = 31; + SENSOR_ID = SHIMMER_LSM303_ACCEL; SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3_LSM303AH; AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH; @@ -39,7 +41,7 @@ public WRAccel(int hardwareVersion) public Dictionary> GetCalibDetails() { - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsAccel2 = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index 8e4ed1fd..859bd2ee 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -10,6 +10,7 @@ public class WRMag : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int SHIMMER_LIS3MDL_MAG = 41; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixMag2 = new double[3, 3]; public double[,] SensitivityMatrixMag2 = new double[3, 3]; @@ -19,9 +20,9 @@ public class WRMag : AbstractSensor public WRMag(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - SENSOR_ID = 41; + SENSOR_ID = SHIMMER_LIS3MDL_MAG; SensitivityMatrixMag2 = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3R_LIS2MDL; AlignmentMatrixMag2 = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS2MDL; OffsetVectorMag2 = OFFSET_VECTOR_MAG_SHIMMER3R_LIS2MDL; @@ -30,7 +31,7 @@ public WRMag(int hardwareVersion) public Dictionary> GetCalibDetails() { - if (ShimmerHardwareVersion == 10) + if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { calibDetailsMag = new Dictionary>() { diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 0b46df92..b0b05df8 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -1310,6 +1310,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) { SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_16G_SHIMMER3R_LIS2DW12; } + AlignmentMatrixAccel2 = SensorWRAccel.AlignmentMatrixAccel2; OffsetVectorAccel2 = SensorWRAccel.OffsetVectorAccel2; } @@ -1333,6 +1334,7 @@ public void CalibByteDumpParse(byte[] calibBytesAll) { SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_16GA_SHIMMER3R_LIS3MDL; } + AlignmentMatrixMag = SensorMag.AlignmentMatrixMag; OffsetVectorMag = SensorMag.OffsetVectorMag; } From 763783f06f95a3caa2c1fede3bb9498f82b3ac96 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Fri, 24 Jan 2025 17:02:05 +0800 Subject: [PATCH 09/20] updates on s3r sensor unit test --- ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 4 +- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 33 +- .../ShimmerLogAndStreamS3Simulator.cs | 14 +- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 505 +++++++++++------- 4 files changed, 333 insertions(+), 223 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 95b9200c..17a24b98 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -169,8 +169,8 @@ public abstract class ShimmerBluetooth : ShimmerDevice public double[,] SensitivityMatrixAccel2 = new double[3, 3] { { 38, 0, 0 }, { 0, 38, 0 }, { 0, 0, 38 } }; public double[,] OffsetVectorAccel2 = new double[3, 1] { { 2048 }, { 2048 }, { 2048 } }; public double[,] AlignmentMatrixAltAccel = new double[3, 3] { { 0, 1, 0 }, { 1, 0, 0 }, { 0, 0, -1 } }; - public double[,] SensitivityMatrixAltAccel = new double[3, 3] { { 16, 0, 0 }, { 0, 16, 0 }, { 0, 0, 16 } }; - public double[,] OffsetVectorAltAccel = new double[3, 1] { { 0 }, { 0 }, { 0 } }; + public double[,] SensitivityMatrixAltAccel = new double[3, 3] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; + public double[,] OffsetVectorAltAccel = new double[3, 1] { { 10 }, { 10 }, { 10 } }; //Default Values for Magnetometer Calibration diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index b0b05df8..07f05039 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -215,6 +215,8 @@ public static double GetScaleFactor(CalibrationScaleFactor factor) private double[,] mSensitivityMatrix; private double[,] mVectorOffset; + public Dictionary>> mapOfSensorCalibration = new Dictionary>>(); + // btsd changes private int trialConfig; private int interval; @@ -1032,9 +1034,38 @@ public void ReadCalibDump() */ } + public object GetSensorDetails(int sensorId) + { + if(mMapOfSensorClasses != null) + { + foreach (AbstractSensor sensorClass in mMapOfSensorClasses) + { + if(sensorClass.SENSOR_ID == sensorId) + { + return sensorClass; + } + } + } + return null; + } + + public object GetSensorCalibDetails(int sensorId) + { + if (mMapOfSensorClasses != null) + { + foreach (AbstractSensor sensorClass in mMapOfSensorClasses) + { + if (sensorClass.SENSOR_ID == sensorId) + { + return sensorClass; + } + } + } + return null; + } + public Dictionary>> GetMapOfSensorCalibrationAll() { - Dictionary>> mapOfSensorCalibration = new Dictionary>>(); foreach (AbstractSensor sensorClass in mMapOfSensorClasses) { diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs index 90901934..68b7789b 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs @@ -188,19 +188,7 @@ protected override void WriteBytes(byte[] buffer, int index, int length) mBuffer.Add((byte)0x9B); } } - else if (buffer[0] == (byte)InstructionsGet.GetBmp390CalibrationCoefficientsCommand) - { - isGetBmp390CalibrationCoefficientsCommand = true; - mBuffer.Add((byte)0xff); - mBuffer.Add((byte)0x9f); - byte[] bytes = UtilShimmer.HexStringToByteArray("7A6A0D6632007F9016D7D00BBC1B2AFFF9FF8C3CF8C67017"); - foreach (byte byteValue in bytes) - { - mBuffer.Add(byteValue); - } - mBuffer.Add((byte)0xDF); - } - else if (buffer[0] == (byte)InstructionsGet.Bmp390CalibrationCoefficientsResponse) + else if (buffer[0] == (byte)PacketTypeShimmer3RSDBT.GET_PRESSURE_CALIBRATION_COEFFICIENTS_COMMAND) { isGetPressureCalibrationCoefficientsCommand = true; mBuffer.Add((byte)0xff); diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index 413bf1a5..d184499a 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -11,6 +11,7 @@ using System.Threading; using System.Threading.Tasks; using static ShimmerAPI.ShimmerBluetooth; +using System.Runtime.Serialization; namespace ShimmerBluetoothTests { @@ -30,104 +31,94 @@ public void SetUp() [TestMethod] public void Test001_testConnectandDisconnect() { - //Comment out test as it is not completed - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - // if (!mDevice.GetFirmwareVersionFullName().Equals("LogAndStream 0.0.1")) - // { - // Assert.Fail(); - // } - - // if (!mDevice.GetShimmerVersion().Equals(10)) //Shimmer3R - // { - // Assert.Fail(); - // } - - // if (!mDevice.isGetPressureCalibrationCoefficientsCommand) - // { - // Assert.Fail(); - // } - - // if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) - // { - // Assert.Fail(); - // } - - // foreach (byte b in mDevice.ListofSensorChannels) - // { - // Debug.WriteLine(b + " ; "); - // } - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + if (!mDevice.GetFirmwareVersionFullName().Equals("LogAndStream 0.0.1")) + { + Assert.Fail(); + } + + if (!mDevice.GetShimmerVersion().Equals(10)) + { + Assert.Fail(); + } + + if (!mDevice.isGetPressureCalibrationCoefficientsCommand) + { + Assert.Fail(); + } + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } [TestMethod] public void Test002_ConnectandTestBMP390() { - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - // try - // { - // mDevice.CalculateBMP390PressureCalibrationCoefficientsResponse(mDevice.GetPressureResoTest()); - - // string[] sensorDataType = { "u24" }; - - // byte[] sensorDataP2 = { 0x00, 0x17, 0x64 }; - // byte[] sensorDataT2 = { 0x00, 0xCF, 0x7F }; - - // long[] uncalibResultP2 = ProgrammerUtilities.ParseData(sensorDataP2, sensorDataType); - // long[] uncalibResultT2 = ProgrammerUtilities.ParseData(sensorDataT2, sensorDataType); - // double[] bmpX80caldata2 = new double[2]; - // bmpX80caldata2 = mDevice.CalibratePressure390SensorData(uncalibResultP2[0], uncalibResultT2[0]); - // Bmp3QuantizedCalibData.TLin = bmpX80caldata2[1]; - - // //Assert.AreEqual(resultP2, 100912.81758676282); - // //Assert.AreEqual(resultT2, 23.26587201654911); - // Assert.AreEqual(Math.Round(bmpX80caldata2[0], 4), 100912.8176); - // Assert.AreEqual(Math.Round(bmpX80caldata2[1], 4), 23.2659); - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + try + { + mDevice.CalculateBMP390PressureCalibrationCoefficientsResponse(mDevice.GetPressureResoTest()); + + string[] sensorDataType = { "u24" }; + + byte[] sensorDataP2 = { 0x00, 0x17, 0x64 }; + byte[] sensorDataT2 = { 0x00, 0xCF, 0x7F }; + + long[] uncalibResultP2 = ProgrammerUtilities.ParseData(sensorDataP2, sensorDataType); + long[] uncalibResultT2 = ProgrammerUtilities.ParseData(sensorDataT2, sensorDataType); + double[] bmpX80caldata2 = new double[2]; + bmpX80caldata2 = mDevice.CalibratePressure390SensorData(uncalibResultP2[0], uncalibResultT2[0]); + Bmp3QuantizedCalibData.TLin = bmpX80caldata2[1]; + + //Assert.AreEqual(resultP2, 100912.81758676282); + //Assert.AreEqual(resultT2, 23.26587201654911); + Assert.AreEqual(Math.Round(bmpX80caldata2[0], 4), 100912.8176); + Assert.AreEqual(Math.Round(bmpX80caldata2[1], 4), 23.2659); + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } [TestMethod] @@ -149,15 +140,21 @@ public void Test003_ConnectandTestCalibParamRead() System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); mDevice.CalibByteDumpParse(deviceCalBytes); - System.Console.WriteLine("done ..."); + foreach (int sensorId in mDevice.mapOfSensorCalibration.Keys) + { + object sensorDetails = mDevice.GetSensorDetails(sensorId); + Assert.IsNotNull(sensorDetails); - mDevice.WriteLNAccelRange(3); - deviceCalBytes = mDevice.CalibByteDumpGenerate(); - System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); - mDevice.CalibByteDumpParse(deviceCalBytes); + foreach (var returnValues in mDevice.mapOfSensorCalibration) + { + if (returnValues.Key == sensorId && returnValues.Value != null) + { + Assert.IsNotNull(returnValues.Value); + } + } + + } - System.Console.WriteLine("done ..."); - } catch (Exception ex) { @@ -185,11 +182,10 @@ public void Test004_ConnectandTestDefaultLNAccelAndGyroCalibParam() { Assert.Fail(); } + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); - //mDevice.ReadCalibrationParameters("All"); - //mDevice.RetrieveKinematicCalibrationParametersFromPacket(); - mDevice.WriteAccelRange(0); - mDevice.WriteSensors((int)ShimmerBluetooth.SensorBitmapShimmer3.SENSOR_A_ACCEL); + mDevice.WriteLNAccelRange(0); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] lnAccelOffset = mDevice.OffsetVectorAccel; Assert.IsTrue(lnAccelOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); @@ -199,36 +195,57 @@ public void Test004_ConnectandTestDefaultLNAccelAndGyroCalibParam() double[,] lnAccelSensitivity0 = mDevice.SensitivityMatrixAccel; Assert.IsTrue(lnAccelSensitivity0.Cast().SequenceEqual(new double[,] { { 1672, 0, 0 }, { 0, 1672, 0 }, { 0, 0, 1672 } }.Cast())); - mDevice.WriteAccelRange(1); + mDevice.WriteLNAccelRange(1); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] lnAccelSensitivity1 = mDevice.SensitivityMatrixAccel; Assert.IsTrue(lnAccelSensitivity1.Cast().SequenceEqual(new double[,] { { 836, 0, 0 }, { 0, 836, 0 }, { 0, 0, 836 } }.Cast())); - mDevice.WriteAccelRange(2); + mDevice.WriteLNAccelRange(2); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] lnAccelSensitivity2 = mDevice.SensitivityMatrixAccel; Assert.IsTrue(lnAccelSensitivity2.Cast().SequenceEqual(new double[,] { { 418, 0, 0 }, { 0, 418, 0 }, { 0, 0, 418 } }.Cast())); - mDevice.WriteAccelRange(3); + mDevice.WriteLNAccelRange(3); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] lnAccelSensitivity3 = mDevice.SensitivityMatrixAccel; Assert.IsTrue(lnAccelSensitivity3.Cast().SequenceEqual(new double[,] { { 209, 0, 0 }, { 0, 209, 0 }, { 0, 0, 209 } }.Cast())); mDevice.WriteGyroRange(0); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] gyroOffset = mDevice.OffsetVectorGyro; - Assert.IsTrue(gyroOffset.Cast().SequenceEqual(new double[,] { { 1843 }, { 1843 }, { 1843 } }.Cast())); + Assert.IsTrue(gyroOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); double[,] gyroAlignment = mDevice.AlignmentMatrixGyro; - Assert.IsTrue(gyroAlignment.Cast().SequenceEqual(new double[,] { { 0, -1, 0 }, { -1, 0, 0 }, { 0, 0, -1 } }.Cast())); + Assert.IsTrue(gyroAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, -1 } }.Cast())); double[,] gyroSensitivity0 = mDevice.SensitivityMatrixGyro; - Assert.IsTrue(gyroSensitivity0.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); + Assert.IsTrue(gyroSensitivity0.Cast().SequenceEqual(new double[,] { { 229, 0, 0 }, { 0, 229, 0 }, { 0, 0, 229 } }.Cast())); mDevice.WriteGyroRange(1); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] gyroSensitivity1 = mDevice.SensitivityMatrixGyro; - Assert.IsTrue(gyroSensitivity1.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); + Assert.IsTrue(gyroSensitivity1.Cast().SequenceEqual(new double[,] { { 114, 0, 0 }, { 0, 114, 0 }, { 0, 0, 114 } }.Cast())); mDevice.WriteGyroRange(2); + mDevice.CalibByteDumpParse(deviceCalBytes); double[,] gyroSensitivity2 = mDevice.SensitivityMatrixGyro; - Assert.IsTrue(gyroSensitivity2.Cast().SequenceEqual(new double[,] { { 2.73, 0, 0 }, { 0, 2.73, 0 }, { 0, 0, 2.73 } }.Cast())); + Assert.IsTrue(gyroSensitivity2.Cast().SequenceEqual(new double[,] { { 57, 0, 0 }, { 0, 57, 0 }, { 0, 0, 57 } }.Cast())); + + mDevice.WriteGyroRange(3); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] gyroSensitivity3 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity3.Cast().SequenceEqual(new double[,] { { 29, 0, 0 }, { 0, 29, 0 }, { 0, 0, 29 } }.Cast())); + + mDevice.WriteGyroRange(4); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] gyroSensitivity4 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity4.Cast().SequenceEqual(new double[,] { { 14, 0, 0 }, { 0, 14, 0 }, { 0, 0, 14 } }.Cast())); + + mDevice.WriteGyroRange(5); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] gyroSensitivity5 = mDevice.SensitivityMatrixGyro; + Assert.IsTrue(gyroSensitivity5.Cast().SequenceEqual(new double[,] { { 7, 0, 0 }, { 0, 7, 0 }, { 0, 0, 7 } }.Cast())); } catch (Exception ex) @@ -241,117 +258,191 @@ public void Test004_ConnectandTestDefaultLNAccelAndGyroCalibParam() Assert.Fail("mDevice is null"); } } - + [TestMethod] public void Test005_ConnectandTestDefaultWRAccelCalibParam() { - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + mDevice.CalibByteDumpParse(deviceCalBytes); + + double[,] wrAccelOffset = mDevice.OffsetVectorAccel2; + Assert.IsTrue(wrAccelOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); + double[,] wrAccelAlignment = mDevice.AlignmentMatrixAccel2; + Assert.IsTrue(wrAccelAlignment.Cast().SequenceEqual(new double[,] { { 0, -1, 0 }, { -1, 0, 0 }, { 0, 0, -1 } }.Cast())); + + mDevice.WriteAccelRange(0); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] wrAccelSensitivity0 = mDevice.SensitivityMatrixAccel2; + Assert.IsTrue(wrAccelSensitivity0.Cast().SequenceEqual(new double[,] { { 1671, 0, 0 }, { 0, 1671, 0 }, { 0, 0, 1671 } }.Cast())); + + mDevice.WriteAccelRange(1); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] wrAccelSensitivity1 = mDevice.SensitivityMatrixAccel2; + Assert.IsTrue(wrAccelSensitivity1.Cast().SequenceEqual(new double[,] { { 836, 0, 0 }, { 0, 836, 0 }, { 0, 0, 836 } }.Cast())); + + mDevice.WriteAccelRange(2); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] wrAccelSensitivity2 = mDevice.SensitivityMatrixAccel2; + Assert.IsTrue(wrAccelSensitivity2.Cast().SequenceEqual(new double[,] { { 418, 0, 0 }, { 0, 418, 0 }, { 0, 0, 418 } }.Cast())); + + mDevice.WriteAccelRange(3); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] wrAccelSensitivity3 = mDevice.SensitivityMatrixAccel2; + Assert.IsTrue(wrAccelSensitivity3.Cast().SequenceEqual(new double[,] { { 209, 0, 0 }, { 0, 209, 0 }, { 0, 0, 209 } }.Cast())); + + + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } - + [TestMethod] public void Test006_ConnectandTestDefaultMagCalibParam() { - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + mDevice.CalibByteDumpParse(deviceCalBytes); + + double[,] magOffset = mDevice.OffsetVectorMag; + Assert.IsTrue(magOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); + double[,] magAlignment = mDevice.AlignmentMatrixMag; + Assert.IsTrue(magAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, 0, -1 }, { 0, -1, 0 } }.Cast())); + + mDevice.WriteMagRange(0); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] magSensitivity0 = mDevice.SensitivityMatrixMag; + Assert.IsTrue(magSensitivity0.Cast().SequenceEqual(new double[,] { { 6842, 0, 0 }, { 0, 6842, 0 }, { 0, 0, 6842 } }.Cast())); + + mDevice.WriteMagRange(1); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] magSensitivity1 = mDevice.SensitivityMatrixMag; + Assert.IsTrue(magSensitivity1.Cast().SequenceEqual(new double[,] { { 3421, 0, 0 }, { 0, 3421, 0 }, { 0, 0, 3421 } }.Cast())); + + mDevice.WriteMagRange(2); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] magSensitivity2 = mDevice.SensitivityMatrixMag; + Assert.IsTrue(magSensitivity2.Cast().SequenceEqual(new double[,] { { 2281, 0, 0 }, { 0, 2281, 0 }, { 0, 0, 2281 } }.Cast())); + + mDevice.WriteMagRange(3); + mDevice.CalibByteDumpParse(deviceCalBytes); + double[,] magSensitivity3 = mDevice.SensitivityMatrixMag; + Assert.IsTrue(magSensitivity3.Cast().SequenceEqual(new double[,] { { 1711, 0, 0 }, { 0, 1711, 0 }, { 0, 0, 1711 } }.Cast())); + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } - + [TestMethod] public void Test007_ConnectandTestDefaultHighGAccelCalibParam() { - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + mDevice.CalibByteDumpParse(deviceCalBytes); + + double[,] altAccelOffset = mDevice.OffsetVectorAltAccel; + Assert.IsTrue(altAccelOffset.Cast().SequenceEqual(new double[,] { { 10 }, { 10 }, { 10 } }.Cast())); + double[,] altAccelAlignment = mDevice.AlignmentMatrixAltAccel; + Assert.IsTrue(altAccelAlignment.Cast().SequenceEqual(new double[,] { { 0, 1, 0 }, { 1, 0, 0 }, { 0, 0, -1 } }.Cast())); + double[,] altAccelSensitivity0 = mDevice.SensitivityMatrixAltAccel; + Assert.IsTrue(altAccelSensitivity0.Cast().SequenceEqual(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }.Cast())); + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } [TestMethod] public void Test008_ConnectandTestDefaultWRMagCalibParam() { - //if (mDevice != null) - //{ - // try - // { - // mDevice.StartConnectThread(); - // mDevice.SetIsNewBMPSupported(true); - // Thread.Sleep(30000); - // if (!mDevice.IsConnected()) - // { - // Assert.Fail(); - // } - - - // } - // catch (Exception ex) - // { - // Assert.Fail($"Test aborted due to exception: {ex.Message}"); - // } - //} - //else - //{ - // Assert.Fail("mDevice is null"); - //} + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + mDevice.SetIsNewBMPSupported(true); + Thread.Sleep(30000); + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + mDevice.CalibByteDumpParse(deviceCalBytes); + + double[,] wrMagOffset = mDevice.OffsetVectorMag2; + Assert.IsTrue(wrMagOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); + double[,] wrMagAlignment = mDevice.AlignmentMatrixMag2; + Assert.IsTrue(wrMagAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, -1, 0 }, { 0, 0, -1 } }.Cast())); + double[,] wrMagSensitivity0 = mDevice.SensitivityMatrixMag2; + Assert.IsTrue(wrMagSensitivity0.Cast().SequenceEqual(new double[,] { { 667, 0, 0 }, { 0, 667, 0 }, { 0, 0, 667 } }.Cast())); + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } } } } \ No newline at end of file From c17365cd5a147c148eac3f2f7012d8b2eafffe47 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 28 Jan 2025 13:13:50 +0800 Subject: [PATCH 10/20] update on mag alignment matrix for unit test --- .../ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index d184499a..a4ba088c 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -337,7 +337,7 @@ public void Test006_ConnectandTestDefaultMagCalibParam() double[,] magOffset = mDevice.OffsetVectorMag; Assert.IsTrue(magOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); double[,] magAlignment = mDevice.AlignmentMatrixMag; - Assert.IsTrue(magAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, 0, -1 }, { 0, -1, 0 } }.Cast())); + Assert.IsTrue(magAlignment.Cast().SequenceEqual(new double[,] { { 1, 0, 0 }, { 0, -1, 0 }, { 0, 0, -1 } }.Cast())); mDevice.WriteMagRange(0); mDevice.CalibByteDumpParse(deviceCalBytes); @@ -407,7 +407,7 @@ public void Test007_ConnectandTestDefaultHighGAccelCalibParam() Assert.Fail("mDevice is null"); } } - + [TestMethod] public void Test008_ConnectandTestDefaultWRMagCalibParam() { From 728329bb1b8da4808409b85919cdd63f425bdaf9 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 28 Jan 2025 14:40:20 +0800 Subject: [PATCH 11/20] fixing shimmer3 unit test failing due to null class for high g, wr mag --- ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 5 +++ ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 5 +++ ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs | 39 +++++++------------ .../ShimmerLogAndStreamS3SimulatorTest.cs | 27 ------------- 4 files changed, 23 insertions(+), 53 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index 47c960b9..5629eaef 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -10,6 +10,7 @@ public class HighGAccel : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int ALT_ACCEL = 33; public readonly int SHIMMER_ADXL371_ACCEL_HIGHG = 40; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixAltAccel = new double[3, 3]; @@ -28,6 +29,10 @@ public HighGAccel(int hardwareVersion) AlignmentMatrixAltAccel = ALIGNMENT_MATRIX_HIGH_G_ACCEL_SHIMMER3R_ADXL371; OffsetVectorAltAccel = OFFSET_VECTOR_ACCEL_HIGH_G_SHIMMER3R_ADXL371; } + else + { + SENSOR_ID = ALT_ACCEL; + } } public Dictionary> GetCalibDetails() diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index 859bd2ee..922cd27b 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -10,6 +10,7 @@ public class WRMag : AbstractSensor { protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; + public readonly int ALT_MAG = 34; public readonly int SHIMMER_LIS3MDL_MAG = 41; public int SENSOR_ID { get; private set; } public double[,] AlignmentMatrixMag2 = new double[3, 3]; @@ -27,6 +28,10 @@ public WRMag(int hardwareVersion) AlignmentMatrixMag2 = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS2MDL; OffsetVectorMag2 = OFFSET_VECTOR_MAG_SHIMMER3R_LIS2MDL; } + else + { + SENSOR_ID = ALT_MAG; + } } public Dictionary> GetCalibDetails() diff --git a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs index 73ca5efc..707722de 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerBluetooth.cs @@ -927,32 +927,18 @@ public void StartConnectThread() protected void InitializeSensors() { - if (HardwareVersion == (int)ShimmerVersion.SHIMMER3R) - { - SensorLNAccel = new LNAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorLNAccel); - SensorGyro = new GyroSensor(HardwareVersion); - mMapOfSensorClasses.Add(SensorGyro); - SensorHighGAccel = new HighGAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorHighGAccel); - SensorWRAccel = new WRAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorWRAccel); - SensorMag = new MagSensor(HardwareVersion); - mMapOfSensorClasses.Add(SensorMag); - SensorWRMag = new WRMag(HardwareVersion); - mMapOfSensorClasses.Add(SensorWRMag); - } - else - { - SensorLNAccel = new LNAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorLNAccel); - SensorWRAccel = new WRAccel(HardwareVersion); - mMapOfSensorClasses.Add(SensorWRAccel); - SensorGyro = new GyroSensor(HardwareVersion); - mMapOfSensorClasses.Add(SensorGyro); - SensorMag = new MagSensor(HardwareVersion); - mMapOfSensorClasses.Add(SensorMag); - } + SensorLNAccel = new LNAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorLNAccel); + SensorGyro = new GyroSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorGyro); + SensorHighGAccel = new HighGAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorHighGAccel); + SensorWRAccel = new WRAccel(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRAccel); + SensorMag = new MagSensor(HardwareVersion); + mMapOfSensorClasses.Add(SensorMag); + SensorWRMag = new WRMag(HardwareVersion); + mMapOfSensorClasses.Add(SensorWRMag); } public void Connect() @@ -1008,6 +994,7 @@ public void Connect() if (GetFirmwareIdentifier() == FW_IDENTIFIER_LOGANDSTREAM) { //WriteBatteryFrequency(0); + InitializeSensors(); ReadExpansionBoard(); InitializeShimmer3SDBT(); } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index 69ccc5e8..4c5f1fb2 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -51,16 +51,6 @@ public void Test001_testConnectandDisconnect() Assert.Fail(); } - //if (!mDevice.isGetBmp390CalibrationCoefficientsCommand) - //{ - // Assert.Fail(); - //} - - //if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) - //{ - // Assert.Fail(); - //} - try { mDevice.Disconnect(); @@ -134,22 +124,5 @@ public void Test002_testConnectandDisconnect_NewBMPSupported() } } - public void ProcessCalibrationData() - { - byte[] calibDump = mDevice.GetCalibrationDump().ToArray(); - - if (calibDump == null || calibDump.Length < 2) - { - throw new ArgumentException("Invalid calibDump: must contain at least 2 bytes."); - } - - //mDevice.WriteAccelRange(0); - LNAccel lnAccel = new LNAccel((int)ShimmerBluetooth.ShimmerVersion.SHIMMER3); - lnAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDump); - //mDevice.WriteGyroRange(0); - //GyroSensor gyro = new GyroSensor(); - //gyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibDumpResponse.ToArray()); - } - } } From 2d5771cd558c8c14453676bd81b815dbf201d28f Mon Sep 17 00:00:00 2001 From: JongChern Date: Mon, 3 Feb 2025 14:03:24 +0800 Subject: [PATCH 12/20] Update LiteProtocolInstructionSet.cs --- ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs index 449899d4..8bb1a39c 100644 --- a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs +++ b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs @@ -374,10 +374,7 @@ public enum InstructionsGet { [pbr::OriginalName("GET_VBATT_COMMAND")] GetVbattCommand = 149, [pbr::OriginalName("GET_CALIB_DUMP_COMMAND")] GetCalibDumpCommand = 154, [pbr::OriginalName("GET_I2C_BATT_STATUS_COMMAND_VALUE")] GetI2CBattStatusCommandValue = 158, - [pbr::OriginalName("GET_BMP280_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp280CalibrationCoefficientsCommand = 159, - [pbr::OriginalName("GET_BMP390_PRESS_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp390CalibrationCoefficientsCommand = 166, - [pbr::OriginalName("BMP390_CALIBRATION_COEFFICIENTS_RESPONSE")] Bmp390CalibrationCoefficientsResponse = 165, - + [pbr::OriginalName("GET_BMP280_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp280CalibrationCoefficientsCommand = 159, } public enum InstructionsResponse { From 3ff485f8c8ba219908c2b4cc3829cde23a15b02c Mon Sep 17 00:00:00 2001 From: JongChern Date: Mon, 3 Feb 2025 14:04:31 +0800 Subject: [PATCH 13/20] Update LiteProtocolInstructionSet.cs --- ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs index 8bb1a39c..4f1d3227 100644 --- a/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs +++ b/ShimmerAPI/ShimmerAPI/LiteProtocolInstructionSet.cs @@ -374,7 +374,7 @@ public enum InstructionsGet { [pbr::OriginalName("GET_VBATT_COMMAND")] GetVbattCommand = 149, [pbr::OriginalName("GET_CALIB_DUMP_COMMAND")] GetCalibDumpCommand = 154, [pbr::OriginalName("GET_I2C_BATT_STATUS_COMMAND_VALUE")] GetI2CBattStatusCommandValue = 158, - [pbr::OriginalName("GET_BMP280_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp280CalibrationCoefficientsCommand = 159, + [pbr::OriginalName("GET_BMP280_CALIBRATION_COEFFICIENTS_COMMAND")] GetBmp280CalibrationCoefficientsCommand = 159, } public enum InstructionsResponse { From 38fdc43f5779bc59d4ac76cf0838ede7c692b9df Mon Sep 17 00:00:00 2001 From: JongChern Date: Mon, 3 Feb 2025 14:44:06 +0800 Subject: [PATCH 14/20] minor refactoring --- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 8 +- .../ShimmerLogAndStreamS3Simulator.cs | 140 ++++++++++++++++++ 2 files changed, 144 insertions(+), 4 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 56391cc0..e2e0e3c2 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -684,7 +684,7 @@ public override void StartStreaming() } // btsd changes - private void readInfoMem() + protected virtual void readInfoMem() { //string status_text = ""; //PChangeStatusLabel("Inquiring Shimmer Info"); // need to be in a UI thread for update @@ -736,12 +736,12 @@ private void readInfoMem() //worker.ReportProgress(40, status_text); if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - ReadCalibDump(); + //ReadCalibDump(); ReadCalibrationParameters("All"); } else { - ReadCalibDump(); + //ReadCalibDump(); ReadCalibrationParameters("All"); } @@ -1536,7 +1536,7 @@ protected byte[] ConvertSystemTimeToShimmerRwcDataBytes() // btsd changes - private void waitTilTimeOut() + protected void waitTilTimeOut() { int timeout = 150; while (timeout > 0) diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs index 68b7789b..984819e7 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs @@ -290,5 +290,145 @@ protected override void WriteBytes(byte[] buffer, int index, int length) } + protected override void readInfoMem() + { + //string status_text = ""; + //PChangeStatusLabel("Inquiring Shimmer Info"); // need to be in a UI thread for update + SetDataReceived(false); + + // btsd changes 2 + string status_text = "Acquiring Accelerometer Range..."; + CustomEventArgs newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring Accelerometer Range..."; + //worker.ReportProgress(15, status_text); + ReadAccelRange(); + + if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) + { + ReadLNAccelRange(); + } + + status_text = "Acquiring ADC Sampling Rate..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring ADC Sampling Rate..."; + //worker.ReportProgress(20, status_text); + ReadSamplingRate(); + + status_text = "Acquiring Magnetometer Range..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring Magnetometer Range..."; + //worker.ReportProgress(25, status_text); + ReadMagRange(); + + status_text = "Acquiring Gyro Range..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring Gyro Range..."; + //worker.ReportProgress(30, status_text); + ReadGyroRange(); + + status_text = "Acquiring Accel Sampling Rate..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring Accel Sampling Rate..."; + //worker.ReportProgress(35, status_text); + ReadAccelSamplingRate(); + + status_text = "Acquiring Calibration settings..."; + //PControlForm.status_text = "Acquiring Calibration settings..."; + //worker.ReportProgress(40, status_text); + if (HardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) + { + ReadCalibDump(); + //ReadCalibrationParameters("All"); + } + else + { + ReadCalibDump(); + //ReadCalibrationParameters("All"); + } + + status_text = "Acquiring EXG1 configure settings..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring EXG1 configure settings..."; + //worker.ReportProgress(45, status_text); + ReadEXGConfigurations(1); + + status_text = "Acquiring EXG2 configure settings..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring EXG2 configure settings..."; + //worker.ReportProgress(50, status_text); + ReadEXGConfigurations(2); + + //There is an inquiry below so no need for this + status_text = "Acquiring Internal Exp Power setting..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + ReadInternalExpPower(); + + status_text = "Acquiring trial configure settings..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring trial configure settings..."; + //worker.ReportProgress(55, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_TRIAL_CONFIG_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring center name..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring center name..."; + //worker.ReportProgress(60, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_CENTER_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring shimmer name..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring shimmer name..."; + //worker.ReportProgress(65, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_SHIMMERNAME_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring experiment ID..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring experiment ID..."; + //worker.ReportProgress(70, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_EXPID_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring Multi Shimmer settings..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring Multi Shimmer settings..."; + //worker.ReportProgress(75, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_MYID_COMMAND }, 0, 1); + waitTilTimeOut(); + + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_NSHIMMER_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring configure time..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring configure time..."; + //worker.ReportProgress(80, status_text); + WriteBytes(new byte[1] { (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.GET_CONFIGTIME_COMMAND }, 0, 1); + waitTilTimeOut(); + + status_text = "Acquiring general settings..."; + newEventArgs = new CustomEventArgs((int)ShimmerIdentifier.MSG_IDENTIFIER_NOTIFICATION_MESSAGE, (object)status_text); + OnNewEvent(newEventArgs); + //PControlForm.status_text = "Acquiring general settings..."; + //worker.ReportProgress(85, status_text); + Inquiry(); + } + } } From 5dfd38fa1243b14e383ee1b384339756fa65f45a Mon Sep 17 00:00:00 2001 From: JongChern Date: Mon, 3 Feb 2025 15:03:46 +0800 Subject: [PATCH 15/20] Update ShimmerLogAndStream.cs --- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 61 +++++++++++--------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index e2e0e3c2..14a533ee 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -888,11 +888,6 @@ public override void WriteShimmerName() byte[] CalibDump = new byte[128]; private Dictionary mCalibBytesDescriptions; private byte[] mCalibBytes; - private int mHardwareVersion; - private int mFirmwareIdentifier; - private int mFirmwareVersionMajor; - private int mFirmwareVersionMinor; - private int mFirmwareVersionInternal; [Obsolete("This method is unfinished and should not be used.")] public void WriteShimmer3Infomem(byte[] infoMem) @@ -1188,22 +1183,22 @@ public byte[] GenerateCalParamByteArray(double[,] offsetVector, double[,] sensit } - public byte[] GenerateVersionByteArrayNew() + protected byte[] GenerateVersionByteArrayNew() { byte[] byteArray = new byte[8]; int index = 0; - byteArray[index++] = (byte)(mHardwareVersion & 0xFF); - byteArray[index++] = (byte)((mHardwareVersion >> 8) & 0xFF); + byteArray[index++] = (byte)(HardwareVersion & 0xFF); + byteArray[index++] = (byte)((HardwareVersion >> 8) & 0xFF); - byteArray[index++] = (byte)(mFirmwareIdentifier & 0xFF); - byteArray[index++] = (byte)((mFirmwareIdentifier >> 8) & 0xFF); + byteArray[index++] = (byte)((int)FirmwareIdentifier & 0xFF); + byteArray[index++] = (byte)(((int)FirmwareIdentifier >> 8) & 0xFF); - byteArray[index++] = (byte)(mFirmwareVersionMajor & 0xFF); - byteArray[index++] = (byte)((mFirmwareVersionMajor >> 8) & 0xFF); + byteArray[index++] = (byte)(FirmwareMajor & 0xFF); + byteArray[index++] = (byte)((FirmwareMajor >> 8) & 0xFF); - byteArray[index++] = (byte)(mFirmwareVersionMinor & 0xFF); - byteArray[index++] = (byte)(mFirmwareVersionInternal & 0xFF); + byteArray[index++] = (byte)(FirmwareMinor & 0xFF); + byteArray[index++] = (byte)(FirmwareMinor & 0xFF); return byteArray; } @@ -1220,11 +1215,13 @@ public void CalibByteDumpParse(byte[] calibBytesAll) var packetLength = calibBytesAll.Take(2).ToArray(); var svoBytes = calibBytesAll.Skip(2).Take(8).ToArray(); - parseVersionByteArray(svoBytes); - mCalibBytesDescriptions[2] = $"HwID_LSB (" + mHardwareVersion + ")"; + //no need this functionality for the time being, as the info parsed should only be relevent to this method and not to any class variables + var(hwVersion, fwIden, fwMajor, fwMinor, fwInternal) = parseVersionByteArray(svoBytes); + + mCalibBytesDescriptions[2] = $"HwID_LSB (" + hwVersion + ")"; mCalibBytesDescriptions[3] = "HwID_MSB"; - mCalibBytesDescriptions[4] = $"FwID_LSB (" + mFirmwareIdentifier + ")"; + mCalibBytesDescriptions[4] = $"FwID_LSB (" + fwIden + ")"; mCalibBytesDescriptions[5] = "FwID_MSB"; mCalibBytesDescriptions[6] = "FWVerMjr_LSB"; mCalibBytesDescriptions[7] = "FWVerMjr_MSB"; @@ -1398,28 +1395,40 @@ public void CalibByteDumpParse(byte[] calibBytesAll) } } - public void parseVersionByteArray(byte[] byteArray) + + public (int,int,int,int,int) parseVersionByteArray(byte[] byteArray) { + int hardwareVersion; + int firmwareIdentifier; + int firmwareMajor; + int firmwareMinor; + int firmwareInternal; + if ((byteArray.Length == 7) || (byteArray.Length == 8)) { int index = 0; if (byteArray.Length == 7) { - mHardwareVersion = byteArray[index++] & 0xFF; + hardwareVersion = byteArray[index++] & 0xFF; } - else if (byteArray.Length == 8) + else // == 8 { - mHardwareVersion = (byteArray[index++] | (byteArray[index++] << 8)) & 0xFFFF; + hardwareVersion = (byteArray[index++] | (byteArray[index++] << 8)) & 0xFFFF; } - mFirmwareIdentifier = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; - mFirmwareVersionMajor = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; - mFirmwareVersionMinor = byteArray[index++] & 0xFF; - mFirmwareVersionInternal = byteArray[index++] & 0xFF; - + firmwareIdentifier = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; + firmwareMajor = ((byteArray[index++]) | (byteArray[index++] << 8)) & 0xFFFF; + firmwareMinor = byteArray[index++] & 0xFF; + firmwareInternal = byteArray[index++] & 0xFF; + return (hardwareVersion, firmwareIdentifier, firmwareMajor, firmwareMinor, firmwareInternal); + } + else + { + throw new Exception("Not enough bytes"); } } + public List GetCalibrationDump() { From bc8c8b5c06d96c2bb4989b44fcb258651232365e Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 4 Feb 2025 16:58:38 +0800 Subject: [PATCH 16/20] refactoring on sensor classes --- .../ShimmerAPI/Sensors/AbstractSensor.cs | 3 +- ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs | 52 +++----- ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 48 +++---- ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs | 56 +++------ ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs | 52 +++----- ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs | 51 +++----- ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 47 +++---- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 118 ++---------------- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 2 +- 9 files changed, 125 insertions(+), 304 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs index f2464022..aaca61c1 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/AbstractSensor.cs @@ -7,7 +7,8 @@ namespace ShimmerAPI.Sensors public interface AbstractSensor { int SENSOR_ID { get; } - Dictionary> GetCalibDetails(); + int ShimmerHardwareVersion { get; } + Dictionary> CalibDetails { get; } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs index d55cb4c7..dabd3b28 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/GyroSensor.cs @@ -8,42 +8,29 @@ namespace ShimmerAPI.Sensors { public class GyroSensor : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int SHIMMER_MPU9X50_GYRO = 30; public readonly int SHIMMER_LSM6DSV_GYRO = 38; public int SENSOR_ID { get; private set; } + public int ShimmerHardwareVersion { get; private set; } + public Dictionary> CalibDetails { get; private set; } + public double[,] AlignmentMatrixGyro = new double[3, 3]; public double[,] SensitivityMatrixGyro = new double[3, 3]; public double[,] OffsetVectorGyro = new double[3, 1]; - public Dictionary> calibDetailsGyro; public GyroSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_LSM6DSV_GYRO; - - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV; - AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3R_LSM6DSV; - OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3R_LSM6DSV; - } - else - { - SENSOR_ID = SHIMMER_MPU9X50_GYRO; - - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3; - AlignmentMatrixGyro = ALIGNMENT_MATRIX_GYRO_SHIMMER3; - OffsetVectorGyro = OFFSET_VECTOR_GYRO_SHIMMER3; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + public void CreateDefaultCalibParams() { if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - calibDetailsGyro = new Dictionary>() + SENSOR_ID = SHIMMER_LSM6DSV_GYRO; + CalibDetails = new Dictionary>() { { 0, @@ -103,7 +90,8 @@ public Dictionary> GetCalibDetails() } else { - calibDetailsGyro = new Dictionary>() + SENSOR_ID = SHIMMER_MPU9X50_GYRO; + CalibDetails = new Dictionary>() { { 0, @@ -143,23 +131,19 @@ public Dictionary> GetCalibDetails() } }; } - return calibDetailsGyro; - } - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) - { - - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == SENSOR_ID) + if (CalibDetails.TryGetValue(0, out var defaultCalib)) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixGyro, SensitivityMatrixGyro, OffsetVectorGyro) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("Gyro calibration parameters"); + AlignmentMatrixGyro = defaultCalib[0]; + SensitivityMatrixGyro = defaultCalib[1]; + OffsetVectorGyro = defaultCalib[2]; } + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + (AlignmentMatrixGyro, SensitivityMatrixGyro, OffsetVectorGyro) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("Gyro calibration parameters"); } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index 5629eaef..1c371bf3 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -8,38 +8,28 @@ namespace ShimmerAPI.Sensors { public class HighGAccel : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int ALT_ACCEL = 33; public readonly int SHIMMER_ADXL371_ACCEL_HIGHG = 40; public int SENSOR_ID { get; private set; } + public int ShimmerHardwareVersion { get; private set; } + public Dictionary> CalibDetails { get; private set; } public double[,] AlignmentMatrixAltAccel = new double[3, 3]; public double[,] SensitivityMatrixAltAccel = new double[3, 3]; public double[,] OffsetVectorAltAccel = new double[3, 1]; - public Dictionary> calibDetailsAltAccel; public HighGAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_ADXL371_ACCEL_HIGHG; - - SensitivityMatrixAltAccel = SENSITIVITY_MATRIX_HIGH_G_ACCEL_200G_SHIMMER3R_ADXL371; - AlignmentMatrixAltAccel = ALIGNMENT_MATRIX_HIGH_G_ACCEL_SHIMMER3R_ADXL371; - OffsetVectorAltAccel = OFFSET_VECTOR_ACCEL_HIGH_G_SHIMMER3R_ADXL371; - } - else - { - SENSOR_ID = ALT_ACCEL; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + public void CreateDefaultCalibParams() { if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - calibDetailsAltAccel = new Dictionary>() + SENSOR_ID = SHIMMER_ADXL371_ACCEL_HIGHG; + CalibDetails = new Dictionary>() { { 0, @@ -52,23 +42,23 @@ public Dictionary> GetCalibDetails() } }; } - return calibDetailsAltAccel; - } - - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) - { + else + { + SENSOR_ID = ALT_ACCEL; + } - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == SENSOR_ID) + if (CalibDetails.TryGetValue(0, out var defaultCalib)) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAltAccel, SensitivityMatrixAltAccel, OffsetVectorAltAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("High-G Accel calibration parameters"); + AlignmentMatrixAltAccel = defaultCalib[0]; + SensitivityMatrixAltAccel = defaultCalib[1]; + OffsetVectorAltAccel = defaultCalib[2]; } + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + (AlignmentMatrixAltAccel, SensitivityMatrixAltAccel, OffsetVectorAltAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("High-G Accel calibration parameters"); } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs index 0625b411..6c3a62e8 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/LNAccel.cs @@ -9,42 +9,29 @@ namespace ShimmerAPI.Sensors { public class LNAccel : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int SHIMMER_ANALOG_ACCEL = 2; public readonly int SHIMMER_LSM6DSV_ACCEL_LN = 37; public int SENSOR_ID { get; private set; } + public int ShimmerHardwareVersion { get; private set; } + public Dictionary> CalibDetails { get; private set; } + public double[,] AlignmentMatrixAccel = new double[3, 3]; public double[,] SensitivityMatrixAccel = new double[3, 3]; public double[,] OffsetVectorAccel = new double[3, 1]; - public Dictionary> calibDetailsAccelLN; public LNAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_LSM6DSV_ACCEL_LN; - - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV; - AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3R_LSM6DSV; - OffsetVectorAccel = OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3R_LSM6DSV; - } - else - { - SENSOR_ID = SHIMMER_ANALOG_ACCEL; - - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; - AlignmentMatrixAccel = ALIGNMENT_MATRIX_LOW_NOISE_ACCEL_SHIMMER3_KXTC9_2050; - OffsetVectorAccel = OFFSET_VECTOR_ACCEL_LOW_NOISE_SHIMMER3_KXTC9_2050; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + private void CreateDefaultCalibParams() { if(ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - calibDetailsAccelLN = new Dictionary>() + { + SENSOR_ID = SHIMMER_LSM6DSV_ACCEL_LN; + CalibDetails = new Dictionary>() { { 0, @@ -86,7 +73,8 @@ public Dictionary> GetCalibDetails() } else { - calibDetailsAccelLN = new Dictionary>() + SENSOR_ID = SHIMMER_ANALOG_ACCEL; + CalibDetails = new Dictionary>() { { 0, @@ -100,26 +88,18 @@ public Dictionary> GetCalibDetails() }; } - return calibDetailsAccelLN; + if (CalibDetails.TryGetValue(0, out var defaultCalib)) + { + AlignmentMatrixAccel = defaultCalib[0]; + SensitivityMatrixAccel = defaultCalib[1]; + OffsetVectorAccel = defaultCalib[2]; + } } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) { - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - - if (sensorID == SENSOR_ID) - { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - Console.WriteLine("LN Accel calibration parameters retrieved successfully."); - } - else - { - Console.WriteLine("Invalid calibration ID."); - } + (AlignmentMatrixAccel, SensitivityMatrixAccel, OffsetVectorAccel) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + Console.WriteLine("LN Accel calibration parameters retrieved successfully."); } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs index e15480bd..36318b9c 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/MagSensor.cs @@ -8,42 +8,28 @@ namespace ShimmerAPI.Sensors { public class MagSensor : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int SHIMMER_LSM303_MAG = 32; public readonly int SHIMMER_LIS2MDL_MAG = 42; public int SENSOR_ID { get; private set; } + public int ShimmerHardwareVersion { get; private set; } + public Dictionary> CalibDetails { get; private set; } public double[,] AlignmentMatrixMag = new double[3, 3]; public double[,] SensitivityMatrixMag = new double[3, 3]; public double[,] OffsetVectorMag = new double[3, 1]; - public Dictionary> calibDetailsMag; public MagSensor(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_LIS2MDL_MAG; - - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; - AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS3MDL; - OffsetVectorMag = OFFSET_VECTOR_MAG_SHIMMER3R_LIS3MDL; - } - else - { - SENSOR_ID = SHIMMER_LSM303_MAG; - - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3_LSM303AH; - AlignmentMatrixMag = ALIGNMENT_MATRIX_MAG_SHIMMER3_LSM303AH; - OffsetVectorMag = OFFSET_VECTOR_MAG_SHIMMER3_LSM303AH; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + public void CreateDefaultCalibParams() { if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - calibDetailsMag = new Dictionary>() + SENSOR_ID = SHIMMER_LIS2MDL_MAG; + CalibDetails = new Dictionary>() { { 0, @@ -85,7 +71,8 @@ public Dictionary> GetCalibDetails() } else { - calibDetailsMag = new Dictionary>() + SENSOR_ID = SHIMMER_LSM303_MAG; + CalibDetails = new Dictionary>() { { 0, @@ -98,23 +85,20 @@ public Dictionary> GetCalibDetails() } }; } - return calibDetailsMag; - } - - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) - { - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == CALIBRATION_ID) + if (CalibDetails.TryGetValue(0, out var defaultCalib)) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixMag, SensitivityMatrixMag, OffsetVectorMag) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("Mag calibration parameters"); + AlignmentMatrixMag = defaultCalib[0]; + SensitivityMatrixMag = defaultCalib[1]; + OffsetVectorMag = defaultCalib[2]; } } + + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + (AlignmentMatrixMag, SensitivityMatrixMag, OffsetVectorMag) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("Mag calibration parameters"); + } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs index d8877ef8..d41b04d6 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRAccel.cs @@ -8,42 +8,28 @@ namespace ShimmerAPI.Sensors { public class WRAccel : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int SHIMMER_LSM303_ACCEL = 31; public readonly int SHIMMER_LIS2DW12_ACCEL_WR = 39; + public int ShimmerHardwareVersion { get; private set; } public int SENSOR_ID { get; private set; } + public Dictionary> CalibDetails { get; private set; } public double[,] AlignmentMatrixAccel2 = new double[3, 3]; public double[,] SensitivityMatrixAccel2 = new double[3, 3]; public double[,] OffsetVectorAccel2 = new double[3, 1]; - public Dictionary> calibDetailsAccel2; public WRAccel(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_LIS2DW12_ACCEL_WR; - - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12; - AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3R_LIS2DW12; - OffsetVectorAccel2 = OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3R_LIS2DW12; - } - else - { - SENSOR_ID = SHIMMER_LSM303_ACCEL; - - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3_LSM303AH; - AlignmentMatrixAccel2 = ALIGNMENT_MATRIX_WIDE_RANGE_ACCEL_SHIMMER3_LSM303AH; - OffsetVectorAccel2 = OFFSET_VECTOR_ACCEL_WIDE_RANGE_SHIMMER3_LSM303AH; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + public void CreateDefaultCalibParams() { if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - calibDetailsAccel2 = new Dictionary>() + SENSOR_ID = SHIMMER_LIS2DW12_ACCEL_WR; + CalibDetails = new Dictionary>() { { 0, @@ -85,7 +71,8 @@ public Dictionary> GetCalibDetails() } else { - calibDetailsAccel2 = new Dictionary>() + SENSOR_ID = SHIMMER_LSM303_ACCEL; + CalibDetails = new Dictionary>() { { 0, @@ -125,23 +112,19 @@ public Dictionary> GetCalibDetails() } }; } - return calibDetailsAccel2; - } - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) - { - - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == SENSOR_ID) + if (CalibDetails.TryGetValue(0, out var defaultCalib)) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixAccel2, SensitivityMatrixAccel2, OffsetVectorAccel2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("WR Accel calibration parameters"); + AlignmentMatrixAccel2 = defaultCalib[0]; + SensitivityMatrixAccel2 = defaultCalib[1]; + OffsetVectorAccel2 = defaultCalib[2]; } + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + (AlignmentMatrixAccel2, SensitivityMatrixAccel2, OffsetVectorAccel2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("WR Accel calibration parameters"); } } } diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index 922cd27b..f9a087c0 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -8,37 +8,28 @@ namespace ShimmerAPI.Sensors { public class WRMag : AbstractSensor { - protected int ShimmerHardwareVersion = -1; public readonly int CALIBRATION_ID = 2; public readonly int ALT_MAG = 34; public readonly int SHIMMER_LIS3MDL_MAG = 41; + public int ShimmerHardwareVersion { get; private set; } public int SENSOR_ID { get; private set; } + public Dictionary> CalibDetails { get; private set; } public double[,] AlignmentMatrixMag2 = new double[3, 3]; public double[,] SensitivityMatrixMag2 = new double[3, 3]; public double[,] OffsetVectorMag2 = new double[3, 1]; - public Dictionary> calibDetailsMag; public WRMag(int hardwareVersion) { ShimmerHardwareVersion = hardwareVersion; - if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) - { - SENSOR_ID = SHIMMER_LIS3MDL_MAG; - SensitivityMatrixMag2 = SENSITIVITY_MATRIX_MAG_50GA_SHIMMER3R_LIS2MDL; - AlignmentMatrixMag2 = ALIGNMENT_MATRIX_MAG_SHIMMER3R_LIS2MDL; - OffsetVectorMag2 = OFFSET_VECTOR_MAG_SHIMMER3R_LIS2MDL; - } - else - { - SENSOR_ID = ALT_MAG; - } + CreateDefaultCalibParams(); } - public Dictionary> GetCalibDetails() + public void CreateDefaultCalibParams() { if (ShimmerHardwareVersion == (int)ShimmerBluetooth.ShimmerVersion.SHIMMER3R) { - calibDetailsMag = new Dictionary>() + SENSOR_ID = SHIMMER_LIS3MDL_MAG; + CalibDetails = new Dictionary>() { { 0, @@ -51,23 +42,23 @@ public Dictionary> GetCalibDetails() } }; } - return calibDetailsMag; - } - - public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) - { + else + { + SENSOR_ID = ALT_MAG; + } - var packetType = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 2); - var sensorID = ((int)packetType[0]) + ((int)packetType[1] << 8); - if (sensorID == SENSOR_ID) + if (CalibDetails.TryGetValue(0, out var defaultCalib)) { - var rangebytes = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var lengthsensorcal = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 1); - var ts = ProgrammerUtilities.CopyAndRemoveBytes(ref sensorcalibrationdump, 8); - (AlignmentMatrixMag2, SensitivityMatrixMag2, OffsetVectorMag2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); - System.Console.WriteLine("WR Mag calibration parameters"); + AlignmentMatrixMag2 = defaultCalib[0]; + SensitivityMatrixMag2 = defaultCalib[1]; + OffsetVectorMag2 = defaultCalib[2]; } + } + public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) + { + (AlignmentMatrixMag2, SensitivityMatrixMag2, OffsetVectorMag2) = UtilCalibration.RetrieveKinematicCalibrationParametersFromCalibrationDump(sensorcalibrationdump); + System.Console.WriteLine("WR Mag calibration parameters"); } } } diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 14a533ee..dabf78b4 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -192,25 +192,6 @@ public enum ChannelContents GsrRaw = 0x1C } - public enum CalibrationScaleFactor - { - NONE = 1, - TEN = 10, - ONE_HUNDRED = 100 - } - - public class Calibration - { - // TODO: offset scale factor currently not used - public static CalibrationScaleFactor mOffsetScaleFactor { get; set; } = CalibrationScaleFactor.NONE; - public static CalibrationScaleFactor mSensitivityScaleFactor { get; set; } = CalibrationScaleFactor.NONE; - public static CalibrationScaleFactor mAlignmentScaleFactor { get; set; } = CalibrationScaleFactor.ONE_HUNDRED; - public static double GetScaleFactor(CalibrationScaleFactor factor) - { - return 1.0; // (double)factor - } - } - private double[,] mAlignmentMatrix; private double[,] mSensitivityMatrix; private double[,] mVectorOffset; @@ -1064,7 +1045,7 @@ public Dictionary>> GetMapOfSensorCalibrati foreach (AbstractSensor sensorClass in mMapOfSensorClasses) { - Dictionary> returnVal = sensorClass.GetCalibDetails(); + Dictionary> returnVal = sensorClass.CalibDetails; if(returnVal != null) { mapOfSensorCalibration.Add(sensorClass.SENSOR_ID, returnVal); @@ -1142,16 +1123,16 @@ public byte[] GenerateCalParamByteArray(double[,] offsetVector, double[,] sensit double[,] sensitivityMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(sensitivityMatrix); for (int i = 0; i <= 2; i++) { - sensitivityMatrixToUse[i,i] = Math.Round(sensitivityMatrixToUse[i,i] * Calibration.GetScaleFactor(Calibration.mSensitivityScaleFactor)); + sensitivityMatrixToUse[i,i] = Math.Round(sensitivityMatrixToUse[i,i] * 1); } // Scale the alignment by 100 double[,] alignmentMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(alignmentMatrix); for (int i = 0; i <= 2; i++) { - alignmentMatrixToUse[i,0] = Math.Round(alignmentMatrixToUse[i,0] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); - alignmentMatrixToUse[i,1] = Math.Round(alignmentMatrixToUse[i,1] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); - alignmentMatrixToUse[i,2] = Math.Round(alignmentMatrixToUse[i,2] * Calibration.GetScaleFactor(Calibration.mAlignmentScaleFactor)); + alignmentMatrixToUse[i,0] = Math.Round(alignmentMatrixToUse[i,0] * 100); + alignmentMatrixToUse[i,1] = Math.Round(alignmentMatrixToUse[i,1] * 100); + alignmentMatrixToUse[i,2] = Math.Round(alignmentMatrixToUse[i,2] * 100); } // Generate the calibration bytes @@ -1261,108 +1242,35 @@ public void CalibByteDumpParse(byte[] calibBytesAll) var calibBytes = remainingBytes.Skip(12).Take(calibLength).ToArray(); - //need to update this to parse correct sensor id, sensor range for correct sensitivity - if (sensorId == SensorLNAccel.SENSOR_ID) + if (sensorId == SensorLNAccel.SENSOR_ID && rangeValue == LNAccelRange) { SensorLNAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); - if (LNAccelRange == 0) - { - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_2G_SHIMMER3R_LSM6DSV; - } - else if (LNAccelRange == 1) - { - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_4G_SHIMMER3R_LSM6DSV; - } - else if (LNAccelRange == 2) - { - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_8G_SHIMMER3R_LSM6DSV; - } - else if (LNAccelRange == 3) - { - SensitivityMatrixAccel = SENSITIVITY_MATRIX_LOW_NOISE_ACCEL_16G_SHIMMER3R_LSM6DSV; - } - + SensitivityMatrixAccel = SensorLNAccel.SensitivityMatrixAccel; AlignmentMatrixAccel = SensorLNAccel.AlignmentMatrixAccel; OffsetVectorAccel = SensorLNAccel.OffsetVectorAccel; } - else if (sensorId == SensorGyro.SENSOR_ID) + else if (sensorId == SensorGyro.SENSOR_ID && rangeValue == GyroRange) { SensorGyro.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); - if (GyroRange == 0) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_125DPS_SHIMMER3R_LSM6DSV; - } - else if (GyroRange == 1) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_250DPS_SHIMMER3R_LSM6DSV; - } - else if (GyroRange == 2) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_500DPS_SHIMMER3R_LSM6DSV; - } - else if (GyroRange == 3) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_1000DPS_SHIMMER3R_LSM6DSV; - } - else if (GyroRange == 4) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_2000DPS_SHIMMER3R_LSM6DSV; - } - else if (GyroRange == 5) - { - SensitivityMatrixGyro = SENSITIVITIY_MATRIX_GYRO_4000DPS_SHIMMER3R_LSM6DSV; - } - + SensitivityMatrixGyro = SensorGyro.SensitivityMatrixGyro; AlignmentMatrixGyro = SensorGyro.AlignmentMatrixGyro; OffsetVectorGyro = SensorGyro.OffsetVectorGyro; } - else if (sensorId == SensorWRAccel.SENSOR_ID) + else if (sensorId == SensorWRAccel.SENSOR_ID && rangeValue == AccelRange) { SensorWRAccel.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); - if (AccelRange == 0) - { - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_2G_SHIMMER3R_LIS2DW12; - } - else if (AccelRange == 1) - { - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_4G_SHIMMER3R_LIS2DW12; - } - else if (AccelRange == 2) - { - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_8G_SHIMMER3R_LIS2DW12; - } - else if (AccelRange == 3) - { - SensitivityMatrixAccel2 = SENSITIVITY_MATRIX_WIDE_RANGE_ACCEL_16G_SHIMMER3R_LIS2DW12; - } - + SensitivityMatrixAccel2 = SensorWRAccel.SensitivityMatrixAccel2; AlignmentMatrixAccel2 = SensorWRAccel.AlignmentMatrixAccel2; OffsetVectorAccel2 = SensorWRAccel.OffsetVectorAccel2; } - else if (sensorId == SensorMag.SENSOR_ID) + else if (sensorId == SensorMag.SENSOR_ID && rangeValue == MagGain) { SensorMag.RetrieveKinematicCalibrationParametersFromCalibrationDump(calibBytes); - if (GetMagRange() == 0) - { - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_4GA_SHIMMER3R_LIS3MDL; - } - else if (GetMagRange() == 1) - { - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_8GA_SHIMMER3R_LIS3MDL; - } - else if (GetMagRange() == 2) - { - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_12GA_SHIMMER3R_LIS3MDL; - } - else if (GetMagRange() == 3) - { - SensitivityMatrixMag = SENSITIVITY_MATRIX_MAG_16GA_SHIMMER3R_LIS3MDL; - } - + SensitivityMatrixMag = SensorMag.SensitivityMatrixMag; AlignmentMatrixMag = SensorMag.AlignmentMatrixMag; OffsetVectorMag = SensorMag.OffsetVectorMag; } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index a4ba088c..c02472da 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -189,7 +189,7 @@ public void Test004_ConnectandTestDefaultLNAccelAndGyroCalibParam() double[,] lnAccelOffset = mDevice.OffsetVectorAccel; Assert.IsTrue(lnAccelOffset.Cast().SequenceEqual(new double[,] { { 0 }, { 0 }, { 0 } }.Cast())); - double[,] lnAccelAlignment = mDevice.AlignmentMatrixAccel; + double[,] lnAccelAlignment = mDevice.AlignmentMatrixAccel; //it become -0.1 Assert.IsTrue(lnAccelAlignment.Cast().SequenceEqual(new double[,] { { -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, -1 } }.Cast())); double[,] lnAccelSensitivity0 = mDevice.SensitivityMatrixAccel; From 0a961352fbb35b4dca9751446451983209867b59 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Tue, 4 Feb 2025 17:19:15 +0800 Subject: [PATCH 17/20] fix on shimmer3 unit test --- ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs | 12 ++++++++---- ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs | 12 ++++++++---- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs index 1c371bf3..99a83366 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/HighGAccel.cs @@ -47,12 +47,16 @@ public void CreateDefaultCalibParams() SENSOR_ID = ALT_ACCEL; } - if (CalibDetails.TryGetValue(0, out var defaultCalib)) + if(CalibDetails != null) { - AlignmentMatrixAltAccel = defaultCalib[0]; - SensitivityMatrixAltAccel = defaultCalib[1]; - OffsetVectorAltAccel = defaultCalib[2]; + if (CalibDetails.TryGetValue(0, out var defaultCalib)) + { + AlignmentMatrixAltAccel = defaultCalib[0]; + SensitivityMatrixAltAccel = defaultCalib[1]; + OffsetVectorAltAccel = defaultCalib[2]; + } } + } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) diff --git a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs index f9a087c0..f9166334 100644 --- a/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs +++ b/ShimmerAPI/ShimmerAPI/Sensors/WRMag.cs @@ -47,12 +47,16 @@ public void CreateDefaultCalibParams() SENSOR_ID = ALT_MAG; } - if (CalibDetails.TryGetValue(0, out var defaultCalib)) + if(CalibDetails != null) { - AlignmentMatrixMag2 = defaultCalib[0]; - SensitivityMatrixMag2 = defaultCalib[1]; - OffsetVectorMag2 = defaultCalib[2]; + if (CalibDetails.TryGetValue(0, out var defaultCalib)) + { + AlignmentMatrixMag2 = defaultCalib[0]; + SensitivityMatrixMag2 = defaultCalib[1]; + OffsetVectorMag2 = defaultCalib[2]; + } } + } public void RetrieveKinematicCalibrationParametersFromCalibrationDump(byte[] sensorcalibrationdump) From 5ee2c9e38a8b41b27470b6e37046d4dd3e6ab68d Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Mon, 10 Feb 2025 09:39:51 +0800 Subject: [PATCH 18/20] update to print the calib dump, and calibration byte --- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 3 + .../ShimmerLogAndStreamS3Simulator.cs | 6 +- .../ShimmerAPI/Utilities/UtilShimmer.cs | 41 ++++++++++++++ .../ShimmerLogAndStreamS3RSimulatorTest.cs | 23 +++++--- .../ShimmerLogAndStreamS3SimulatorTest.cs | 56 +++++++++++++++++++ 5 files changed, 119 insertions(+), 10 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index dabf78b4..6ab3d885 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -2336,7 +2336,10 @@ public override void SDBT_switch(byte b) buffer.Add((byte)ReadByte()); } tempCalibDump = buffer.ToArray(); + Debug.WriteLine("CALIB_DUMP Received:\t" + UtilShimmer.BytesToHexString(tempCalibDump)); tempCalibDump = ProgrammerUtilities.RemoveLastBytes(tempCalibDump, (int)BluetoothCRCMode); + Debug.WriteLine("CALIB_DUMP concat:\t" + UtilShimmer.BytesToHexString(tempCalibDump)); + break; } case (byte)ShimmerBluetooth.PacketTypeShimmer3SDBT.TRIAL_CONFIG_RESPONSE: diff --git a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs index 984819e7..1924bbdc 100644 --- a/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs +++ b/ShimmerAPI/ShimmerAPI/Simulators/ShimmerLogAndStreamS3Simulator.cs @@ -252,7 +252,7 @@ protected override void WriteBytes(byte[] buffer, int index, int length) { mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x99); - byte[] bytes = UtilShimmer.HexStringToByteArray("8000005201030002000000160702000015BE0FB95C7E330000083C081F0825005300530054019C009C0100FEFF9C1E0000150000000000000000000000000000332C332C332C009C009C000000009C1E0001150000000000000000FDC00474FFC219D31957133C009CFF9C0100FE009C1E00021500000000000000000000000000000C"); + byte[] bytes = UtilShimmer.HexStringToByteArray("1802030003000000100B0200001520902D20D23300000805083907F9005300530054009C039C00FFF9019C1E0000150000000000000000000000000000332C332C332C009C009C000000009C1E0001154B57A224D23300000043FFCDFEEA19301911198A039CFF9DF50409049D1E00021500000000000000000000000000000C"); foreach (byte byteValue in bytes) { mBuffer.Add(byteValue); @@ -263,7 +263,7 @@ protected override void WriteBytes(byte[] buffer, int index, int length) { mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x99); - byte[] bytes = UtilShimmer.HexStringToByteArray("808000D00CD00CD0009C009C000000009C1E0003150000000000000000000000000000066806680668009C009C000000009C1F00001539D4942779330000FFF5018CFD240683067F0692FF9C00640000F8FE9C1F000115000000000000000000000000000000D100D100D1009C0064000000009C1F0002150000000000000000FFEB01"); + byte[] bytes = UtilShimmer.HexStringToByteArray("D00CD00CD0009C009C000000009C1E0003150000000000000000000000000000066806680668009C009C000000009C1F00001533B3A220D2330000FFF60047F7C7064B067606729CFFFFFF64FDFA019C1F0001150000000000000000000000000000032F032F032F9C000000640000009C1F0002150000000000000000000000"); foreach (byte byteValue in bytes) { mBuffer.Add(byteValue); @@ -274,7 +274,7 @@ protected override void WriteBytes(byte[] buffer, int index, int length) { mBuffer.Add((byte)0xff); mBuffer.Add((byte)0x99); - byte[] bytes = UtilShimmer.HexStringToByteArray("5400015AFDF8034203410348009C0064FF00FC009C1F000315000000000000000000000000000001A201A201A2009C0064000000009C200000150000000000000000002AFBEDFC21010700E500FD009C0064000000009C"); + byte[] bytes = UtilShimmer.HexStringToByteArray("0000000198019801989C000000640000009C1F00031500000000000000000000000000000087008700879C000000640000009C20000115F5980C0000000000000000000000044C044C03D49C000000640000009C2000021500000000000000000000000000000357035702F89C000000640000009C2000031500000000000000"); foreach (byte byteValue in bytes) { mBuffer.Add(byteValue); diff --git a/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs b/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs index a52a945d..38a3df59 100644 --- a/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs +++ b/ShimmerAPI/ShimmerAPI/Utilities/UtilShimmer.cs @@ -102,6 +102,47 @@ public static byte[] ConvertMilliSecondsToShimmerRtcDataBytesMSB(long millisecon } return result; } + public static string GetDebugString(int mRangeValue, List sensorClass) + { + string debugString = $"RangeValue: {mRangeValue}\n"; + debugString += GenerateDebugStringPerProperty("Default Offset Vector", sensorClass[2]); + debugString += GenerateDebugStringPerProperty("Default Sensitivity", sensorClass[1]); + debugString += GenerateDebugStringPerProperty("Default Alignment", sensorClass[0]); + + return debugString; + } + + public static String GenerateDebugStringPerProperty(String property, double[,] calMatrix) + { + String debugString = property + " =\n"; + if (calMatrix == null) + { + debugString += "NULL\n"; + } + else + { + debugString += UtilShimmer.DoubleArrayToString(calMatrix); + } + return debugString; + } + + public static string DoubleArrayToString(double[,] doubleArray) + { + StringBuilder returnString = new StringBuilder(); + int rows = doubleArray.GetLength(0); + int cols = doubleArray.GetLength(1); + + for (int x = 0; x < rows; x++) + { + for (int y = 0; y < cols; y++) + { + returnString.Append(doubleArray[x, y]).Append("\t"); + } + returnString.AppendLine(); + } + + return returnString.ToString(); + } } } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index c02472da..26927f2c 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -140,18 +140,27 @@ public void Test003_ConnectandTestCalibParamRead() System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); mDevice.CalibByteDumpParse(deviceCalBytes); - foreach (int sensorId in mDevice.mapOfSensorCalibration.Keys) + Object returnValue = mDevice.mapOfSensorCalibration; + + Assert.IsNotNull(returnValue); + + Dictionary>> mapOfKinematicSensorCalibrationAll = returnValue as Dictionary>>; + + foreach (int sensorId in mapOfKinematicSensorCalibrationAll.Keys) { - object sensorDetails = mDevice.GetSensorDetails(sensorId); + Dictionary> route1CalParamMapPerSensor = mapOfKinematicSensorCalibrationAll[sensorId]; + + Object sensorDetails = mDevice.GetSensorDetails(sensorId); Assert.IsNotNull(sensorDetails); - foreach (var returnValues in mDevice.mapOfSensorCalibration) + Console.WriteLine("sensorId : " + sensorId); + + Dictionary> mapOfKinematicCalibPerRange = mapOfKinematicSensorCalibrationAll[sensorId]; + foreach (var calibDetails in mapOfKinematicCalibPerRange) { - if (returnValues.Key == sensorId && returnValues.Value != null) - { - Assert.IsNotNull(returnValues.Value); - } + Console.WriteLine(UtilShimmer.GetDebugString(calibDetails.Key, calibDetails.Value)); } + Console.WriteLine("\n"); } diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index 4c5f1fb2..6800d019 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -2,6 +2,7 @@ using ShimmerAPI; using ShimmerAPI.Sensors; using ShimmerAPI.Simulators; +using ShimmerAPI.Utilities; using System; using System.Collections.Concurrent; using System.Collections.Generic; @@ -124,5 +125,60 @@ public void Test002_testConnectandDisconnect_NewBMPSupported() } } + [TestMethod] + public void Test003_ConnectandTestCalibParamRead() + { + if (mDevice != null) + { + try + { + mDevice.StartConnectThread(); + Thread.Sleep(30000); + + if (!mDevice.IsConnected()) + { + Assert.Fail(); + } + + byte[] deviceCalBytes = mDevice.CalibByteDumpGenerate(); + System.Console.WriteLine("deviceCalBytes : " + UtilShimmer.BytesToHexString(deviceCalBytes)); + mDevice.CalibByteDumpParse(deviceCalBytes); + + Object returnValue = mDevice.mapOfSensorCalibration; + + Assert.IsNotNull(returnValue); + + Dictionary>> mapOfKinematicSensorCalibrationAll = returnValue as Dictionary>>; + + foreach (int sensorId in mapOfKinematicSensorCalibrationAll.Keys) + { + Dictionary> route1CalParamMapPerSensor = mapOfKinematicSensorCalibrationAll[sensorId]; + + Object sensorDetails = mDevice.GetSensorDetails(sensorId); + Assert.IsNotNull(sensorDetails); + + Console.WriteLine("sensorId : " + sensorId); + + Dictionary> mapOfKinematicCalibPerRange = mapOfKinematicSensorCalibrationAll[sensorId]; + foreach (var calibDetails in mapOfKinematicCalibPerRange) + { + Console.WriteLine(UtilShimmer.GetDebugString(calibDetails.Key, calibDetails.Value)); + } + Console.WriteLine("\n"); + + } + + } + catch (Exception ex) + { + Assert.Fail($"Test aborted due to exception: {ex.Message}"); + } + } + else + { + Assert.Fail("mDevice is null"); + } + } + } } From f9ce64629466bae2d60cd8b50cc1818fbc5837e1 Mon Sep 17 00:00:00 2001 From: Harith Jamadi <114632577+harithjamadi@users.noreply.github.com> Date: Mon, 10 Feb 2025 10:18:17 +0800 Subject: [PATCH 19/20] fix the failed build unit test --- .../ShimmerLogAndStreamS3RSimulatorTest.cs | 5 ----- .../ShimmerLogAndStreamS3SimulatorTest.cs | 20 ++++++------------- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs index 26927f2c..a946f3d3 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3RSimulatorTest.cs @@ -53,11 +53,6 @@ public void Test001_testConnectandDisconnect() Assert.Fail(); } - if (!mDevice.isGetPressureCalibrationCoefficientsCommand) - { - Assert.Fail(); - } - } catch (Exception ex) { diff --git a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs index 6800d019..f95564fd 100644 --- a/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs +++ b/ShimmerAPI/ShimmerUnitTests/ShimmerLogAndStreamS3SimulatorTest.cs @@ -98,20 +98,12 @@ public void Test002_testConnectandDisconnect_NewBMPSupported() Assert.Fail(); } - //if (!mDevice.isGetBmp390CalibrationCoefficientsCommand) - //{ - // Assert.Fail(); - //} - - //if (mDevice.GetEnabledSensors() != (0x00 | (int)SensorBitmapShimmer3R.SENSOR_BMP380_PRESSURE)) - //{ - // Assert.Fail(); - //} - - //foreach (byte b in mDevice.ListofSensorChannels) - //{ - // Console.WriteLine(b + " ; "); - //} + mDevice.ReadPressureCalibrationCoefficients(); + if (!mDevice.isGetPressureCalibrationCoefficientsCommand) + { + Assert.Fail(); + } + } catch (Exception ex) From 89e8b146dfa421c14b366474c936e46339c2aaaf Mon Sep 17 00:00:00 2001 From: JongChern Date: Wed, 23 Apr 2025 14:28:18 +0800 Subject: [PATCH 20/20] Update ShimmerLogAndStream.cs --- ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs | 112 ------------------- 1 file changed, 112 deletions(-) diff --git a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs index 6ab3d885..2276efa8 100644 --- a/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs +++ b/ShimmerAPI/ShimmerAPI/ShimmerLogAndStream.cs @@ -192,10 +192,6 @@ public enum ChannelContents GsrRaw = 0x1C } - private double[,] mAlignmentMatrix; - private double[,] mSensitivityMatrix; - private double[,] mVectorOffset; - public Dictionary>> mapOfSensorCalibration = new Dictionary>>(); // btsd changes @@ -1056,114 +1052,6 @@ public Dictionary>> GetMapOfSensorCalibrati return mapOfSensorCalibration; } - public byte[] CalibByteDumpGenerate() - { - byte[] calibBytesAll = new byte[] { }; - Dictionary>> mapOfAllCalib = GetMapOfSensorCalibrationAll(); - - foreach (var sensorEntry in mapOfAllCalib) - { - int sensorId = sensorEntry.Key; - var calibMapPerSensor = sensorEntry.Value; - - foreach (var calibEntry in calibMapPerSensor) - { - byte[] calibBytesPerSensor = GenerateCalibDump(calibEntry); - - if (calibBytesPerSensor != null) - { - byte[] calibSensorKeyBytes = new byte[2]; - calibSensorKeyBytes[0] = (byte)((sensorId >> 0) & 0xFF); - calibSensorKeyBytes[1] = (byte)((sensorId >> 8) & 0xFF); - - calibBytesPerSensor = calibSensorKeyBytes.Concat(calibBytesPerSensor).ToArray(); - calibBytesAll = calibBytesAll.Concat(calibBytesPerSensor).ToArray(); - } - } - } - - byte[] svoBytes = GenerateVersionByteArrayNew(); - byte[] concatBytes = svoBytes.Concat(calibBytesAll).ToArray(); - - byte[] packetLength = new byte[2]; - packetLength[0] = (byte)(concatBytes.Length & 0xFF); - packetLength[1] = (byte)((concatBytes.Length >> 8) & 0xFF); - - concatBytes = packetLength.Concat(concatBytes).ToArray(); - - return concatBytes; - } - - public byte[] GenerateCalibDump(KeyValuePair> calibEntry) - { - byte[] rangeBytes = new byte[1]; - int rangeValue = (int)calibEntry.Key; - rangeBytes[0] = (byte)(rangeValue & 0xFF); - - (mAlignmentMatrix, mSensitivityMatrix, mVectorOffset) = (calibEntry.Value[0], calibEntry.Value[1], calibEntry.Value[2]); - byte[] timestamp = UtilShimmer.ConvertMilliSecondsToShimmerRtcDataBytesLSB(0); //usually used getCalibTimeMs() - byte[] bufferCalibParam = GenerateCalParamByteArray(); - byte[] calibLength = new byte[] { (byte)bufferCalibParam.Length }; - - byte[] returnArray = rangeBytes.Concat(calibLength).ToArray(); - returnArray = returnArray.Concat(timestamp).ToArray(); - returnArray = returnArray.Concat(bufferCalibParam).ToArray(); - - return returnArray; - } - - public byte[] GenerateCalParamByteArray() - { - return GenerateCalParamByteArray(mVectorOffset, mSensitivityMatrix, mAlignmentMatrix); - } - - public byte[] GenerateCalParamByteArray(double[,] offsetVector, double[,] sensitivityMatrix, double[,] alignmentMatrix) - { - // Scale the sensitivity if needed - double[,] sensitivityMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(sensitivityMatrix); - for (int i = 0; i <= 2; i++) - { - sensitivityMatrixToUse[i,i] = Math.Round(sensitivityMatrixToUse[i,i] * 1); - } - - // Scale the alignment by 100 - double[,] alignmentMatrixToUse = UtilShimmer.DeepCopyDoubleMatrix(alignmentMatrix); - for (int i = 0; i <= 2; i++) - { - alignmentMatrixToUse[i,0] = Math.Round(alignmentMatrixToUse[i,0] * 100); - alignmentMatrixToUse[i,1] = Math.Round(alignmentMatrixToUse[i,1] * 100); - alignmentMatrixToUse[i,2] = Math.Round(alignmentMatrixToUse[i,2] * 100); - } - - // Generate the calibration bytes - byte[] bufferCalibParam = new byte[21]; - - // offsetVector -> buffer offset = 0 - for (int i = 0; i < 3; i++) - { - bufferCalibParam[0 + (i * 2)] = (byte)((((int)offsetVector[i,0]) >> 8) & 0xFF); - bufferCalibParam[0 + (i * 2) + 1] = (byte)((((int)offsetVector[i,0]) >> 0) & 0xFF); - } - - // sensitivityMatrix -> buffer offset = 6 - for (int i = 0; i < 3; i++) - { - bufferCalibParam[6 + (i * 2)] = (byte)((((int)sensitivityMatrixToUse[i,i]) >> 8) & 0xFF); - bufferCalibParam[6 + (i * 2) + 1] = (byte)((((int)sensitivityMatrixToUse[i,i]) >> 0) & 0xFF); - } - - // alignmentMatrix -> buffer offset = 12 - for (int i = 0; i < 3; i++) - { - bufferCalibParam[12 + (i * 3)] = (byte)(((int)(alignmentMatrixToUse[i,0])) & 0xFF); - bufferCalibParam[12 + (i * 3) + 1] = (byte)(((int)(alignmentMatrixToUse[i,1])) & 0xFF); - bufferCalibParam[12 + (i * 3) + 2] = (byte)(((int)(alignmentMatrixToUse[i,2])) & 0xFF); - } - - return bufferCalibParam; - } - - protected byte[] GenerateVersionByteArrayNew() { byte[] byteArray = new byte[8];