IoT.Devices.Qmp6988.cs

using System;
using System.Device.I2c;
using System.Threading;

namespace IoT.Devices.Qmp6988
{
    internal class Qmp6988
    {
        internal PressureResolution PressureResolution { get; set; } = PressureResolution.High;

        private PowerMode PowerMode = PowerMode.Sleep;
        private CalibrationData Qmp6988Cali = new();
        private IkData Qmp6988Ik = new();
        private I2cDevice I2c;

        public Qmp6988(I2cDevice device)
        {
            this.I2c = device;

            // 初期化
            SoftwareReset();
            GetCalibrationData();
        }
        internal void SetPowerMode(PowerMode powerMode)
        {
            // 解像度やパワーモードを設定
            var data = this.ReadToRegister(Register.CTRL_MEAS, 1);
            this.PowerMode = powerMode;
            this.WriteToRegister(Register.CTRL_MEAS, (byte)((byte)data[0] | (byte)TempResolution.Low | (byte)PressureResolution | (byte)PowerMode));
        }

        internal float CalcPressureHectopascals()
        {
            return this.CalcPressure() / 100;
        }
        internal float CalcPressure()
        {

            // データを取得
            var data = this.ReadToPressure(Register.PRESS_TXD);
            var pRaw = (int)(System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(data) - Math.Pow(2, 23));
            data = this.ReadToPressure(Register.TEMP_TXD);
            var tRaw = (int)(System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(data) - Math.Pow(2, 23));

            // 温度補正
            var tInt = ConvTx02e(tRaw);
            var pInt = Pressure02e(pRaw, tInt);

            return (float)pInt / 16.0f;
        }

        private void WriteToRegister(Register register, byte value)
        {
            SpanByte writeBuff = new byte[]
            {
                (byte)register, (byte)value
            };
            this.I2c.Write(writeBuff);
        }

        private SpanByte ReadToRegister(Register register, int valueSize)
        {
            var result = new byte[valueSize];

            SpanByte writeBuff = new byte[]
            {
                    (byte)register,
            };
            SpanByte readBuff = new byte[valueSize];
            this.I2c.Write(writeBuff);
            Thread.Sleep(20);
            this.I2c.Read(readBuff);
            result = readBuff.ToArray();

            return result;
        }
        private SpanByte ReadToPressure(Register register)
        {
            var result = new byte[4];

            // Normalモード以外でセンサー値を要求されたときは、Forceモードで１度だけ取得する
            var data = this.ReadToRegister(Register.CTRL_MEAS, 1);
            if (((byte)data[0] & (byte)PowerMode.Normal) != (byte)PowerMode.Normal)
            {
                if (this.PowerMode == PowerMode.Normal)
                {
                    this.WriteToRegister(Register.CTRL_MEAS, (byte)((byte)data[0] | (byte)TempResolution.Low | (byte)PressureResolution | (byte)PowerMode.Normal));
                }
                else
                {
                    this.WriteToRegister(Register.CTRL_MEAS, (byte)((byte)data[0] | (byte)TempResolution.Low | (byte)PressureResolution | (byte)PowerMode.Force));
                }
                Thread.Sleep(20);
            }

            // Normalモードならば定期的にレジスタ更新、それ以外ならばForceモードでメソッド呼び出し時にレジスタに更新
            SpanByte writeBuff = new byte[]
            {
                    (byte)register,
            };
            SpanByte readBuff = new byte[4];
            this.I2c.Write(writeBuff);
            Thread.Sleep(20);
            this.I2c.Read(readBuff.Slice(1));
            result = readBuff.ToArray();

            return result;
        }
        private short ConvTx02e(int dt)
        {
            short ret;
            long wk1, wk2;

            // wk1: 60Q4 // bit size
            wk1 = ((long)this.Qmp6988Ik.a1 * (long)dt); // 31Q23+24-1=54 (54Q23)
            wk2 = ((long)this.Qmp6988Ik.a2 * (long)dt) >> 14; // 30Q47+24-1=53 (39Q33)
            wk2 = (wk2 * (long)dt) >> 10; // 39Q33+24-1=62 (52Q23)
            wk2 = ((wk1 + wk2) / 32767) >> 19; // 54,52->55Q23 (20Q04)
            ret = (short)((this.Qmp6988Ik.a0 + wk2) >> 4); // 21Q4 -> 17Q0
            return ret;
        }

