diff --git a/ShimmerBluetooth/ShimmerBluetooth/PressureTempSensor.swift b/ShimmerBluetooth/ShimmerBluetooth/PressureTempSensor.swift index 8beea06..bc4ca59 100644 --- a/ShimmerBluetooth/ShimmerBluetooth/PressureTempSensor.swift +++ b/ShimmerBluetooth/ShimmerBluetooth/PressureTempSensor.swift @@ -39,82 +39,259 @@ public class PressureTempSensor: Sensor , SensorProcessing{ var pressureResolution = 0 public static let TEMPERATURE = "Temperature" public static let PRESSURE = "Pressure" - public static var DIG_T1 = 27504.0; // unsigned short - public static var DIG_T2 = 26435.0; // signed short - public static var DIG_T3 = -1000.0; // signed short - public static var DIG_P1 = 36477.0; // unsigned short - public static var DIG_P2 = -10685.0; // signed short - public static var DIG_P3 = 3024.0; // signed short - public static var DIG_P4 = 2855.0; // signed short - public static var DIG_P5 = 140.0; // signed short - public static var DIG_P6 = -7.0; // signed short - public static var DIG_P7 = 15500.0; // signed short - public static var DIG_P8 = -14600.0; // signed short - public static var DIG_P9 = 6000.0; // signed short + public var packetIndexTemp:Int = -1 + public var packetIndexPressure:Int = -1 + public var DIG_T1 = 27504.0; // unsigned short + public var DIG_T2 = 26435.0; // signed short + public var DIG_T3 = -1000.0; // signed short + public var DIG_P1 = 36477.0; // unsigned short + public var DIG_P2 = -10685.0; // signed short + public var DIG_P3 = 3024.0; // signed short + public var DIG_P4 = 2855.0; // signed short + public var DIG_P5 = 140.0; // signed short + public var DIG_P6 = -7.0; // signed short + public var DIG_P7 = 15500.0; // signed short + public var DIG_P8 = -14600.0; // signed short + public var DIG_P9 = 6000.0; // signed short + + //3R COEFFICIENTS + + var bmp380_t1: UInt16 = 0 + var bmp380_t2: UInt16 = 0 + var bmp380_t3: Int8 = 0 + + var bmp380_p1: Int16 = 0 + var bmp380_p2: Int16 = 0 + var bmp380_p3: Int8 = 0 + var bmp380_p4: Int8 = 0 + var bmp380_p5: UInt16 = 0 + var bmp380_p6: UInt16 = 0 + var bmp380_p7: Int8 = 0 + var bmp380_p8: Int8 = 0 + var bmp380_p9: Int16 = 0 + var bmp380_p10: Int16 = 0 + var bmp380_p11: Int8 = 0 + + // Intermediate for compensation + var bmp380_tLin: Double = 0.0 + + // Quantized calibration (floating-point) + var par_T1: Double = 0 + var par_T2: Double = 0 + var par_T3: Double = 0 + var par_P1: Double = 0 + var par_P2: Double = 0 + var par_P3: Double = 0 + var par_P4: Double = 0 + var par_P5: Double = 0 + var par_P6: Double = 0 + var par_P7: Double = 0 + var par_P8: Double = 0 + var par_P9: Double = 0 + var par_P10: Double = 0 + var par_P11: Double = 0 + + // TLin used for pressure + var quantized_TLin: Double = 0 + + + public func processData(sensorPacket: [UInt8], objectCluster: ObjectCluster) -> ObjectCluster { - let temperature = Array(sensorPacket[3..<5]) - let pressure = Array(sensorPacket[5..<8]) - - var rawTemperature = Double(ShimmerUtilities.parseSensorData(sensorData: temperature, dataType: SensorDataType.u16MSB)!) - var rawPressure = Double(ShimmerUtilities.parseSensorData(sensorData: pressure, dataType: SensorDataType.u24MSB)!) - - objectCluster.addData(sensorName: PressureTempSensor.TEMPERATURE, formatName: SensorFormats.Raw.rawValue, unitName: SensorUnits.noUnit.rawValue, value: Double(rawTemperature)) - objectCluster.addData(sensorName: PressureTempSensor.PRESSURE, formatName: SensorFormats.Raw.rawValue, unitName: SensorUnits.noUnit.rawValue, value: Double(rawPressure)) - - if (calibrationEnabled){ - //for SensorBMP280 - rawTemperature = rawTemperature * pow(2,4) - rawPressure = rawPressure/pow(2,4) - - let adc_T = rawTemperature; - let adc_P = rawPressure; - - // Returns temperature in DegC, double precision. Output value of “51.23” equals 51.23 DegC. - // t_fine carries fine temperature as global value - var var1 = ((adc_T)/16384.0 - PressureTempSensor.DIG_T1/1024.0) * PressureTempSensor.DIG_T2; - var var2 = (((adc_T)/131072.0 - PressureTempSensor.DIG_T1/8192.0) * (adc_T/131072.0 - PressureTempSensor.DIG_T1/8192.0)) * PressureTempSensor.DIG_T3; - let t_fine = var1 + var2; - let T = t_fine / 5120.0; - //double fTemp = T * 1.8 + 32; // Fahrenheit - //T = T/100.0; - // Returns pressure in Pa as double. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa - var1 = (t_fine/2.0) - 64000.0; - var2 = var1 * var1 * PressureTempSensor.DIG_P6 / 32768.0; - var2 = var2 + var1 * PressureTempSensor.DIG_P5 * 2.0; - var2 = (var2/4.0)+(PressureTempSensor.DIG_P4 * 65536.0); - var1 = (PressureTempSensor.DIG_P3 * var1 * var1 / 524288.0 + PressureTempSensor.DIG_P2 * var1) / 524288.0; - var1 = (1.0 + var1 / 32768.0)*PressureTempSensor.DIG_P1; - if (var1 == 0.0) { - // return 0; // avoid exception caused by division by zero + if self.HardwareVersion == Shimmer3Protocol.HardwareType.Shimmer3R.rawValue { + let idxT = packetIndexTemp + let idxP = packetIndexPressure + + let rawT = u24(sensorPacket[idxT], + sensorPacket[idxT+1], + sensorPacket[idxT+2]) + + let rawP = u24(sensorPacket[idxP], + sensorPacket[idxP+1], + sensorPacket[idxP+2]) + + objectCluster.addData(sensorName: PressureTempSensor.TEMPERATURE, + formatName: SensorFormats.Raw.rawValue, + unitName: SensorUnits.noUnit.rawValue, + value: Double(rawT)) + + objectCluster.addData(sensorName: PressureTempSensor.PRESSURE, + formatName: SensorFormats.Raw.rawValue, + unitName: SensorUnits.noUnit.rawValue, + value: Double(rawP)) + + let (compPress, tLin) = calibratePressure390(UP: Double(rawP), + UT: Double(rawT)) + + let temperatureC = tLin + let pressurekPa = compPress / 1000.0 + + objectCluster.addData(sensorName: PressureTempSensor.TEMPERATURE, + formatName: SensorFormats.Calibrated.rawValue, + unitName: SensorUnits.degreescelcius.rawValue, + value: temperatureC) + + objectCluster.addData(sensorName: PressureTempSensor.PRESSURE, + formatName: SensorFormats.Calibrated.rawValue, + unitName: SensorUnits.kpascal.rawValue, + value: pressurekPa) + + + print("RawT=\(rawT) RawP=\(rawP)") + print("Current temperature: \(temperatureC)") + print("Current pressure: \(pressurekPa)") + return objectCluster } - var p = 1048576.0 - adc_P; - p = (p - (var2 / 4096.0)) * 6250.0 / var1; - var1 = PressureTempSensor.DIG_P9 * p * p / 2147483648.0; - var2 = p * PressureTempSensor.DIG_P8 / 32768.0; - p = p + (var1 + var2 + PressureTempSensor.DIG_P7) / 16.0; - let calTemperature = T - let calPressure = p + else { + let temperature = Array(sensorPacket[packetIndexTemp.. UInt16 { + // ConcatenateBytes(high, low) style + return (UInt16(b[i+1]) << 8) | UInt16(b[i]) + } + func s16(_ i: Int) -> Int16 { + return Int16(bitPattern: u16(i)) + } + func s8(_ i: Int) -> Int8 { + return Int8(bitPattern: b[i]) + } + + // Raw register values (same indices as C# / Java) + let regT1 = u16(0) + let regT2 = s16(2) + let regT3 = s8(4) + + let regP1 = s16(5) + let regP2 = s16(7) + let regP3 = s8(9) + let regP4 = s8(10) + let regP5 = u16(11) + let regP6 = u16(13) + let regP7 = s8(15) + let regP8 = s8(16) + let regP9 = s16(17) + let regP10 = s8(19) + let regP11 = s8(20) + + // Quantized conversions — EXACT C#/Java scaling + + // 1 / 2^8 + par_T1 = Double(regT1) / 0.00390625 + // 2^30 + par_T2 = Double(regT2) / 1073741824.0 + // 2^48 + par_T3 = Double(regT3) / 281474976710656.0 + + // 2^20 + par_P1 = (Double(regP1) - 16384.0) / 1048576.0 + // 2^29 + par_P2 = (Double(regP2) - 16384.0) / 536870912.0 + // 2^32 + par_P3 = Double(regP3) / 4294967296.0 + // 2^37 + par_P4 = Double(regP4) / 137438953472.0 + + // 2^-3 + par_P5 = Double(regP5) / 0.125 + // 2^6 + par_P6 = Double(regP6) / 64.0 + // 2^8 + par_P7 = Double(regP7) / 256.0 + // 2^15 + par_P8 = Double(regP8) / 32768.0 + + // 2^48 + par_P9 = Double(regP9) / 281474976710656.0 + par_P10 = Double(regP10) / 281474976710656.0 + // 2^65 + par_P11 = Double(regP11) / 36893488147419103232.0 + + print("par_T1 = \(par_T1)") + print("par_T2 = \(par_T2)") + print("par_T3 = \(par_T3)") + print("par_P1 = \(par_P1)") + print("par_P2 = \(par_P2)") + print("par_P3 = \(par_P3)") + print("par_P4 = \(par_P4)") + print("par_P5 = \(par_P5)") + print("par_P6 = \(par_P6)") + print("par_P7 = \(par_P7)") + print("par_P8 = \(par_P8)") + print("par_P9 = \(par_P9)") + print("par_P10 = \(par_P10)") + print("par_P11 = \(par_P11)") + + return + return + } else { + DIG_T1 = Double((Int(pressureResoRes[0]) + (Int(pressureResoRes[1]) << 8))) + DIG_T2 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[2]) + (Int(pressureResoRes[3]) << 8))), bitLength: 16)) + DIG_T3 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[4]) + (Int(pressureResoRes[5]) << 8))), bitLength: 16)) + DIG_P1 = Double((Int(pressureResoRes[6]) + (Int(pressureResoRes[7]) << 8))) + DIG_P2 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[8]) + (Int(pressureResoRes[9]) << 8))), bitLength: 16)) + DIG_P3 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[10]) + (Int(pressureResoRes[11]) << 8))), bitLength: 16)) + DIG_P4 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[12]) + (Int(pressureResoRes[13]) << 8))), bitLength: 16)) + DIG_P5 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[14]) + (Int(pressureResoRes[15]) << 8))), bitLength: 16)) + DIG_P6 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[16]) + (Int(pressureResoRes[17]) << 8))), bitLength: 16)) + DIG_P7 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[18]) + (Int(pressureResoRes[19]) << 8))), bitLength: 16)) + DIG_P8 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[20]) + (Int(pressureResoRes[21]) << 8))), bitLength: 16)) + DIG_P9 = Double(ShimmerUtilities.calculateTwosComplement(signedData: Int((Int(pressureResoRes[22]) + (Int(pressureResoRes[23]) << 8))), bitLength: 16)) + } } public func updateInfoMemPressureResolution(infomem: [UInt8],res: Resolution) -> [UInt8]{ @@ -171,4 +348,110 @@ public class PressureTempSensor: Sensor , SensorProcessing{ } } + @inline(__always) + func u24(_ b0: UInt8, _ b1: UInt8, _ b2: UInt8) -> Int32 { + return Int32(b2) << 16 | Int32(b1) << 8 | Int32(b0) + } + + @inline(__always) + private func u16(_ b: [UInt8], _ i: Int) -> UInt16 { + return UInt16(b[i]) | (UInt16(b[i+1]) << 8) + } + + @inline(__always) + private func s16(_ b: [UInt8], _ i: Int) -> Int16 { + return Int16(bitPattern: u16(b, i)) + } + + @inline(__always) + private func u8(_ b: [UInt8], _ i: Int) -> UInt8 { + return b[i] + } + + @inline(__always) + private func s8(_ b: [UInt8], _ i: Int) -> Int8 { + return Int8(bitPattern: b[i]) + } + + func compensateBMP390_T(rawT: Int32) -> Double { + + let uncompTemp = Double(rawT) + + let partial1 = uncompTemp - par_T1 + let partial2 = partial1 * par_T2 + + quantized_TLin = partial2 + partial1 * partial1 * par_T3 + + // clamp like Java + quantized_TLin = max(-40.0, min(85.0, quantized_TLin)) + + return quantized_TLin + } + + + func compensateBMP390_P(rawP: Int32) -> Double { + + let UP = Double(rawP) + let T = quantized_TLin + + // Direct port of Java: + let part1 = par_P6 * T + let part2 = par_P7 * (T * T) + let part3 = par_P8 * (T * T * T) + let out1 = par_P5 + part1 + part2 + part3 + + let part4 = par_P2 * T + let part5 = par_P3 * (T * T) + let part6 = par_P4 * (T * T * T) + let out2 = UP * (par_P1 + part4 + part5 + part6) + + let up2 = UP * UP + let part7 = par_P9 + par_P10 * T + let part8 = up2 * part7 + let part9 = (UP * UP * UP) * par_P11 + + var pressure = out1 + out2 + part8 + part9 + + // clamp like Java + pressure = max(30000.0, min(125000.0, pressure)) + + return pressure + } + + func calibratePressure390(UP: Double, UT: Double) -> (Double, Double) { + + var partialT1 = UT - par_T1 + var partialT2 = partialT1 * par_T2 + + quantized_TLin = partialT2 + (partialT1 * partialT1 * par_T3) + + // clamp + if quantized_TLin < -40 { quantized_TLin = -40 } + if quantized_TLin > 85 { quantized_TLin = 85 } + + let T = quantized_TLin + + // pressure + let p1 = par_P6 * T + let p2 = par_P7 * (T * T) + let p3 = par_P8 * (T * T * T) + let out1 = par_P5 + p1 + p2 + p3 + + let q1 = par_P2 * T + let q2 = par_P3 * (T * T) + let q3 = par_P4 * (T * T * T) + let out2 = UP * (par_P1 + q1 + q2 + q3) + + let up2 = UP * UP + let r = up2 * (par_P9 + par_P10 * T) + let s = (UP * UP * UP) * par_P11 + + var pressure = out1 + out2 + r + s + + pressure = max(30000.0, min(125000.0, pressure)) + + return (pressure, T) + } + + } diff --git a/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift b/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift index d052460..3ac562c 100644 --- a/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift +++ b/ShimmerBluetooth/ShimmerBluetooth/Shimmer3Protocol.swift @@ -331,6 +331,7 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { wrAccelSensor = WRAccelSensor(hwid: REV_HW_MAJOR) timeSensor = TimeSensor() altMagSensor = AltMagSensor(hwid: REV_HW_MAJOR) + pressureTempSensor = PressureTempSensor(hwid: REV_HW_MAJOR) } } @@ -359,6 +360,7 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { magSensor.setInfoMem(infomem: infoMem) gyroSensor.setInfoMem(infomem: infoMem) wrAccelSensor.setInfoMem(infomem: infoMem) + pressureTempSensor.setInfoMom(infomem: infoMem) } } @@ -824,10 +826,28 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { } print("Command ACK Received and Processed: \(self.commandSent!)") //self.receivedBytes.removeAll() - - + } else if (self.commandSent == PacketTypeShimmer.getpressureCalibrationCoefficientsCommand) { + if self.receivedBytes[1] == PacketTypeShimmer.pressureCalibrationCoefficientsResponse.rawValue { + + // Remove ACK + response type + var data = self.receivedBytes + data.removeFirst(2) + + // Remove CRC + if self.CRCMode != .OFF { + data.removeLast(Int(self.CRCMode.rawValue)) + } + + // Forward to same PressureTempSensor + self.pressureTempSensor.parseCalParamByteArray(pressureResoRes: data) + + self.continuation?.resume(returning: true) + self.continuation = nil + self.receivedBytes.removeAll() + } } + } } @@ -991,9 +1011,11 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { packetSize += 2 enabledSensors |= Int(SensorBitmapShimmer3.SENSOR_EXT_A7.rawValue) case ChannelContentsShimmer3.Temperature.rawValue: - packetSize += 2 + pressureTempSensor.packetIndexTemp = packetSize + packetSize += 3 enabledSensors |= Int(SensorBitmapShimmer3.SENSOR_BMP180_PRESSURE.rawValue) case ChannelContentsShimmer3.Pressure.rawValue: + pressureTempSensor.packetIndexPressure = packetSize packetSize += 3 enabledSensors |= Int(SensorBitmapShimmer3.SENSOR_BMP180_PRESSURE.rawValue) default: @@ -1277,9 +1299,17 @@ public class Shimmer3Protocol : NSObject, ShimmerProtocol { } private func sendPressureCalibCoefficientsCommand() async -> Bool?{ - return true; + let bytes: [UInt8] = [PacketTypeShimmer.getpressureCalibrationCoefficientsCommand.rawValue] + commandSent = PacketTypeShimmer.getpressureCalibrationCoefficientsCommand + radio!.writeBytes(bytes: bytes) + return await withCheckedContinuation { continuation in + if self.continuation == nil { + self.continuation = continuation + } + } } + public func sendSetSamplingRateCommand(samplingRate: Double) async -> Bool?{ var samplingByteValue = (Int)(32768/samplingRate) var bytes = [UInt8]()