mpu6886.py

   1 # C.LEBOCQ 02/2020
   2 # MicroPython library for the MPU6886 imu ( M5StickC / ATOM Matrix )
   3 # Based on https://github.com/m5stack/M5StickC/blob/master/src/utility/MPU6886.cpp
   4
   5 from machine import I2C
   6 from time import sleep
   7
   8 MPU6886_ADDRESS           = const(0x68)
   9 MPU6886_WHOAMI            = const(0x75)
  10 MPU6886_ACCEL_INTEL_CTRL  = const(0x69)
  11 MPU6886_SMPLRT_DIV        = const(0x19)
  12 MPU6886_INT_PIN_CFG       = const(0x37)
  13 MPU6886_INT_ENABLE        = const(0x38)
  14 MPU6886_ACCEL_XOUT_H      = const(0x3B)
  15 MPU6886_ACCEL_XOUT_L      = const(0x3C)
  16 MPU6886_ACCEL_YOUT_H      = const(0x3D)
  17 MPU6886_ACCEL_YOUT_L      = const(0x3E)
  18 MPU6886_ACCEL_ZOUT_H      = const(0x3F)
  19 MPU6886_ACCEL_ZOUT_L      = const(0x40)
  20
  21 MPU6886_TEMP_OUT_H        = const(0x41)
  22 MPU6886_TEMP_OUT_L        = const(0x42)
  23
  24 MPU6886_GYRO_XOUT_H       = const(0x43)
  25 MPU6886_GYRO_XOUT_L       = const(0x44)
  26 MPU6886_GYRO_YOUT_H       = const(0x45)
  27 MPU6886_GYRO_YOUT_L       = const(0x46)
  28 MPU6886_GYRO_ZOUT_H       = const(0x47)
  29 MPU6886_GYRO_ZOUT_L       = const(0x48)
  30
  31 MPU6886_USER_CTRL         = const(0x6A)
  32 MPU6886_PWR_MGMT_1        = const(0x6B)
  33 MPU6886_PWR_MGMT_2        = const(0x6C)
  34 MPU6886_CONFIG            = const(0x1A)
  35 MPU6886_GYRO_CONFIG       = const(0x1B)
  36 MPU6886_ACCEL_CONFIG      = const(0x1C)
  37 MPU6886_ACCEL_CONFIG2     = const(0x1D)
  38 MPU6886_FIFO_EN           = const(0x23)
  39
  40 #consts for Acceleration & Resolution scale
  41 AFS_2G      = const(0x00)
  42 AFS_4G      = const(0x01)
  43 AFS_8G      = const(0x02)
  44 AFS_16G     = const(0x03)
  45
  46 GFS_250DPS  = const(0x00)
  47 GFS_500DPS  = const(0x01)
  48 GFS_1000DPS = const(0x02)
  49 GFS_2000DPS = const(0x03)
  50
  51 class MPU6886():
  52
  53     def __init__(self, i2c, Gscale = GFS_2000DPS, Ascale = AFS_8G):
  54         self.i2c = i2c
  55         self.Gscale = Gscale
  56         self.Ascale = Ascale
  57         if self.init():
  58             self.setAccelFsr(Ascale)
  59             self.setGyroFsr(Gscale)
  60
  61     # sleep in ms
  62     def sleepms(self,n):
  63         sleep(n / 1000)
  64
  65     # set I2C reg (1 byte)
  66     def setReg(self, reg, dat):
  67         self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg, dat]))
  68
  69     # get I2C reg (1 byte)
  70     def getReg(self, reg):
  71         self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg]))
  72         t =     self.i2c.readfrom(MPU6886_ADDRESS, 1)
  73         return t[0]
  74
  75     # get n reg
  76     def getnReg(self, reg, n):
  77         self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg]))
  78         t =     self.i2c.readfrom(MPU6886_ADDRESS, n)
  79         return t
  80
  81     def init(self):
  82         tempdata = self.getReg(MPU6886_WHOAMI)
  83         if tempdata != 0x19:
  84             return False
  85         self.sleepms(1)
  86         regdata = 0x00
  87         self.setReg(MPU6886_PWR_MGMT_1, regdata)
  88         self.sleepms(10)
  89         regdata = (0x01<<7)
  90         self.setReg(MPU6886_PWR_MGMT_1, regdata)
  91         self.sleepms(10)
  92         regdata = (0x01<<0)
  93         self.setReg(MPU6886_PWR_MGMT_1, regdata)
  94         self.sleepms(10)
  95         regdata = 0x10
  96         self.setReg(MPU6886_ACCEL_CONFIG, regdata)
  97         self.sleepms(1)
  98         regdata = 0x18
  99         self.setReg(MPU6886_GYRO_CONFIG, regdata)
 100         self.sleepms(1)
 101         regdata = 0x01
 102         self.setReg(MPU6886_CONFIG, regdata)
 103         self.sleepms(1)
 104         regdata = 0x05
 105         self.setReg(MPU6886_SMPLRT_DIV, regdata)
 106         self.sleepms(1)
 107         regdata = 0x00
 108         self.setReg(MPU6886_INT_ENABLE, regdata)
 109         self.sleepms(1)
 110         regdata = 0x00
 111         self.setReg(MPU6886_ACCEL_CONFIG2, regdata)
 112         self.sleepms(1)
 113         regdata = 0x00
 114         self.setReg(MPU6886_USER_CTRL, regdata)
 115         self.sleepms(1)
 116         regdata = 0x00
 117         self.setReg(MPU6886_FIFO_EN, regdata)
 118         self.sleepms(1)
 119         regdata = 0x22
 120         self.setReg(MPU6886_INT_PIN_CFG, regdata)
 121         self.sleepms(1)
 122         regdata = 0x01
 123         self.setReg(MPU6886_INT_ENABLE, regdata)
 124         self.sleepms(100)
 125         self.getGres()
 126         self.getAres()
 127         return True
 128
 129     def getGres(self):
 130         if self.Gscale == GFS_250DPS:
 131             self.gRes = 250.0 / 32768.0
 132         elif self.Gscale == GFS_500DPS:
 133             self.gRes = 500.0/32768.0
 134         elif self.Gscale == GFS_1000DPS:
 135             self.gRes = 1000.0/32768.0
 136         elif self.Gscale == GFS_2000DPS:
 137             self.gRes = 2000.0/32768.0
 138         else:
 139             self.gRes = 250.0/32768.0
 140
 141     def getAres(self):
 142         if self.Ascale == AFS_2G:
 143             self.aRes = 2.0/32768.0
 144         elif self.Ascale == AFS_4G:
 145             self.aRes = 4.0/32768.0
 146         elif self.Ascale == AFS_8G:
 147             self.aRes = 8.0/32768.0
 148         elif self.Ascale == AFS_16G:
 149             self.aRes = 16.0/32768.0
 150         else:
 151             self.aRes = 2.0/32768.0
 152
 153     def getAccelAdc(self):
 154         buf = self.getnReg(MPU6886_ACCEL_XOUT_H,6)
 155
 156         ax = (buf[0]<<8) | buf[1]
 157         ay = (buf[2]<<8) | buf[3]
 158         az = (buf[4]<<8) | buf[5]
 159         return ax,ay,az
 160
 161     def getAccelData(self):
 162         ax,ay,az = self.getAccelAdc()
 163         if ax > 32768:
 164             ax -= 65536
 165         if ay > 32768:
 166             ay -= 65536
 167         if az > 32768:
 168             az -= 65536
 169         ax *=  self.aRes
 170         ay *=  self.aRes
 171         az *=  self.aRes
 172         return ax,ay,az
 173
 174     def getGyroAdc(self):
 175         buf = self.getnReg(MPU6886_GYRO_XOUT_H,6)
 176         gx = (buf[0]<<8) | buf[1]
 177         gy = (buf[2]<<8) | buf[3]
 178         gz = (buf[4]<<8) | buf[5]
 179         return gx,gy,gz
 180
 181     def getGyroData(self):
 182         gx,gy,gz = self.getGyroAdc()
 183         if gx > 32768:
 184             gx -= 65536
 185         if gy > 32768:
 186             gy -= 65536
 187         if gz > 32768:
 188             gz -= 65536
 189         gx *= self.gRes
 190         gy *= self.gRes
 191         gz *= self.gRes
 192         return gx, gy, gz
 193
 194     def getTempAdc(self):
 195         buf = self.getnReg(MPU6886_TEMP_OUT_H,2)
 196         return (buf[0]<<8) | buf[1]
 197
 198     def getTempData(self):
 199         return self.getTempAdc() / 326.8 + 25.0
 200
 201     def setGyroFsr(self,scale):
 202         regdata = (scale<<3)
 203         self.setReg(MPU6886_GYRO_CONFIG, regdata)
 204         self.sleepms(10)
 205         self.Gscale = scale
 206         self.getGres()
 207
 208     def setAccelFsr(self,scale):
 209         regdata = (scale<<3)
 210         self.setReg(MPU6886_ACCEL_CONFIG, regdata)
 211         self.sleepms(10)
 212         self.Ascale = scale
 213         self.getAres()