        private int Pressure02e(int dp, short tx)
        {
            int ret;
            long wk1, wk2, wk3;

            // wk1 = 48Q16 // bit size
            wk1 = ((long)this.Qmp6988Ik.bt1 * (long)tx); // 28Q15+16-1=43 (43Q15)
            wk2 = ((long)this.Qmp6988Ik.bp1 * (long)dp) >> 5; // 31Q20+24-1=54 (49Q15)
            wk1 += wk2; // 43,49->50Q15
            wk2 = ((long)this.Qmp6988Ik.bt2 * (long)tx) >> 1; // 34Q38+16-1=49 (48Q37)
            wk2 = (wk2 * (long)tx) >> 8; // 48Q37+16-1=63 (55Q29)
            wk3 = wk2; // 55Q29
            wk2 = ((long)this.Qmp6988Ik.b11 * (long)tx) >> 4; // 28Q34+16-1=43 (39Q30)
            wk2 = (wk2 * (long)dp) >> 1; // 39Q30+24-1=62 (61Q29)
            wk3 += wk2; // 55,61->62Q29
            wk2 = ((long)this.Qmp6988Ik.bp2 * (long)dp) >> 13; // 29Q43+24-1=52 (39Q30)
            wk2 = (wk2 * (long)dp) >> 1; // 39Q30+24-1=62 (61Q29)
            wk3 += wk2; // 62,61->63Q29
            wk1 += wk3 >> 14; // Q29 >> 14 -> Q15
            wk2 = ((long)this.Qmp6988Ik.b12 * (long)tx); // 29Q53+16-1=45 (45Q53)
            wk2 = (wk2 * (long)tx) >> 22; // 45Q53+16-1=61 (39Q31)
            wk2 = (wk2 * (long)dp) >> 1; // 39Q31+24-1=62 (61Q30)
            wk3 = wk2; // 61Q30
            wk2 = ((long)this.Qmp6988Ik.b21 * (long)tx) >> 6; // 29Q60+16-1=45 (39Q54)
            wk2 = (wk2 * (long)dp) >> 23; // 39Q54+24-1=62 (39Q31)
            wk2 = (wk2 * (long)dp) >> 1; // 39Q31+24-1=62 (61Q20)
            wk3 += wk2; // 61,61->62Q30
            wk2 = ((long)this.Qmp6988Ik.bp3 * (long)dp) >> 12; // 28Q65+24-1=51 (39Q53)
            wk2 = (wk2 * (long)dp) >> 23; // 39Q53+24-1=62 (39Q30)
            wk2 = (wk2 * (long)dp); // 39Q30+24-1=62 (62Q30)
            wk3 += wk2; // 62,62->63Q30
            wk1 += wk3 >> 15; // Q30 >> 15 = Q15
            wk1 /= 32767L;
            wk1 >>= 11; // Q15 >> 7 = Q4
            wk1 += this.Qmp6988Ik.b00; // Q4 + 20Q4
                            //wk1 >>= 4; // 28Q4 -> 24Q0
            ret = (int)wk1;
            return ret;
        }

        private void SoftwareReset()
        {
            WriteToRegister(Register.RESET, 0xe6);
            Thread.Sleep(20);
            WriteToRegister(Register.RESET, 0x00);
        }

