diff --git a/ShimmerBluetooth/ShimmerBluetooth/ConfigByteLayoutShimmer3.swift b/ShimmerBluetooth/ShimmerBluetooth/ConfigByteLayoutShimmer3.swift index 0a6c76f..bf91fe5 100644 --- a/ShimmerBluetooth/ShimmerBluetooth/ConfigByteLayoutShimmer3.swift +++ b/ShimmerBluetooth/ShimmerBluetooth/ConfigByteLayoutShimmer3.swift @@ -42,6 +42,8 @@ public class ConfigByteLayoutShimmer3{ static let idxMPU9150GyroCalibration = 52 static let idxLSM303DLHCMagCalibration = 73 //static let idxLSM303DLHCAccelCalibration = 94 //94->114 + static let idxConfigSetupByte4 = 128+2; + static let idxSDExperimentConfig0 = 128+89; static let idxSDExperimentConfig1 = 128+90; @@ -51,6 +53,9 @@ public class ConfigByteLayoutShimmer3{ static let bitShiftMPU9150GyroRange = 0; static let maskMPU9150GyroRange = 0x03; + static let bitShiftLSM6DSVyroRangeMSB = 2; + static let maskLSM6DSVyroRangeMSB = 0x01; + static let bitShiftMPU9150AccelGyroSamplingRate = 0; static let maskMPU9150AccelGyroSamplingRate = 0xFF; @@ -73,6 +78,7 @@ public class ConfigByteLayoutShimmer3{ static let maskLSM303DLHCAccelSamplingRate = 0x0F; static let idxLSM6DSVAccelCalibration = 34; + static let idxGyroCalibration = 55; static let idxLSM303DLHCAccelCalibration = 97; static let lengthGeneralCalibrationBytes = 21; diff --git a/ShimmerBluetooth/ShimmerBluetooth/GyroSensor.swift b/ShimmerBluetooth/ShimmerBluetooth/GyroSensor.swift index 31a1f96..be77749 100644 --- a/ShimmerBluetooth/ShimmerBluetooth/GyroSensor.swift +++ b/ShimmerBluetooth/ShimmerBluetooth/GyroSensor.swift @@ -31,15 +31,49 @@ public class GyroSensor : IMUSensor , SensorProcessing{ } } - var CurrentRange = Range.RANGE_250DPS + public enum Range3R: UInt8 { + case RANGE_125DPS = 0x0 + case RANGE_250DPS = 0x1 + case RANGE_500DPS = 0x2 + case RANGE_1000DPS = 0x3 + case RANGE_2000DPS = 0x4 + case RANGE_4000DPS = 0x5 + + public static func fromValue(_ value : UInt8) -> Range3R? { + switch value{ + case 0: + return .RANGE_125DPS + case 1: + return .RANGE_250DPS + case 2: + return .RANGE_500DPS + case 3: + return .RANGE_1000DPS + case 4: + return .RANGE_2000DPS + case 5: + return .RANGE_4000DPS + default: + return nil + } + } + } + + var CurrentRange = Range.RANGE_250DPS + var Current3RRange = Range3R.RANGE_250DPS + + public var packetIndexGyroX:Int = -1 public var packetIndexGyroY:Int = -1 public var packetIndexGyroZ:Int = -1 public static let GYROSCOPE_X = "Gyroscope X" public static let GYROSCOPE_Y = "Gyroscope Y" public static let GYROSCOPE_Z = "Gyroscope Z" - let CALIBRATION_ID = 30 + var CALIBRATION_ID = 30 + var AlignmentMatrix_125DPS:[[Double]] = [[]] + var SensitivityMatrix_125DPS:[[Double]] = [[]] + var OffsetVector_125DPS:[Double]=[] var AlignmentMatrix_250DPS:[[Double]] = [[]] var SensitivityMatrix_250DPS:[[Double]] = [[]] var OffsetVector_250DPS:[Double]=[] @@ -52,18 +86,40 @@ public class GyroSensor : IMUSensor , SensorProcessing{ var AlignmentMatrix_2000DPS:[[Double]] = [[]] var SensitivityMatrix_2000DPS:[[Double]] = [[]] var OffsetVector_2000DPS:[Double]=[] + var AlignmentMatrix_4000DPS:[[Double]] = [[]] + var SensitivityMatrix_4000DPS:[[Double]] = [[]] + var OffsetVector_4000DPS:[Double]=[] + var gyroRange = 1 var AlignmentMatrix : [[Double]] = [[]] var SensitivityMatrix : [[Double]] = [[]] var OffsetVector : [Double] = [] + var calibBytes_125DPS: [UInt8] = [] + var calibBytes_250DPS: [UInt8] = [] + var calibBytes_500DPS: [UInt8] = [] + var calibBytes_1000DPS: [UInt8] = [] + var calibBytes_2000DPS: [UInt8] = [] + var calibBytes_4000DPS: [UInt8] = [] + public func processData(sensorPacket: [UInt8], objectCluster: ObjectCluster) -> ObjectCluster { let x = Array(sensorPacket[packetIndexGyroX.. [UInt8]{ var infomemtoupdate = infomem print("oriinfomem: \(infomemtoupdate)") - if(HardwareVersion == Shimmer3Protocol.HardwareType.Shimmer3.rawValue){ - var gyroRange = 0 + var gyroRange = 0 + var calibBytes = calibBytes_250DPS if (range == Range.RANGE_250DPS){ gyroRange = 0 + calibBytes = calibBytes_250DPS } else if (range == Range.RANGE_500DPS){ gyroRange = 1 + calibBytes = calibBytes_500DPS } else if (range == Range.RANGE_1000DPS){ gyroRange = 2 + calibBytes = calibBytes_1000DPS } else if (range == Range.RANGE_2000DPS){ gyroRange = 3 + calibBytes = calibBytes_2000DPS } + + + infomemtoupdate.replaceSubrange( + ConfigByteLayoutShimmer3.idxGyroCalibration.. [UInt8]{ + var infomemtoupdate = infomem + print("oriinfomem: \(infomemtoupdate)") + var gyroRange = 0 + var calibBytes = calibBytes_125DPS + if (range == Range3R.RANGE_125DPS){ + gyroRange = 0 + calibBytes = calibBytes_125DPS + }else if (range == Range3R.RANGE_250DPS){ + gyroRange = 1 + calibBytes = calibBytes_250DPS + } else if (range == Range3R.RANGE_500DPS){ + gyroRange = 2 + calibBytes = calibBytes_500DPS + } else if (range == Range3R.RANGE_1000DPS){ + gyroRange = 3 + calibBytes = calibBytes_1000DPS + } else if (range == Range3R.RANGE_2000DPS){ + gyroRange = 4 + calibBytes = calibBytes_2000DPS + }else if (range == Range3R.RANGE_4000DPS){ + gyroRange = 5 + calibBytes = calibBytes_4000DPS + } + + + infomemtoupdate.replaceSubrange( + ConfigByteLayoutShimmer3.idxGyroCalibration..>2)<Range{ return CurrentRange } + public func get3RRange()->Range3R{ + return Current3RRange + } + public func setInfoMem(infomem: [UInt8]) { var enabled = Int(infomem[ConfigByteLayoutShimmer3.idxSensors0]>>6) & 1 @@ -149,30 +319,73 @@ public class GyroSensor : IMUSensor , SensorProcessing{ sensorEnabled = false } - gyroRange = (Int(infomem[ConfigByteLayoutShimmer3.idxConfigSetupByte2] >> ConfigByteLayoutShimmer3.bitShiftMPU9150GyroRange) & ConfigByteLayoutShimmer3.maskMPU9150GyroRange) - if (gyroRange == 0){ - CurrentRange = Range.RANGE_250DPS - AlignmentMatrix = AlignmentMatrix_250DPS - SensitivityMatrix = SensitivityMatrix_250DPS - OffsetVector = OffsetVector_250DPS - } - if (gyroRange == 1){ - CurrentRange = Range.RANGE_500DPS - AlignmentMatrix = AlignmentMatrix_500DPS - SensitivityMatrix = SensitivityMatrix_500DPS - OffsetVector = OffsetVector_500DPS - } - if (gyroRange == 2){ - CurrentRange = Range.RANGE_1000DPS - AlignmentMatrix = AlignmentMatrix_1000DPS - SensitivityMatrix = SensitivityMatrix_1000DPS - OffsetVector = OffsetVector_1000DPS - } - if (gyroRange == 3){ - CurrentRange = Range.RANGE_2000DPS - AlignmentMatrix = AlignmentMatrix_2000DPS - SensitivityMatrix = SensitivityMatrix_2000DPS - OffsetVector = OffsetVector_2000DPS + if(HardwareVersion == Shimmer3Protocol.HardwareType.Shimmer3.rawValue){ + gyroRange = (Int(infomem[ConfigByteLayoutShimmer3.idxConfigSetupByte2] >> ConfigByteLayoutShimmer3.bitShiftMPU9150GyroRange) & ConfigByteLayoutShimmer3.maskMPU9150GyroRange) + if (gyroRange == 0){ + CurrentRange = Range.RANGE_250DPS + AlignmentMatrix = AlignmentMatrix_250DPS + SensitivityMatrix = SensitivityMatrix_250DPS + OffsetVector = OffsetVector_250DPS + } + if (gyroRange == 1){ + CurrentRange = Range.RANGE_500DPS + AlignmentMatrix = AlignmentMatrix_500DPS + SensitivityMatrix = SensitivityMatrix_500DPS + OffsetVector = OffsetVector_500DPS + } + if (gyroRange == 2){ + CurrentRange = Range.RANGE_1000DPS + AlignmentMatrix = AlignmentMatrix_1000DPS + SensitivityMatrix = SensitivityMatrix_1000DPS + OffsetVector = OffsetVector_1000DPS + } + if (gyroRange == 3){ + CurrentRange = Range.RANGE_2000DPS + AlignmentMatrix = AlignmentMatrix_2000DPS + SensitivityMatrix = SensitivityMatrix_2000DPS + OffsetVector = OffsetVector_2000DPS + } + }else if(HardwareVersion == Shimmer3Protocol.HardwareType.Shimmer3R.rawValue){ + let gyroRangeLSB = (Int(infomem[ConfigByteLayoutShimmer3.idxConfigSetupByte2] >> ConfigByteLayoutShimmer3.bitShiftMPU9150GyroRange) & ConfigByteLayoutShimmer3.maskMPU9150GyroRange) + let gyroRangeMSB = (Int(infomem[ConfigByteLayoutShimmer3.idxConfigSetupByte4] >> ConfigByteLayoutShimmer3.bitShiftLSM6DSVyroRangeMSB) & ConfigByteLayoutShimmer3.maskLSM6DSVyroRangeMSB) + + gyroRange = ((gyroRangeMSB<<2) | gyroRangeLSB) + if (gyroRange == 0){ + Current3RRange = Range3R.RANGE_125DPS + AlignmentMatrix = AlignmentMatrix_125DPS + SensitivityMatrix = SensitivityMatrix_125DPS + OffsetVector = OffsetVector_125DPS + } + if (gyroRange == 1){ + Current3RRange = Range3R.RANGE_250DPS + AlignmentMatrix = AlignmentMatrix_250DPS + SensitivityMatrix = SensitivityMatrix_250DPS + OffsetVector = OffsetVector_250DPS + } + if (gyroRange == 2){ + Current3RRange = Range3R.RANGE_500DPS + AlignmentMatrix = AlignmentMatrix_500DPS + SensitivityMatrix = SensitivityMatrix_500DPS + OffsetVector = OffsetVector_500DPS + } + if (gyroRange == 3){ + Current3RRange = Range3R.RANGE_1000DPS + AlignmentMatrix = AlignmentMatrix_1000DPS + SensitivityMatrix = SensitivityMatrix_1000DPS + OffsetVector = OffsetVector_1000DPS + } + if (gyroRange == 4){ + Current3RRange = Range3R.RANGE_2000DPS + AlignmentMatrix = AlignmentMatrix_2000DPS + SensitivityMatrix = SensitivityMatrix_2000DPS + OffsetVector = OffsetVector_2000DPS + } + if (gyroRange == 5){ + Current3RRange = Range3R.RANGE_4000DPS + AlignmentMatrix = AlignmentMatrix_4000DPS + SensitivityMatrix = SensitivityMatrix_4000DPS + OffsetVector = OffsetVector_4000DPS + } } } diff --git a/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift b/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift index 4b53e5d..6d24c09 100644 --- a/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift +++ b/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift @@ -324,6 +324,7 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { battVoltageSensor = BattVoltageSensor() } else if(REV_HW_MAJOR==HardwareType.Shimmer3R.rawValue){ lnAccelSensor = LNAccelSensor(hwid: REV_HW_MAJOR) + gyroSensor = GyroSensor(hwid: REV_HW_MAJOR) wrAccelSensor = WRAccelSensor(hwid: REV_HW_MAJOR) timeSensor = TimeSensor() } @@ -350,6 +351,7 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { } else if (REV_HW_MAJOR==HardwareType.Shimmer3R.rawValue){ timeSensor.setInfoMem(infomem: infoMem) lnAccelSensor.setInfoMem(infomem: infoMem) + gyroSensor.setInfoMem(infomem: infoMem) wrAccelSensor.setInfoMem(infomem: infoMem) } } diff --git a/ShimmerExamplePlot/ShimmerExamplePlot/ContentView.swift b/ShimmerExamplePlot/ShimmerExamplePlot/ContentView.swift index 1ebcdff..e26b27f 100644 --- a/ShimmerExamplePlot/ShimmerExamplePlot/ContentView.swift +++ b/ShimmerExamplePlot/ShimmerExamplePlot/ContentView.swift @@ -263,6 +263,16 @@ struct ContentView: View { } } }) + + Button("WriteInfoMem Gyro Shimmer3R",action:{ Task { + do { + await viewModel.sendInfoMemS3RGyro() + } catch { + print("Error: \(error)") + } + } + }) + Button("WriteInfoMem WRAccel Shimmer3R",action:{ Task { do { await viewModel.sendInfoMemS3RWRAccel() @@ -341,6 +351,15 @@ struct ContentView: View { // Update the ViewModel's wrRangeIndex property viewModel.lnAccelRangeIndex = newValue } + Picker("Select Gyro Range", selection: $viewModel.gyroRange3RIndex) { + ForEach(0..