text: # Copyright (c) 2018-2020 Mika Tuupola
  #
  # Permission is hereby granted, free of charge, to any person obtaining a copy
  # of  this software and associated documentation files (the "Software"), to
  # deal in  the Software without restriction, including without limitation the
  # rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  # sell copied of the Software, and to permit persons to whom the Software is
  # furnished to do so, subject to the following conditions:
  #
  # The above copyright notice and this permission notice shall be included in
  # all copies or substantial portions of the Software.
  #
  # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  # SOFTWARE.
  
  # https://github.com/tuupola/micropython-mpu9250
  
  """
  MicroPython I2C driver for MPU6500 6-axis motion tracking device
  """
  
  __version__ = "0.3.0"
  
  # pylint: disable=import-error
  import ustruct
  import utime
  from machine import I2C, Pin
  from micropython import const
  
  # pylint: enable=import-error
  
  _GYRO_CONFIG = const(0x1b)
  _ACCEL_CONFIG = const(0x1c)
  _ACCEL_CONFIG2 = const(0x1d)
  _ACCEL_XOUT_H = const(0x3b)
  _ACCEL_XOUT_L = const(0x3c)
  _ACCEL_YOUT_H = const(0x3d)
  _ACCEL_YOUT_L = const(0x3e)
  _ACCEL_ZOUT_H = const(0x3f)
  _ACCEL_ZOUT_L = const(0x40)
  _TEMP_OUT_H = const(0x41)
  _TEMP_OUT_L = const(0x42)
  _GYRO_XOUT_H = const(0x43)
  _GYRO_XOUT_L = const(0x44)
  _GYRO_YOUT_H = const(0x45)
  _GYRO_YOUT_L = const(0x46)
  _GYRO_ZOUT_H = const(0x47)
  _GYRO_ZOUT_L = const(0x48)
  _WHO_AM_I = const(0x75)
  
  # _ACCEL_FS_MASK = const(0b00011000)
  ACCEL_FS_SEL_2G = const(0b00000000)
  ACCEL_FS_SEL_4G = const(0b00001000)
  ACCEL_FS_SEL_8G = const(0b00010000)
  ACCEL_FS_SEL_16G = const(0b00011000)
  
  _ACCEL_SO_2G = 16384  # 1 / 16384 ie. 0.061 mg / digit
  _ACCEL_SO_4G = 8192  # 1 / 8192 ie. 0.122 mg / digit
  _ACCEL_SO_8G = 4096  # 1 / 4096 ie. 0.244 mg / digit
  _ACCEL_SO_16G = 2048  # 1 / 2048 ie. 0.488 mg / digit
  
  # _GYRO_FS_MASK = const(0b00011000)
  GYRO_FS_SEL_250DPS = const(0b00000000)
  GYRO_FS_SEL_500DPS = const(0b00001000)
  GYRO_FS_SEL_1000DPS = const(0b00010000)
  GYRO_FS_SEL_2000DPS = const(0b00011000)
  
  _GYRO_SO_250DPS = 131
  _GYRO_SO_500DPS = 62.5
  _GYRO_SO_1000DPS = 32.8
  _GYRO_SO_2000DPS = 16.4
  
  _TEMP_SO = 333.87
  _TEMP_OFFSET = 21
  
  SF_G = 1
  SF_M_S2 = 9.80665  # 1 g = 9.80665 m/s2 ie. standard gravity
  SF_DEG_S = 1
  SF_RAD_S = 0.017453292519943  # 1 deg/s is 0.017453292519943 rad/s
  
  
  class MPU6500:
      """Class which provides interface to MPU6500 6-axis motion tracking device."""
  
      def __init__(
              self, i2c, address=0x68,
              accel_fs=ACCEL_FS_SEL_2G, gyro_fs=GYRO_FS_SEL_250DPS,
              accel_sf=SF_M_S2, gyro_sf=SF_RAD_S,
              gyro_offset=(0, 0, 0)
      ):
          self.i2c = i2c
          self.address = address
  
          # 0x70 = standalone MPU6500, 0x71 = MPU6250 SIP
          if self.whoami not in [0x71, 0x70]:
              raise RuntimeError("MPU6500 not found in I2C bus.")
  
          self._accel_so = self._accel_fs(accel_fs)
          self._gyro_so = self._gyro_fs(gyro_fs)
          self._accel_sf = accel_sf
          self._gyro_sf = gyro_sf
          self._gyro_offset = gyro_offset
  
      @property
      def acceleration(self):
          """
          Acceleration measured by the sensor. By default will return a
          3-tuple of X, Y, Z axis acceleration values in m/s^2 as floats. Will
          return values in g if constructor was provided `accel_sf=SF_M_S2`
          parameter.
          """
          so = self._accel_so
          sf = self._accel_sf
  
          xyz = self._register_three_shorts(_ACCEL_XOUT_H)
  
          return tuple([value / so * sf for value in xyz])
  
      @property
      def gyro(self):
          """
          X, Y, Z radians per second as floats.
          """
          so = self._gyro_so
          sf = self._gyro_sf
          ox, oy, oz = self._gyro_offset
  
          xyz = self._register_three_shorts(_GYRO_XOUT_H)
          xyz = [value / so * sf for value in xyz]
  
          xyz[0] -= ox
          xyz[1] -= oy
          xyz[2] -= oz
  
          return tuple(xyz)
  
      @property
      def temperature(self):
          """
          Die temperature in celcius as a float.
          """
          temp = self._register_short(_TEMP_OUT_H)
          return ((temp - _TEMP_OFFSET) / _TEMP_SO) + _TEMP_OFFSET
  
      @property
      def whoami(self):
          """ Value of the whoami register. """
          return self._register_char(_WHO_AM_I)
  
      def calibrate(self, count=256, delay=0):
          ox, oy, oz = (0.0, 0.0, 0.0)
          self._gyro_offset = (0.0, 0.0, 0.0)
          n = float(count)
  
          while count:
              utime.sleep_ms(delay)
              gx, gy, gz = self.gyro
              ox += gx
              oy += gy
              oz += gz
              count -= 1
  
          self._gyro_offset = (ox / n, oy / n, oz / n)
          return self._gyro_offset
  
      def _register_short(self, register, value=None, buf=bytearray(2)):
          if value is None:
              self.i2c.readfrom_mem_into(self.address, register, buf)
              return ustruct.unpack(">h", buf)[0]
  
          ustruct.pack_into(">h", buf, 0, value)
          return self.i2c.writeto_mem(self.address, register, buf)
  
      def _register_three_shorts(self, register, buf=bytearray(6)):
          self.i2c.readfrom_mem_into(self.address, register, buf)
          return ustruct.unpack(">hhh", buf)
  
      def _register_char(self, register, value=None, buf=bytearray(1)):
          if value is None:
              self.i2c.readfrom_mem_into(self.address, register, buf)
              return buf[0]
  
          ustruct.pack_into("<b", buf, 0, value)
          return self.i2c.writeto_mem(self.address, register, buf)
  
      def _accel_fs(self, value):
          self._register_char(_ACCEL_CONFIG, value)
  
          # Return the sensitivity divider
          if ACCEL_FS_SEL_2G == value:
              return _ACCEL_SO_2G
          elif ACCEL_FS_SEL_4G == value:
              return _ACCEL_SO_4G
          elif ACCEL_FS_SEL_8G == value:
              return _ACCEL_SO_8G
          elif ACCEL_FS_SEL_16G == value:
              return _ACCEL_SO_16G
  
      def _gyro_fs(self, value):
          self._register_char(_GYRO_CONFIG, value)
  
          # Return the sensitivity divider
          if GYRO_FS_SEL_250DPS == value:
              return _GYRO_SO_250DPS
          elif GYRO_FS_SEL_500DPS == value:
              return _GYRO_SO_500DPS
          elif GYRO_FS_SEL_1000DPS == value:
              return _GYRO_SO_1000DPS
          elif GYRO_FS_SEL_2000DPS == value:
              return _GYRO_SO_2000DPS
  
      def __enter__(self):
          return self
  
      def __exit__(self, exception_type, exception_value, traceback):
          pass