        private void GetCalibrationData()
        {
            var data = this.ReadToRegister(Register.COE_b00, 25);

            this.Qmp6988Cali.COE_a0 = (int)(((data[18] << 12) | (data[19] << 4) | (data[24] & 0x0f)) << 12);
            this.Qmp6988Cali.COE_a0 = this.Qmp6988Cali.COE_a0 >> 12;

            this.Qmp6988Cali.COE_a1 = (short)(((data[20]) << 8) | data[21]);
            this.Qmp6988Cali.COE_a2 = (short)(((data[22]) << 8) | data[23]);

            this.Qmp6988Cali.COE_b00 = (int)(((data[0] << 12) | (data[1] << 4) | ((data[24] & 0xf0) >> 4)) << 12);
            this.Qmp6988Cali.COE_b00 = this.Qmp6988Cali.COE_b00 >> 12;

            this.Qmp6988Cali.COE_bt1 = (short)(((data[2]) << 8) | data[3]);
            this.Qmp6988Cali.COE_bt2 = (short)(((data[4]) << 8) | data[5]);
            this.Qmp6988Cali.COE_bp1 = (short)(((data[6]) << 8) | data[7]);
            this.Qmp6988Cali.COE_b11 = (short)(((data[8]) << 8) | data[9]);
            this.Qmp6988Cali.COE_bp2 = (short)(((data[10]) << 8) | data[11]);
            this.Qmp6988Cali.COE_b12 = (short)(((data[12]) << 8) | data[13]);
            this.Qmp6988Cali.COE_b21 = (short)(((data[14]) << 8) | data[15]);
            this.Qmp6988Cali.COE_bp3 = (short)(((data[16]) << 8) | data[17]);

            this.Qmp6988Ik.a0 = this.Qmp6988Cali.COE_a0; // 20Q4
            this.Qmp6988Ik.b00 = this.Qmp6988Cali.COE_b00; // 20Q4

            this.Qmp6988Ik.a1 = (int)(3608L * (int)this.Qmp6988Cali.COE_a1 - 1731677965L); // 31Q23
            this.Qmp6988Ik.a2 = (int)(16889L * (int)this.Qmp6988Cali.COE_a2 - 87619360L); // 30Q47

            this.Qmp6988Ik.bt1 = 2982L * (long)this.Qmp6988Cali.COE_bt1 + 107370906L; // 28Q15
            this.Qmp6988Ik.bt2 = 329854L * (long)this.Qmp6988Cali.COE_bt2 + 108083093L; // 34Q38
            this.Qmp6988Ik.bp1 = 19923L * (long)this.Qmp6988Cali.COE_bp1 + 1133836764L; // 31Q20
            this.Qmp6988Ik.b11 = 2406L * (long)this.Qmp6988Cali.COE_b11 + 118215883L; // 28Q34
            this.Qmp6988Ik.bp2 = 3079L * (long)this.Qmp6988Cali.COE_bp2 - 181579595L; // 29Q43
            this.Qmp6988Ik.b12 = 6846L * (long)this.Qmp6988Cali.COE_b12 + 85590281L; // 29Q53
            this.Qmp6988Ik.b21 = 13836L * (long)this.Qmp6988Cali.COE_b21 + 79333336L; // 29Q60
            this.Qmp6988Ik.bp3 = 2915L * (long)this.Qmp6988Cali.COE_bp3 + 157155561L; // 28Q65
        }

        // CalibrationData
        private class CalibrationData
        {
            public int COE_a0;
            public short COE_a1;
            public short COE_a2;
            public int COE_b00;
            public short COE_bt1;
            public short COE_bt2;
            public short COE_bp1;
            public short COE_b11;
            public short COE_bp2;
            public short COE_b12;
            public short COE_b21;
            public short COE_bp3;
        }
        private class IkData
        {
            public int a0, b00;
            public int a1, a2;
            public long bt1, bt2, bp1, b11, bp2, b12, b21, bp3;
        }
    }

    internal enum PowerMode : byte
    {
        Sleep = 0b00,
        Force = 0b01,
        Normal = 0b11,
    }

    internal enum PressureResolution : byte
    {
        Skip = 0b00000,
        Low = 0b00100,
        Middle = 0b01000,
        High = 0b10000,
    }

    internal enum TempResolution : byte
    {
        Skip = 0b00000000,
        Low = 0b00100000,
        Middle = 0b01000000,
        High = 0b10000000,
    }

    internal enum Register : byte
    {
        CTRL_MEAS = 0xF4,
        PRESS_TXD = 0xF7,
        TEMP_TXD = 0xFA,
        IO_SETUP = 0xF5,
        COE_b00 = 0xA0,
        RESET = 0xE0,
    }
}