|
| 1 | +import struct |
| 2 | +import time |
| 3 | + |
| 4 | +from machine import I2C |
| 5 | + |
| 6 | + |
| 7 | +# Sensor constants |
| 8 | +_QMI8685_PARTID = const(0x05) |
| 9 | +_REG_PARTID = const(0x00) |
| 10 | +_REG_REVISION = const(0x01) |
| 11 | + |
| 12 | +_REG_CTRL1 = const(0x02) # Serial interface and sensor enable |
| 13 | +_REG_CTRL2 = const(0x03) # Accelerometer settings |
| 14 | +_REG_CTRL3 = const(0x04) # Gyroscope settings |
| 15 | +_REG_CTRL4 = const(0x05) # Magnetomer settings (support not implemented in this driver yet) |
| 16 | +_REG_CTRL5 = const(0x06) # Sensor data processing settings |
| 17 | +_REG_CTRL6 = const(0x07) # Attitude Engine ODR and Motion on Demand |
| 18 | +_REG_CTRL7 = const(0x08) # Enable Sensors and Configure Data Reads |
| 19 | + |
| 20 | +_REG_TEMP = const(0x33) # Temperature sensor. |
| 21 | + |
| 22 | +_REG_AX_L = const(0x35) # Read accelerometer |
| 23 | +_REG_AX_H = const(0x36) |
| 24 | +_REG_AY_L = const(0x37) |
| 25 | +_REG_AY_H = const(0x38) |
| 26 | +_REG_AZ_L = const(0x39) |
| 27 | +_REG_AZ_H = const(0x3A) |
| 28 | + |
| 29 | +_REG_GX_L = const(0x3B) # read gyro |
| 30 | +_REG_GX_H = const(0x3C) |
| 31 | +_REG_GY_L = const(0x3D) |
| 32 | +_REG_GY_H = const(0x3E) |
| 33 | +_REG_GZ_L = const(0x3F) |
| 34 | +_REG_GZ_H = const(0x40) |
| 35 | + |
| 36 | +_QMI8658_I2CADDR_DEFAULT = const(0X6B) |
| 37 | + |
| 38 | + |
| 39 | +_ACCELSCALE_RANGE_2G = const(0b00) |
| 40 | +_ACCELSCALE_RANGE_4G = const(0b01) |
| 41 | +_ACCELSCALE_RANGE_8G = const(0b10) |
| 42 | +_ACCELSCALE_RANGE_16G = const(0b11) |
| 43 | + |
| 44 | +_GYROSCALE_RANGE_16DPS = const(0b000) |
| 45 | +_GYROSCALE_RANGE_32DPS = const(0b001) |
| 46 | +_GYROSCALE_RANGE_64DPS = const(0b010) |
| 47 | +_GYROSCALE_RANGE_128DPS = const(0b011) |
| 48 | +_GYROSCALE_RANGE_256DPS = const(0b100) |
| 49 | +_GYROSCALE_RANGE_512DPS = const(0b101) |
| 50 | +_GYROSCALE_RANGE_1024DPS = const(0b110) |
| 51 | +_GYROSCALE_RANGE_2048DPS = const(0b111) |
| 52 | + |
| 53 | +_ODR_8000HZ = const(0b0000) |
| 54 | +_ODR_4000HZ = const(0b0001) |
| 55 | +_ODR_2000HZ = const(0b0010) |
| 56 | +_ODR_1000HZ = const(0b0011) |
| 57 | +_ODR_500HZ = const(0b0100) |
| 58 | +_ODR_250HZ = const(0b0101) |
| 59 | +_ODR_125HZ = const(0b0110) |
| 60 | +_ODR_62_5HZ = const(0b0111) |
| 61 | + |
| 62 | + |
| 63 | +class QMI8658: |
| 64 | + global _QMI8658_I2CADDR_DEFAULT |
| 65 | + def __init__(self,i2c_bus: I2C,address: int = _QMI8658_I2CADDR_DEFAULT,accel_scale: int = _ACCELSCALE_RANGE_8G,gyro_scale: int = _GYROSCALE_RANGE_256DPS): |
| 66 | + self.i2c = i2c_bus |
| 67 | + self.address = address |
| 68 | + # Verify sensor part ID |
| 69 | + if self._read_u8(_REG_PARTID) != _QMI8685_PARTID: |
| 70 | + raise AttributeError("Cannot find a QMI8658") |
| 71 | + # Setup initial configuration |
| 72 | + self._configure_sensor(accel_scale, gyro_scale) |
| 73 | + # Configure scales/divisors for the driver |
| 74 | + self.acc_scale_divisor = { |
| 75 | + _ACCELSCALE_RANGE_2G: 1 << 14, |
| 76 | + _ACCELSCALE_RANGE_4G: 1 << 13, |
| 77 | + _ACCELSCALE_RANGE_8G: 1 << 12, |
| 78 | + _ACCELSCALE_RANGE_16G: 1 << 11, |
| 79 | + }[accel_scale] |
| 80 | + self.gyro_scale_divisor = { |
| 81 | + _GYROSCALE_RANGE_16DPS: 2048, |
| 82 | + _GYROSCALE_RANGE_32DPS: 1024, |
| 83 | + _GYROSCALE_RANGE_64DPS: 512, |
| 84 | + _GYROSCALE_RANGE_128DPS: 256, |
| 85 | + _GYROSCALE_RANGE_256DPS: 128, |
| 86 | + _GYROSCALE_RANGE_512DPS: 64, |
| 87 | + _GYROSCALE_RANGE_1024DPS: 32, |
| 88 | + _GYROSCALE_RANGE_2048DPS: 16, |
| 89 | + }[gyro_scale] |
| 90 | + |
| 91 | + |
| 92 | + def _configure_sensor(self, accel_scale: int, gyro_scale: int): |
| 93 | + # Initialize accelerometer and gyroscope settings |
| 94 | + self._write_u8(_REG_CTRL1, 0x60) # Set SPI auto increment and big endian (Ctrl 1) |
| 95 | + self._write_u8(_REG_CTRL2, (accel_scale << 4) | _ODR_1000HZ) # Accel Config |
| 96 | + self._write_u8(_REG_CTRL3, (gyro_scale << 4) | _ODR_1000HZ) # Gyro Config |
| 97 | + self._write_u8(_REG_CTRL5, 0x01) # Low-pass filter enable |
| 98 | + self._write_u8(_REG_CTRL7, 0x03) # Enable accel and gyro |
| 99 | + time.sleep_ms(100) |
| 100 | + |
| 101 | + |
| 102 | + # Helper functions for register operations |
| 103 | + def _read_u8(self, reg:int) -> int: |
| 104 | + return self.i2c.readfrom_mem(self.address, reg, 1)[0] |
| 105 | + |
| 106 | + def _read_xyz(self, reg:int) -> tuple[int, int, int]: |
| 107 | + data = self.i2c.readfrom_mem(self.address, reg, 6) |
| 108 | + return struct.unpack('<hhh', data) |
| 109 | + |
| 110 | + def _write_u8(self, reg: int, value: int): |
| 111 | + self.i2c.writeto_mem(self.address, reg, bytes([value])) |
| 112 | + |
| 113 | + |
| 114 | + @property |
| 115 | + def temperature(self) -> float: |
| 116 | + """Get the device temperature.""" |
| 117 | + temp_raw = self._read_u8(_REG_TEMP) |
| 118 | + return temp_raw / 256 |
| 119 | + |
| 120 | + @property |
| 121 | + def acceleration(self) -> tuple[float, float, float]: |
| 122 | + """Get current acceleration reading.""" |
| 123 | + raw_accel = self._read_xyz(_REG_AX_L) |
| 124 | + return tuple(val / self.acc_scale_divisor for val in raw_accel) |
| 125 | + |
| 126 | + @property |
| 127 | + def gyro(self) -> tuple[float, float, float]: |
| 128 | + """Get current gyroscope reading.""" |
| 129 | + raw_gyro = self._read_xyz(_REG_GX_L) |
| 130 | + return tuple(val / self.gyro_scale_divisor for val in raw_gyro) |
| 131 | + |
| 132 | + |
| 133 | +import machine |
| 134 | +sensor = QMI8658(I2C(0, sda=machine.Pin(48), scl=machine.Pin(47))) |
| 135 | +while True: |
| 136 | + print(f""" |
| 137 | +QMI8685 |
| 138 | +{sensor.temperature=} |
| 139 | +{sensor.acceleration=} |
| 140 | +{sensor.gyro=} |
| 141 | +""") |
| 142 | + time.sleep(1) |
0 commit comments