├── BMP085 ├── bmp085.py └── examples │ └── bmp085_example.py ├── HMC5883L ├── examples │ └── hmc5883l_raw.py └── hmc5883l.py ├── LICENSE ├── MPU6050 ├── Examples │ └── 6axis_dmp.py └── mpu6050.py ├── PCA9685 └── pca9685.py ├── PyComms └── pycomms.py └── README /BMP085/bmp085.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Python Standard Library Imports 4 | import time 5 | 6 | # External Imports 7 | pass 8 | 9 | # Custom Imports 10 | from pycomms import PyComms 11 | 12 | # =========================================================================== 13 | # Adafruit BMP085 Class (slightly modified) 14 | # =========================================================================== 15 | 16 | class BMP085: 17 | i2c = None 18 | 19 | # Operating Modes 20 | __BMP085_ULTRALOWPOWER = 0 21 | __BMP085_STANDARD = 1 22 | __BMP085_HIGHRES = 2 23 | __BMP085_ULTRAHIGHRES = 3 24 | 25 | # BMP085 Registers 26 | __BMP085_CAL_AC1 = 0xAA # R Calibration data (16 bits) 27 | __BMP085_CAL_AC2 = 0xAC # R Calibration data (16 bits) 28 | __BMP085_CAL_AC3 = 0xAE # R Calibration data (16 bits) 29 | __BMP085_CAL_AC4 = 0xB0 # R Calibration data (16 bits) 30 | __BMP085_CAL_AC5 = 0xB2 # R Calibration data (16 bits) 31 | __BMP085_CAL_AC6 = 0xB4 # R Calibration data (16 bits) 32 | __BMP085_CAL_B1 = 0xB6 # R Calibration data (16 bits) 33 | __BMP085_CAL_B2 = 0xB8 # R Calibration data (16 bits) 34 | __BMP085_CAL_MB = 0xBA # R Calibration data (16 bits) 35 | __BMP085_CAL_MC = 0xBC # R Calibration data (16 bits) 36 | __BMP085_CAL_MD = 0xBE # R Calibration data (16 bits) 37 | __BMP085_CONTROL = 0xF4 38 | __BMP085_TEMPDATA = 0xF6 39 | __BMP085_PRESSUREDATA = 0xF6 40 | __BMP085_READTEMPCMD = 0x2E 41 | __BMP085_READPRESSURECMD = 0x34 42 | 43 | # Private Fields 44 | _cal_AC1 = 0 45 | _cal_AC2 = 0 46 | _cal_AC3 = 0 47 | _cal_AC4 = 0 48 | _cal_AC5 = 0 49 | _cal_AC6 = 0 50 | _cal_B1 = 0 51 | _cal_B2 = 0 52 | _cal_MB = 0 53 | _cal_MC = 0 54 | _cal_MD = 0 55 | 56 | def __init__(self, address = 0x77, mode = 3): 57 | self.i2c = PyComms(address) 58 | self.address = address 59 | 60 | # Make sure the specified mode is in the appropriate range 61 | if ((mode < 0) | (mode > 3)): 62 | self.mode = self.__BMP085_STANDARD 63 | else: 64 | self.mode = mode 65 | 66 | # Read the calibration data 67 | self.readCalibrationData() 68 | 69 | def readCalibrationData(self): 70 | # Reads the calibration data from the IC 71 | self._cal_AC1 = self.i2c.readS16(self.__BMP085_CAL_AC1) # INT16 72 | self._cal_AC2 = self.i2c.readS16(self.__BMP085_CAL_AC2) # INT16 73 | self._cal_AC3 = self.i2c.readS16(self.__BMP085_CAL_AC3) # INT16 74 | self._cal_AC4 = self.i2c.readU16(self.__BMP085_CAL_AC4) # UINT16 75 | self._cal_AC5 = self.i2c.readU16(self.__BMP085_CAL_AC5) # UINT16 76 | self._cal_AC6 = self.i2c.readU16(self.__BMP085_CAL_AC6) # UINT16 77 | self._cal_B1 = self.i2c.readS16(self.__BMP085_CAL_B1) # INT16 78 | self._cal_B2 = self.i2c.readS16(self.__BMP085_CAL_B2) # INT16 79 | self._cal_MB = self.i2c.readS16(self.__BMP085_CAL_MB) # INT16 80 | self._cal_MC = self.i2c.readS16(self.__BMP085_CAL_MC) # INT16 81 | self._cal_MD = self.i2c.readS16(self.__BMP085_CAL_MD) # INT16 82 | 83 | def readRawTemp(self): 84 | # Reads the raw (uncompensated) temperature from the sensor 85 | self.i2c.write8(self.__BMP085_CONTROL, self.__BMP085_READTEMPCMD) 86 | time.sleep(0.005) # Wait 5ms 87 | raw = self.i2c.readU16(self.__BMP085_TEMPDATA) 88 | 89 | return raw 90 | 91 | def readRawPressure(self): 92 | # Reads the raw (uncompensated) pressure level from the sensor 93 | self.i2c.write8(self.__BMP085_CONTROL, self.__BMP085_READPRESSURECMD + (self.mode << 6)) 94 | if (self.mode == self.__BMP085_ULTRALOWPOWER): 95 | time.sleep(0.005) 96 | elif (self.mode == self.__BMP085_HIGHRES): 97 | time.sleep(0.014) 98 | elif (self.mode == self.__BMP085_ULTRAHIGHRES): 99 | time.sleep(0.026) 100 | else: 101 | time.sleep(0.008) 102 | 103 | msb = self.i2c.readU8(self.__BMP085_PRESSUREDATA) 104 | lsb = self.i2c.readU8(self.__BMP085_PRESSUREDATA + 1) 105 | xlsb = self.i2c.readU8(self.__BMP085_PRESSUREDATA + 2) 106 | 107 | raw = ((msb << 16) + (lsb << 8) + xlsb) >> (8 - self.mode) 108 | 109 | return raw 110 | 111 | def readTemperature(self): 112 | # Gets the compensated temperature in degrees celcius 113 | UT = 0 114 | X1 = 0 115 | X2 = 0 116 | B5 = 0 117 | temp = 0.0 118 | 119 | # Read raw temp before aligning it with the calibration values 120 | UT = self.readRawTemp() 121 | X1 = ((UT - self._cal_AC6) * self._cal_AC5) >> 15 122 | X2 = (self._cal_MC << 11) / (X1 + self._cal_MD) 123 | B5 = X1 + X2 124 | temp = ((B5 + 8) >> 4) / 10.0 125 | 126 | return temp 127 | 128 | def readPressure(self): 129 | # Gets the compensated pressure in pascal 130 | UT = 0 131 | UP = 0 132 | B3 = 0 133 | B5 = 0 134 | B6 = 0 135 | X1 = 0 136 | X2 = 0 137 | X3 = 0 138 | p = 0 139 | B4 = 0 140 | B7 = 0 141 | 142 | UT = self.readRawTemp() 143 | UP = self.readRawPressure() 144 | 145 | # True Temperature Calculations 146 | X1 = ((UT - self._cal_AC6) * self._cal_AC5) >> 15 147 | X2 = (self._cal_MC << 11) / (X1 + self._cal_MD) 148 | B5 = X1 + X2 149 | 150 | # Pressure Calculations 151 | B6 = B5 - 4000 152 | X1 = (self._cal_B2 * (B6 * B6) >> 12) >> 11 153 | X2 = (self._cal_AC2 * B6) >> 11 154 | X3 = X1 + X2 155 | B3 = (((self._cal_AC1 * 4 + X3) << self.mode) + 2) / 4 156 | 157 | X1 = (self._cal_AC3 * B6) >> 13 158 | X2 = (self._cal_B1 * ((B6 * B6) >> 12)) >> 16 159 | X3 = ((X1 + X2) + 2) >> 2 160 | B4 = (self._cal_AC4 * (X3 + 32768)) >> 15 161 | B7 = (UP - B3) * (50000 >> self.mode) 162 | 163 | if (B7 < 0x80000000): 164 | p = (B7 * 2) / B4 165 | else: 166 | p = (B7 / B4) * 2 167 | 168 | X1 = (p >> 8) * (p >> 8) 169 | X1 = (X1 * 3038) >> 16 170 | X2 = (-7375 * p) >> 16 171 | 172 | p = p + ((X1 + X2 + 3791) >> 4) 173 | 174 | return p 175 | 176 | def readAltitude(self, seaLevelPressure = 101325): 177 | # Calculates the altitude in meters 178 | altitude = 0.0 179 | pressure = float(self.readPressure()) 180 | altitude = 44330.0 * (1.0 - pow(pressure / seaLevelPressure, 0.1903)) 181 | 182 | return altitude 183 | return 0 -------------------------------------------------------------------------------- /BMP085/examples/bmp085_example.py: -------------------------------------------------------------------------------- 1 | from bmp085 import BMP085 2 | 3 | bmp = BMP085() 4 | 5 | temp = bmp.readTemperature() 6 | pressure = bmp.readPressure() 7 | altitude = bmp.readAltitude() 8 | 9 | print ("Temperature: %.2f C" % temp) 10 | print ("Pressure: %.2f hPa" % (pressure / 100.0)) 11 | print ("Altitude: %.2f" % altitude) 12 | -------------------------------------------------------------------------------- /HMC5883L/examples/hmc5883l_raw.py: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | import hmc5883l 4 | 5 | # Sensor initialization 6 | mag = hmc5883l.HMC5883L() 7 | mag.initialize() 8 | 9 | while True: 10 | data = mag.getHeading() 11 | 12 | print(data['x']), 13 | print(data['y']), 14 | print(data['z']) 15 | -------------------------------------------------------------------------------- /HMC5883L/hmc5883l.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Python Standard Library Imports 4 | pass 5 | 6 | # External Imports 7 | pass 8 | 9 | # Custom Imports 10 | from pycomms import PyComms 11 | 12 | class HMC5883L: 13 | # Register map based on Jeff Rowberg source code at 14 | # https://github.com/jrowberg/i2cdevlib/ 15 | 16 | HMC5883L_ADDRESS = 0x1E # this device only has one address 17 | HMC5883L_DEFAULT_ADDRESS = 0x1E 18 | 19 | HMC5883L_RA_CONFIG_A = 0x00 20 | HMC5883L_RA_CONFIG_B = 0x01 21 | HMC5883L_RA_MODE = 0x02 22 | HMC5883L_RA_DATAX_H = 0x03 23 | HMC5883L_RA_DATAX_L = 0x04 24 | HMC5883L_RA_DATAZ_H = 0x05 25 | HMC5883L_RA_DATAZ_L = 0x06 26 | HMC5883L_RA_DATAY_H = 0x07 27 | HMC5883L_RA_DATAY_L = 0x08 28 | HMC5883L_RA_STATUS = 0x09 29 | HMC5883L_RA_ID_A = 0x0A 30 | HMC5883L_RA_ID_B = 0x0B 31 | HMC5883L_RA_ID_C = 0x0C 32 | 33 | HMC5883L_CRA_AVERAGE_BIT = 6 34 | HMC5883L_CRA_AVERAGE_LENGTH = 2 35 | HMC5883L_CRA_RATE_BIT = 4 36 | HMC5883L_CRA_RATE_LENGTH = 3 37 | HMC5883L_CRA_BIAS_BIT = 1 38 | HMC5883L_CRA_BIAS_LENGTH = 2 39 | 40 | HMC5883L_AVERAGING_1 = 0x00 41 | HMC5883L_AVERAGING_2 = 0x01 42 | HMC5883L_AVERAGING_4 = 0x02 43 | HMC5883L_AVERAGING_8 = 0x03 44 | 45 | HMC5883L_RATE_0P75 = 0x00 46 | HMC5883L_RATE_1P5 = 0x01 47 | HMC5883L_RATE_3 = 0x02 48 | HMC5883L_RATE_7P5 = 0x03 49 | HMC5883L_RATE_15 = 0x04 50 | HMC5883L_RATE_30 = 0x05 51 | HMC5883L_RATE_75 = 0x06 52 | 53 | HMC5883L_BIAS_NORMAL = 0x00 54 | HMC5883L_BIAS_POSITIVE = 0x01 55 | HMC5883L_BIAS_NEGATIVE = 0x02 56 | 57 | HMC5883L_CRB_GAIN_BIT = 7 58 | HMC5883L_CRB_GAIN_LENGTH = 3 59 | 60 | HMC5883L_GAIN_1370 = 0x00 61 | HMC5883L_GAIN_1090 = 0x01 62 | HMC5883L_GAIN_820 = 0x02 63 | HMC5883L_GAIN_660 = 0x03 64 | HMC5883L_GAIN_440 = 0x04 65 | HMC5883L_GAIN_390 = 0x05 66 | HMC5883L_GAIN_330 = 0x06 67 | HMC5883L_GAIN_220 = 0x07 68 | 69 | HMC5883L_MODEREG_BIT = 1 70 | HMC5883L_MODEREG_LENGTH = 2 71 | 72 | HMC5883L_MODE_CONTINUOUS = 0x00 73 | HMC5883L_MODE_SINGLE = 0x01 74 | HMC5883L_MODE_IDLE = 0x02 75 | 76 | HMC5883L_STATUS_LOCK_BIT = 1 77 | HMC5883L_STATUS_READY_BIT = 0 78 | 79 | mode = 0 80 | 81 | def __init__(self, address = HMC5883L_DEFAULT_ADDRESS): 82 | self.i2c = PyComms(address) 83 | self.address = address 84 | 85 | def initialize(self): 86 | # write CONFIG_A register 87 | self.i2c.write8(self.HMC5883L_RA_CONFIG_A, 88 | (self.HMC5883L_AVERAGING_8 << (self.HMC5883L_CRA_AVERAGE_BIT - self.HMC5883L_CRA_AVERAGE_LENGTH + 1)) | 89 | (self.HMC5883L_RATE_15 << (self.HMC5883L_CRA_RATE_BIT - self.HMC5883L_CRA_RATE_LENGTH + 1)) | 90 | (self.HMC5883L_BIAS_NORMAL << (self.HMC5883L_CRA_BIAS_BIT - self.HMC5883L_CRA_BIAS_LENGTH + 1))) 91 | 92 | # write CONFIG_B register 93 | self.setGain(self.HMC5883L_GAIN_1090); 94 | 95 | # write MODE register 96 | self.setMode(self.HMC5883L_MODE_SINGLE); 97 | 98 | def testConnection(self): 99 | pass 100 | 101 | def getSampleAveraging(self): 102 | pass 103 | 104 | def setSampleAveraging(self, value): 105 | self.i2c.writeBits(self.HMC5883L_RA_CONFIG_A, self.HMC5883L_CRA_AVERAGE_BIT, self.HMC5883L_CRA_AVERAGE_LENGTH, value) 106 | 107 | def getDataRate(self): 108 | pass 109 | 110 | def setDataRate(self, value): 111 | self.i2c.writeBits(self.HMC5883L_RA_CONFIG_A, self.HMC5883L_CRA_RATE_BIT, self.HMC5883L_CRA_RATE_LENGTH, value) 112 | 113 | def getMeasurementBias(self): 114 | pass 115 | 116 | def setMeasurementBias(self, value): 117 | self.i2c.writeBits(self.HMC5883L_RA_CONFIG_A, self.HMC5883L_CRA_BIAS_BIT, self.HMC5883L_CRA_BIAS_LENGTH, value); 118 | 119 | def getGain(self): 120 | pass 121 | 122 | def setGain(self, value): 123 | self.i2c.write8(self.HMC5883L_RA_CONFIG_B, value << (self.HMC5883L_CRB_GAIN_BIT - self.HMC5883L_CRB_GAIN_LENGTH + 1)) 124 | 125 | def getMode(self): 126 | pass 127 | 128 | def setMode(self, newMode): 129 | # use this method to guarantee that bits 7-2 are set to zero, which is a 130 | # requirement specified in the datasheet 131 | self.i2c.write8(self.HMC5883L_RA_MODE, self.mode << (self.HMC5883L_MODEREG_BIT - self.HMC5883L_MODEREG_LENGTH + 1)) 132 | self.mode = newMode # track to tell if we have to clear bit 7 after a read 133 | 134 | def getHeading(self): 135 | packet = self.i2c.readBytesListS(self.HMC5883L_RA_DATAX_H, 6) 136 | if (self.mode == self.HMC5883L_MODE_SINGLE): 137 | self.i2c.write8(self.HMC5883L_RA_MODE, self.HMC5883L_MODE_SINGLE << (self.HMC5883L_MODEREG_BIT - self.HMC5883L_MODEREG_LENGTH + 1)) 138 | 139 | data = { 140 | 'x' : ((packet[0] << 8) | packet[1]), 141 | 'y' : ((packet[4] << 8) | packet[5]), 142 | 'z' : ((packet[2] << 8) | packet[3])} 143 | 144 | return data 145 | 146 | def getHeadingX(self): 147 | # each axis read requires that ALL axis registers be read, even if only 148 | # one is used; this was not done ineffiently in the code by accident 149 | packet = self.i2c.readBytesListS(self.HMC5883L_RA_DATAX_H, 6) 150 | if (self.mode == self.HMC5883L_MODE_SINGLE): 151 | self.i2c.write8(self.HMC5883L_RA_MODE, self.HMC5883L_MODE_SINGLE << (self.HMC5883L_MODEREG_BIT - self.HMC5883L_MODEREG_LENGTH + 1)) 152 | 153 | return ((packet[0] << 8) | packet[1]) 154 | 155 | def getHeadingY(self): 156 | # each axis read requires that ALL axis registers be read, even if only 157 | # one is used; this was not done ineffiently in the code by accident 158 | packet = self.i2c.readBytesListS(self.HMC5883L_RA_DATAX_H, 6) 159 | if (self.mode == self.HMC5883L_MODE_SINGLE): 160 | self.i2c.write8(self.HMC5883L_RA_MODE, self.HMC5883L_MODE_SINGLE << (self.HMC5883L_MODEREG_BIT - self.HMC5883L_MODEREG_LENGTH + 1)) 161 | 162 | return ((packet[4] << 8) | packet[5]) 163 | 164 | def getHeadingZ(self): 165 | # each axis read requires that ALL axis registers be read, even if only 166 | # one is used; this was not done ineffiently in the code by accident 167 | packet = self.i2c.readBytesListS(self.HMC5883L_RA_DATAX_H, 6) 168 | if (self.mode == self.HMC5883L_MODE_SINGLE): 169 | self.i2c.write8(self.HMC5883L_RA_MODE, self.HMC5883L_MODE_SINGLE << (self.HMC5883L_MODEREG_BIT - self.HMC5883L_MODEREG_LENGTH + 1)) 170 | 171 | return ((packet[2] << 8) | packet[3]) 172 | 173 | def getLockStatus(self): 174 | result = self.i2c.readBit(self.HMC5883L_RA_STATUS, self.HMC5883L_STATUS_LOCK_BIT) 175 | return result 176 | 177 | def getReadyStatus(self): 178 | result = self.i2c.readBit(self.HMC5883L_RA_STATUS, self.HMC5883L_STATUS_READY_BIT) 179 | return result 180 | 181 | def getIDA(self): 182 | result = self.i2c.readByte(self.HMC5883L_RA_ID_A) 183 | return result 184 | 185 | def getIDB(self): 186 | result = self.i2c.readByte(self.HMC5883L_RA_ID_B) 187 | return result 188 | 189 | def getIDC(self): 190 | result = self.i2c.readByte(self.HMC5883L_RA_ID_C) 191 | return result -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Stefan Kolla 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /MPU6050/Examples/6axis_dmp.py: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | import mpu6050 4 | 5 | # Sensor initialization 6 | mpu = mpu6050.MPU6050() 7 | mpu.dmpInitialize() 8 | mpu.setDMPEnabled(True) 9 | 10 | # get expected DMP packet size for later comparison 11 | packetSize = mpu.dmpGetFIFOPacketSize() 12 | 13 | while True: 14 | # Get INT_STATUS byte 15 | mpuIntStatus = mpu.getIntStatus() 16 | 17 | if mpuIntStatus >= 2: # check for DMP data ready interrupt (this should happen frequently) 18 | # get current FIFO count 19 | fifoCount = mpu.getFIFOCount() 20 | 21 | # check for overflow (this should never happen unless our code is too inefficient) 22 | if fifoCount == 1024: 23 | # reset so we can continue cleanly 24 | mpu.resetFIFO() 25 | print('FIFO overflow!') 26 | 27 | 28 | # wait for correct available data length, should be a VERY short wait 29 | fifoCount = mpu.getFIFOCount() 30 | while fifoCount < packetSize: 31 | fifoCount = mpu.getFIFOCount() 32 | 33 | result = mpu.getFIFOBytes(packetSize) 34 | q = mpu.dmpGetQuaternion(result) 35 | g = mpu.dmpGetGravity(q) 36 | ypr = mpu.dmpGetYawPitchRoll(q, g) 37 | 38 | print(ypr['yaw'] * 180 / math.pi), 39 | print(ypr['pitch'] * 180 / math.pi), 40 | print(ypr['roll'] * 180 / math.pi) 41 | 42 | # track FIFO count here in case there is > 1 packet available 43 | # (this lets us immediately read more without waiting for an interrupt) 44 | fifoCount -= packetSize -------------------------------------------------------------------------------- /MPU6050/mpu6050.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Python Standard Library Imports 4 | from time import sleep 5 | from math import atan, atan2, sqrt 6 | 7 | # External Imports 8 | pass 9 | 10 | # Custom Imports 11 | from pycomms import PyComms 12 | 13 | class MPU6050: 14 | # Register map based on Jeff Rowberg source code at 15 | # https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.h 16 | 17 | MPU6050_ADDRESS_AD0_LOW = 0x68 # address pin low (GND), default for InvenSense evaluation board 18 | MPU6050_ADDRESS_AD0_HIGH = 0x69 # address pin high (VCC) 19 | MPU6050_DEFAULT_ADDRESS = MPU6050_ADDRESS_AD0_LOW 20 | 21 | MPU6050_RA_XG_OFFS_TC = 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 22 | MPU6050_RA_YG_OFFS_TC = 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 23 | MPU6050_RA_ZG_OFFS_TC = 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 24 | MPU6050_RA_X_FINE_GAIN = 0x03 # [7:0] X_FINE_GAIN 25 | MPU6050_RA_Y_FINE_GAIN = 0x04 # [7:0] Y_FINE_GAIN 26 | MPU6050_RA_Z_FINE_GAIN = 0x05 # [7:0] Z_FINE_GAIN 27 | MPU6050_RA_XA_OFFS_H = 0x06 # [15:0] XA_OFFS 28 | MPU6050_RA_XA_OFFS_L_TC = 0x07 29 | MPU6050_RA_YA_OFFS_H = 0x08 # [15:0] YA_OFFS 30 | MPU6050_RA_YA_OFFS_L_TC = 0x09 31 | MPU6050_RA_ZA_OFFS_H = 0x0A # [15:0] ZA_OFFS 32 | MPU6050_RA_ZA_OFFS_L_TC = 0x0B 33 | MPU6050_RA_XG_OFFS_USRH = 0x13 # [15:0] XG_OFFS_USR 34 | MPU6050_RA_XG_OFFS_USRL = 0x14 35 | MPU6050_RA_YG_OFFS_USRH = 0x15 # [15:0] YG_OFFS_USR 36 | MPU6050_RA_YG_OFFS_USRL = 0x16 37 | MPU6050_RA_ZG_OFFS_USRH = 0x17 # [15:0] ZG_OFFS_USR 38 | MPU6050_RA_ZG_OFFS_USRL = 0x18 39 | MPU6050_RA_SMPLRT_DIV = 0x19 40 | MPU6050_RA_CONFIG = 0x1A 41 | MPU6050_RA_GYRO_CONFIG = 0x1B 42 | MPU6050_RA_ACCEL_CONFIG = 0x1C 43 | MPU6050_RA_FF_THR = 0x1D 44 | MPU6050_RA_FF_DUR = 0x1E 45 | MPU6050_RA_MOT_THR = 0x1F 46 | MPU6050_RA_MOT_DUR = 0x20 47 | MPU6050_RA_ZRMOT_THR = 0x21 48 | MPU6050_RA_ZRMOT_DUR = 0x22 49 | MPU6050_RA_FIFO_EN = 0x23 50 | MPU6050_RA_I2C_MST_CTRL = 0x24 51 | MPU6050_RA_I2C_SLV0_ADDR = 0x25 52 | MPU6050_RA_I2C_SLV0_REG = 0x26 53 | MPU6050_RA_I2C_SLV0_CTRL = 0x27 54 | MPU6050_RA_I2C_SLV1_ADDR = 0x28 55 | MPU6050_RA_I2C_SLV1_REG = 0x29 56 | MPU6050_RA_I2C_SLV1_CTRL = 0x2A 57 | MPU6050_RA_I2C_SLV2_ADDR = 0x2B 58 | MPU6050_RA_I2C_SLV2_REG = 0x2C 59 | MPU6050_RA_I2C_SLV2_CTRL = 0x2D 60 | MPU6050_RA_I2C_SLV3_ADDR = 0x2E 61 | MPU6050_RA_I2C_SLV3_REG = 0x2F 62 | MPU6050_RA_I2C_SLV3_CTRL = 0x30 63 | MPU6050_RA_I2C_SLV4_ADDR = 0x31 64 | MPU6050_RA_I2C_SLV4_REG = 0x32 65 | MPU6050_RA_I2C_SLV4_DO = 0x33 66 | MPU6050_RA_I2C_SLV4_CTRL = 0x34 67 | MPU6050_RA_I2C_SLV4_DI = 0x35 68 | MPU6050_RA_I2C_MST_STATUS = 0x36 69 | MPU6050_RA_INT_PIN_CFG = 0x37 70 | MPU6050_RA_INT_ENABLE = 0x38 71 | MPU6050_RA_DMP_INT_STATUS = 0x39 72 | MPU6050_RA_INT_STATUS = 0x3A 73 | MPU6050_RA_ACCEL_XOUT_H = 0x3B 74 | MPU6050_RA_ACCEL_XOUT_L = 0x3C 75 | MPU6050_RA_ACCEL_YOUT_H = 0x3D 76 | MPU6050_RA_ACCEL_YOUT_L = 0x3E 77 | MPU6050_RA_ACCEL_ZOUT_H = 0x3F 78 | MPU6050_RA_ACCEL_ZOUT_L = 0x40 79 | MPU6050_RA_TEMP_OUT_H = 0x41 80 | MPU6050_RA_TEMP_OUT_L = 0x42 81 | MPU6050_RA_GYRO_XOUT_H = 0x43 82 | MPU6050_RA_GYRO_XOUT_L = 0x44 83 | MPU6050_RA_GYRO_YOUT_H = 0x45 84 | MPU6050_RA_GYRO_YOUT_L = 0x46 85 | MPU6050_RA_GYRO_ZOUT_H = 0x47 86 | MPU6050_RA_GYRO_ZOUT_L = 0x48 87 | MPU6050_RA_EXT_SENS_DATA_00 = 0x49 88 | MPU6050_RA_EXT_SENS_DATA_01 = 0x4A 89 | MPU6050_RA_EXT_SENS_DATA_02 = 0x4B 90 | MPU6050_RA_EXT_SENS_DATA_03 = 0x4C 91 | MPU6050_RA_EXT_SENS_DATA_04 = 0x4D 92 | MPU6050_RA_EXT_SENS_DATA_05 = 0x4E 93 | MPU6050_RA_EXT_SENS_DATA_06 = 0x4F 94 | MPU6050_RA_EXT_SENS_DATA_07 = 0x50 95 | MPU6050_RA_EXT_SENS_DATA_08 = 0x51 96 | MPU6050_RA_EXT_SENS_DATA_09 = 0x52 97 | MPU6050_RA_EXT_SENS_DATA_10 = 0x53 98 | MPU6050_RA_EXT_SENS_DATA_11 = 0x54 99 | MPU6050_RA_EXT_SENS_DATA_12 = 0x55 100 | MPU6050_RA_EXT_SENS_DATA_13 = 0x56 101 | MPU6050_RA_EXT_SENS_DATA_14 = 0x57 102 | MPU6050_RA_EXT_SENS_DATA_15 = 0x58 103 | MPU6050_RA_EXT_SENS_DATA_16 = 0x59 104 | MPU6050_RA_EXT_SENS_DATA_17 = 0x5A 105 | MPU6050_RA_EXT_SENS_DATA_18 = 0x5B 106 | MPU6050_RA_EXT_SENS_DATA_19 = 0x5C 107 | MPU6050_RA_EXT_SENS_DATA_20 = 0x5D 108 | MPU6050_RA_EXT_SENS_DATA_21 = 0x5E 109 | MPU6050_RA_EXT_SENS_DATA_22 = 0x5F 110 | MPU6050_RA_EXT_SENS_DATA_23 = 0x60 111 | MPU6050_RA_MOT_DETECT_STATUS = 0x61 112 | MPU6050_RA_I2C_SLV0_DO = 0x63 113 | MPU6050_RA_I2C_SLV1_DO = 0x64 114 | MPU6050_RA_I2C_SLV2_DO = 0x65 115 | MPU6050_RA_I2C_SLV3_DO = 0x66 116 | MPU6050_RA_I2C_MST_DELAY_CTRL = 0x67 117 | MPU6050_RA_SIGNAL_PATH_RESET = 0x68 118 | MPU6050_RA_MOT_DETECT_CTRL = 0x69 119 | MPU6050_RA_USER_CTRL = 0x6A 120 | MPU6050_RA_PWR_MGMT_1 = 0x6B 121 | MPU6050_RA_PWR_MGMT_2 = 0x6C 122 | MPU6050_RA_BANK_SEL = 0x6D 123 | MPU6050_RA_MEM_START_ADDR = 0x6E 124 | MPU6050_RA_MEM_R_W = 0x6F 125 | MPU6050_RA_DMP_CFG_1 = 0x70 126 | MPU6050_RA_DMP_CFG_2 = 0x71 127 | MPU6050_RA_FIFO_COUNTH = 0x72 128 | MPU6050_RA_FIFO_COUNTL = 0x73 129 | MPU6050_RA_FIFO_R_W = 0x74 130 | MPU6050_RA_WHO_AM_I = 0x75 131 | 132 | MPU6050_TC_PWR_MODE_BIT = 7 133 | MPU6050_TC_OFFSET_BIT = 6 134 | MPU6050_TC_OFFSET_LENGTH = 6 135 | MPU6050_TC_OTP_BNK_VLD_BIT = 0 136 | 137 | MPU6050_VDDIO_LEVEL_VLOGIC = 0 138 | MPU6050_VDDIO_LEVEL_VDD = 1 139 | 140 | MPU6050_CFG_EXT_SYNC_SET_BIT = 5 141 | MPU6050_CFG_EXT_SYNC_SET_LENGTH = 3 142 | MPU6050_CFG_DLPF_CFG_BIT = 2 143 | MPU6050_CFG_DLPF_CFG_LENGTH = 3 144 | 145 | MPU6050_EXT_SYNC_DISABLED = 0x0 146 | MPU6050_EXT_SYNC_TEMP_OUT_L = 0x1 147 | MPU6050_EXT_SYNC_GYRO_XOUT_L = 0x2 148 | MPU6050_EXT_SYNC_GYRO_YOUT_L = 0x3 149 | MPU6050_EXT_SYNC_GYRO_ZOUT_L = 0x4 150 | MPU6050_EXT_SYNC_ACCEL_XOUT_L = 0x5 151 | MPU6050_EXT_SYNC_ACCEL_YOUT_L = 0x6 152 | MPU6050_EXT_SYNC_ACCEL_ZOUT_L = 0x7 153 | 154 | MPU6050_DLPF_BW_256 = 0x00 155 | MPU6050_DLPF_BW_188 = 0x01 156 | MPU6050_DLPF_BW_98 = 0x02 157 | MPU6050_DLPF_BW_42 = 0x03 158 | MPU6050_DLPF_BW_20 = 0x04 159 | MPU6050_DLPF_BW_10 = 0x05 160 | MPU6050_DLPF_BW_5 = 0x06 161 | 162 | MPU6050_GCONFIG_FS_SEL_BIT = 4 163 | MPU6050_GCONFIG_FS_SEL_LENGTH = 2 164 | 165 | MPU6050_GYRO_FS_250 = 0x00 166 | MPU6050_GYRO_FS_500 = 0x01 167 | MPU6050_GYRO_FS_1000 = 0x02 168 | MPU6050_GYRO_FS_2000 = 0x03 169 | 170 | MPU6050_ACONFIG_XA_ST_BIT = 7 171 | MPU6050_ACONFIG_YA_ST_BIT = 6 172 | MPU6050_ACONFIG_ZA_ST_BIT = 5 173 | MPU6050_ACONFIG_AFS_SEL_BIT = 4 174 | MPU6050_ACONFIG_AFS_SEL_LENGTH = 2 175 | MPU6050_ACONFIG_ACCEL_HPF_BIT = 2 176 | MPU6050_ACONFIG_ACCEL_HPF_LENGTH = 3 177 | 178 | MPU6050_ACCEL_FS_2 = 0x00 179 | MPU6050_ACCEL_FS_4 = 0x01 180 | MPU6050_ACCEL_FS_8 = 0x02 181 | MPU6050_ACCEL_FS_16 = 0x03 182 | 183 | MPU6050_DHPF_RESET = 0x00 184 | MPU6050_DHPF_5 = 0x01 185 | MPU6050_DHPF_2P5 = 0x02 186 | MPU6050_DHPF_1P25 = 0x03 187 | MPU6050_DHPF_0P63 = 0x04 188 | MPU6050_DHPF_HOLD = 0x07 189 | 190 | MPU6050_TEMP_FIFO_EN_BIT = 7 191 | MPU6050_XG_FIFO_EN_BIT = 6 192 | MPU6050_YG_FIFO_EN_BIT = 5 193 | MPU6050_ZG_FIFO_EN_BIT = 4 194 | MPU6050_ACCEL_FIFO_EN_BIT = 3 195 | MPU6050_SLV2_FIFO_EN_BIT = 2 196 | MPU6050_SLV1_FIFO_EN_BIT = 1 197 | MPU6050_SLV0_FIFO_EN_BIT = 0 198 | 199 | MPU6050_MULT_MST_EN_BIT = 7 200 | MPU6050_WAIT_FOR_ES_BIT = 6 201 | MPU6050_SLV_3_FIFO_EN_BIT = 5 202 | MPU6050_I2C_MST_P_NSR_BIT = 4 203 | MPU6050_I2C_MST_CLK_BIT = 3 204 | MPU6050_I2C_MST_CLK_LENGTH = 4 205 | 206 | MPU6050_CLOCK_DIV_348 = 0x0 207 | MPU6050_CLOCK_DIV_333 = 0x1 208 | MPU6050_CLOCK_DIV_320 = 0x2 209 | MPU6050_CLOCK_DIV_308 = 0x3 210 | MPU6050_CLOCK_DIV_296 = 0x4 211 | MPU6050_CLOCK_DIV_286 = 0x5 212 | MPU6050_CLOCK_DIV_276 = 0x6 213 | MPU6050_CLOCK_DIV_267 = 0x7 214 | MPU6050_CLOCK_DIV_258 = 0x8 215 | MPU6050_CLOCK_DIV_500 = 0x9 216 | MPU6050_CLOCK_DIV_471 = 0xA 217 | MPU6050_CLOCK_DIV_444 = 0xB 218 | MPU6050_CLOCK_DIV_421 = 0xC 219 | MPU6050_CLOCK_DIV_400 = 0xD 220 | MPU6050_CLOCK_DIV_381 = 0xE 221 | MPU6050_CLOCK_DIV_364 = 0xF 222 | 223 | MPU6050_I2C_SLV_RW_BIT = 7 224 | MPU6050_I2C_SLV_ADDR_BIT = 6 225 | MPU6050_I2C_SLV_ADDR_LENGTH = 7 226 | MPU6050_I2C_SLV_EN_BIT = 7 227 | MPU6050_I2C_SLV_BYTE_SW_BIT = 6 228 | MPU6050_I2C_SLV_REG_DIS_BIT = 5 229 | MPU6050_I2C_SLV_GRP_BIT = 4 230 | MPU6050_I2C_SLV_LEN_BIT = 3 231 | MPU6050_I2C_SLV_LEN_LENGTH = 4 232 | 233 | MPU6050_I2C_SLV4_RW_BIT = 7 234 | MPU6050_I2C_SLV4_ADDR_BIT = 6 235 | MPU6050_I2C_SLV4_ADDR_LENGTH = 7 236 | MPU6050_I2C_SLV4_EN_BIT = 7 237 | MPU6050_I2C_SLV4_INT_EN_BIT = 6 238 | MPU6050_I2C_SLV4_REG_DIS_BIT = 5 239 | MPU6050_I2C_SLV4_MST_DLY_BIT = 4 240 | MPU6050_I2C_SLV4_MST_DLY_LENGTH = 5 241 | 242 | MPU6050_MST_PASS_THROUGH_BIT = 7 243 | MPU6050_MST_I2C_SLV4_DONE_BIT = 6 244 | MPU6050_MST_I2C_LOST_ARB_BIT = 5 245 | MPU6050_MST_I2C_SLV4_NACK_BIT = 4 246 | MPU6050_MST_I2C_SLV3_NACK_BIT = 3 247 | MPU6050_MST_I2C_SLV2_NACK_BIT = 2 248 | MPU6050_MST_I2C_SLV1_NACK_BIT = 1 249 | MPU6050_MST_I2C_SLV0_NACK_BIT = 0 250 | 251 | MPU6050_INTCFG_INT_LEVEL_BIT = 7 252 | MPU6050_INTCFG_INT_OPEN_BIT = 6 253 | MPU6050_INTCFG_LATCH_INT_EN_BIT = 5 254 | MPU6050_INTCFG_INT_RD_CLEAR_BIT = 4 255 | MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT = 3 256 | MPU6050_INTCFG_FSYNC_INT_EN_BIT = 2 257 | MPU6050_INTCFG_I2C_BYPASS_EN_BIT = 1 258 | MPU6050_INTCFG_CLKOUT_EN_BIT = 0 259 | 260 | MPU6050_INTMODE_ACTIVEHIGH = 0x00 261 | MPU6050_INTMODE_ACTIVELOW = 0x01 262 | 263 | MPU6050_INTDRV_PUSHPULL = 0x00 264 | MPU6050_INTDRV_OPENDRAIN = 0x01 265 | 266 | MPU6050_INTLATCH_50USPULSE = 0x00 267 | MPU6050_INTLATCH_WAITCLEAR = 0x01 268 | 269 | MPU6050_INTCLEAR_STATUSREAD = 0x00 270 | MPU6050_INTCLEAR_ANYREAD = 0x01 271 | 272 | MPU6050_INTERRUPT_FF_BIT = 7 273 | MPU6050_INTERRUPT_MOT_BIT = 6 274 | MPU6050_INTERRUPT_ZMOT_BIT = 5 275 | MPU6050_INTERRUPT_FIFO_OFLOW_BIT = 4 276 | MPU6050_INTERRUPT_I2C_MST_INT_BIT = 3 277 | MPU6050_INTERRUPT_PLL_RDY_INT_BIT = 2 278 | MPU6050_INTERRUPT_DMP_INT_BIT = 1 279 | MPU6050_INTERRUPT_DATA_RDY_BIT = 0 280 | 281 | # TODO: figure out what these actually do 282 | # UMPL source code is not very obivous 283 | MPU6050_DMPINT_5_BIT = 5 284 | MPU6050_DMPINT_4_BIT = 4 285 | MPU6050_DMPINT_3_BIT = 3 286 | MPU6050_DMPINT_2_BIT = 2 287 | MPU6050_DMPINT_1_BIT = 1 288 | MPU6050_DMPINT_0_BIT = 0 289 | 290 | MPU6050_MOTION_MOT_XNEG_BIT = 7 291 | MPU6050_MOTION_MOT_XPOS_BIT = 6 292 | MPU6050_MOTION_MOT_YNEG_BIT = 5 293 | MPU6050_MOTION_MOT_YPOS_BIT = 4 294 | MPU6050_MOTION_MOT_ZNEG_BIT = 3 295 | MPU6050_MOTION_MOT_ZPOS_BIT = 2 296 | MPU6050_MOTION_MOT_ZRMOT_BIT = 0 297 | 298 | MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT = 7 299 | MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT = 4 300 | MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT = 3 301 | MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT = 2 302 | MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT = 1 303 | MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT = 0 304 | 305 | MPU6050_PATHRESET_GYRO_RESET_BIT = 2 306 | MPU6050_PATHRESET_ACCEL_RESET_BIT = 1 307 | MPU6050_PATHRESET_TEMP_RESET_BIT = 0 308 | 309 | MPU6050_DETECT_ACCEL_ON_DELAY_BIT = 5 310 | MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH = 2 311 | MPU6050_DETECT_FF_COUNT_BIT = 3 312 | MPU6050_DETECT_FF_COUNT_LENGTH = 2 313 | MPU6050_DETECT_MOT_COUNT_BIT = 1 314 | MPU6050_DETECT_MOT_COUNT_LENGTH = 2 315 | 316 | MPU6050_DETECT_DECREMENT_RESET = 0x0 317 | MPU6050_DETECT_DECREMENT_1 = 0x1 318 | MPU6050_DETECT_DECREMENT_2 = 0x2 319 | MPU6050_DETECT_DECREMENT_4 = 0x3 320 | 321 | MPU6050_USERCTRL_DMP_EN_BIT = 7 322 | MPU6050_USERCTRL_FIFO_EN_BIT = 6 323 | MPU6050_USERCTRL_I2C_MST_EN_BIT = 5 324 | MPU6050_USERCTRL_I2C_IF_DIS_BIT = 4 325 | MPU6050_USERCTRL_DMP_RESET_BIT = 3 326 | MPU6050_USERCTRL_FIFO_RESET_BIT = 2 327 | MPU6050_USERCTRL_I2C_MST_RESET_BIT = 1 328 | MPU6050_USERCTRL_SIG_COND_RESET_BIT = 0 329 | 330 | MPU6050_PWR1_DEVICE_RESET_BIT = 7 331 | MPU6050_PWR1_SLEEP_BIT = 6 332 | MPU6050_PWR1_CYCLE_BIT = 5 333 | MPU6050_PWR1_TEMP_DIS_BIT = 3 334 | MPU6050_PWR1_CLKSEL_BIT = 2 335 | MPU6050_PWR1_CLKSEL_LENGTH = 3 336 | 337 | MPU6050_CLOCK_INTERNAL = 0x00 338 | MPU6050_CLOCK_PLL_XGYRO = 0x01 339 | MPU6050_CLOCK_PLL_YGYRO = 0x02 340 | MPU6050_CLOCK_PLL_ZGYRO = 0x03 341 | MPU6050_CLOCK_PLL_EXT32K = 0x04 342 | MPU6050_CLOCK_PLL_EXT19M = 0x05 343 | MPU6050_CLOCK_KEEP_RESET = 0x07 344 | 345 | MPU6050_PWR2_LP_WAKE_CTRL_BIT = 7 346 | MPU6050_PWR2_LP_WAKE_CTRL_LENGTH = 2 347 | MPU6050_PWR2_STBY_XA_BIT = 5 348 | MPU6050_PWR2_STBY_YA_BIT = 4 349 | MPU6050_PWR2_STBY_ZA_BIT = 3 350 | MPU6050_PWR2_STBY_XG_BIT = 2 351 | MPU6050_PWR2_STBY_YG_BIT = 1 352 | MPU6050_PWR2_STBY_ZG_BIT = 0 353 | 354 | MPU6050_WAKE_FREQ_1P25 = 0x0 355 | MPU6050_WAKE_FREQ_2P5 = 0x1 356 | MPU6050_WAKE_FREQ_5 = 0x2 357 | MPU6050_WAKE_FREQ_10 = 0x3 358 | 359 | MPU6050_BANKSEL_PRFTCH_EN_BIT = 6 360 | MPU6050_BANKSEL_CFG_USER_BANK_BIT = 5 361 | MPU6050_BANKSEL_MEM_SEL_BIT = 4 362 | MPU6050_BANKSEL_MEM_SEL_LENGTH = 5 363 | 364 | MPU6050_BANKSEL_PRFTCH_EN_BIT = 6 365 | MPU6050_BANKSEL_CFG_USER_BANK_BIT = 5 366 | MPU6050_BANKSEL_MEM_SEL_BIT = 4 367 | MPU6050_BANKSEL_MEM_SEL_LENGTH = 5 368 | 369 | MPU6050_WHO_AM_I_BIT = 6 370 | MPU6050_WHO_AM_I_LENGTH = 6 371 | 372 | # DMP 373 | 374 | MPU6050_DMP_MEMORY_BANKS = 8 375 | MPU6050_DMP_MEMORY_BANK_SIZE = 256 376 | MPU6050_DMP_MEMORY_CHUNK_SIZE = 16 377 | 378 | MPU6050_DMP_CODE_SIZE = 1929 # dmpMemory[] 379 | MPU6050_DMP_CONFIG_SIZE = 192 # dmpConfig[] 380 | MPU6050_DMP_UPDATES_SIZE = 47 # dmpUpdates[] 381 | # ==================================================================================================== 382 | # | Default MotionApps v2.0 42-byte FIFO packet structure: | 383 | # | | 384 | # | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 385 | # | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 386 | # | | 387 | # | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 388 | # | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 389 | # ==================================================================================================== 390 | 391 | # this block of memory gets written to the MPU on start-up, and it seems 392 | # to be volatile memory, so it has to be done each time (it only takes ~1 second though) 393 | dmpMemory = [ 394 | # bank 0, 256 bytes 395 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 396 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 397 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 398 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 399 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 400 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 401 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 402 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 403 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 404 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 405 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 406 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 407 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 408 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 409 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 410 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 411 | 412 | # bank 1, 256 bytes 413 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 414 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 415 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 416 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 417 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 418 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 419 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 420 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 421 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 422 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 423 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 424 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 425 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 426 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 427 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 428 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 429 | 430 | # bank 2, 256 bytes 431 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 432 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 433 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 434 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 435 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 436 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 437 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 438 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 439 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 440 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 441 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 442 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 443 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 444 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 445 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 446 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 447 | 448 | # bank 3, 256 bytes 449 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 450 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 451 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 452 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 453 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 454 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 455 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 456 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 457 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 458 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 459 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 460 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 461 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 462 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 463 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 464 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 465 | 466 | # bank 4, 256 bytes 467 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 468 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 469 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 470 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 471 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 472 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 473 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 474 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 475 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 476 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 477 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 478 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 479 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 480 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 481 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 482 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 483 | 484 | # bank 5, 256 bytes 485 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 486 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 487 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 488 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 489 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 490 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 491 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 492 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 493 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 494 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 495 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 496 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 497 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 498 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 499 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 500 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 501 | 502 | # bank 6, 256 bytes 503 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 504 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 505 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 506 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 507 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 508 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 509 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 510 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 511 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 512 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 513 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 514 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 515 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 516 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 517 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 518 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 519 | 520 | # bank 7, 138 bytes (remainder) 521 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 522 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 523 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 524 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 525 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 526 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 527 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 528 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 529 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF] 530 | 531 | dmpConfig = [ 532 | # BANK OFFSET LENGTH [DATA] 533 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, # FCFG_1 inv_set_gyro_calibration 534 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, # FCFG_3 inv_set_gyro_calibration 535 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, # D_0_104 inv_set_gyro_calibration 536 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, # D_0_24 inv_set_gyro_calibration 537 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, # D_1_152 inv_set_accel_calibration 538 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, # FCFG_2 inv_set_accel_calibration 539 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, # FCFG_7 inv_set_accel_calibration 540 | 0x00, 0x6C, 0x02, 0x20, 0x00, # D_0_108 inv_set_accel_calibration 541 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_00 inv_set_compass_calibration 542 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_01 543 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_02 544 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_10 545 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_11 546 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_12 547 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_20 548 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_21 549 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_22 550 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, # D_1_236 inv_apply_endian_accel 551 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, # FCFG_2 inv_set_mpu_sensors 552 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, # CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 553 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, # FCFG_5 inv_set_bias_update 554 | 0x00, 0xA3, 0x01, 0x00, # D_0_163 inv_set_dead_zone 555 | # SPECIAL 0x01 = enable interrupts 556 | 0x00, 0x00, 0x00, 0x01, # SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 557 | 0x07, 0x86, 0x01, 0xFE, # CFG_6 inv_set_fifo_interupt 558 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, # CFG_8 inv_send_quaternion 559 | 0x07, 0x7E, 0x01, 0x30, # CFG_16 inv_set_footer 560 | 0x07, 0x46, 0x01, 0x9A, # CFG_GYRO_SOURCE inv_send_gyro 561 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, # CFG_9 inv_send_gyro -> inv_construct3_fifo 562 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, # CFG_12 inv_send_accel -> inv_construct3_fifo 563 | 0x02, 0x16, 0x02, 0x00, 0x05 # D_0_22 inv_set_fifo_rate 564 | 565 | 566 | # This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 567 | # 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 568 | # DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 569 | 570 | # It is important to make sure the host processor can keep up with reading and processing 571 | # the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 572 | ] 573 | 574 | dmpUpdates = [ 575 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 576 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 577 | 0x01, 0x6A, 0x02, 0x06, 0x00, 578 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 579 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 580 | 0x01, 0x62, 0x02, 0x00, 0x00, 581 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00] 582 | 583 | # Setting up internal 42-byte (default) DMP packet buffer 584 | dmpPacketSize = 42 585 | 586 | # construct a new object with the I2C address of the MPU6050 587 | def __init__(self, address = MPU6050_DEFAULT_ADDRESS): 588 | self.i2c = PyComms(address) 589 | self.address = address 590 | 591 | def initialize(self): 592 | self.setClockSource(self.MPU6050_CLOCK_PLL_XGYRO) 593 | self.setFullScaleGyroRange(self.MPU6050_GYRO_FS_250) 594 | self.setFullScaleAccelRange(self.MPU6050_ACCEL_FS_2) 595 | self.setSleepEnabled(False) 596 | 597 | def testConnection(self): 598 | return self.getDeviceID() == 0x34 599 | 600 | def getAuxVDDIOLevel(self): 601 | return self.i2c.readBit(self.MPU6050_RA_YG_OFFS_TC, self.MPU6050_TC_PWR_MODE_BIT) 602 | 603 | def setAuxVDDIOLevel(self, level): 604 | self.i2c.writeBit(self.MPU6050_RA_YG_OFFS_TC, self.MPU6050_TC_PWR_MODE_BIT, level) 605 | 606 | def getRate(self): 607 | return self.i2c.readU8(self.MPU6050_RA_SMPLRT_DIV) 608 | 609 | def setRate(self, value): 610 | self.i2c.write8(self.MPU6050_RA_SMPLRT_DIV, value) 611 | 612 | def getExternalFrameSync(self): 613 | return self.i2c.readBits(self.MPU6050_RA_CONFIG, self.MPU6050_CFG_EXT_SYNC_SET_BIT, self.MPU6050_CFG_EXT_SYNC_SET_LENGTH) 614 | 615 | def setExternalFrameSync(self, sync): 616 | self.i2c.writeBits(self.MPU6050_RA_CONFIG, self.MPU6050_CFG_EXT_SYNC_SET_BIT, self.MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync) 617 | 618 | def getDLPFMode(self): 619 | return self.i2c.readBits(self.MPU6050_RA_CONFIG, self.MPU6050_CFG_DLPF_CFG_BIT, self.MPU6050_CFG_DLPF_CFG_LENGTH) 620 | 621 | def setDLPFMode(self, mode): 622 | self.i2c.writeBits(self.MPU6050_RA_CONFIG, self.MPU6050_CFG_DLPF_CFG_BIT, self.MPU6050_CFG_DLPF_CFG_LENGTH, mode) 623 | 624 | def getFullScaleGyroRange(self): 625 | return self.i2c.readBits(self.MPU6050_RA_GYRO_CONFIG, self.MPU6050_GCONFIG_FS_SEL_BIT, self.MPU6050_GCONFIG_FS_SEL_LENGTH) 626 | 627 | def setFullScaleGyroRange(self, range): 628 | self.i2c.writeBits(self.MPU6050_RA_GYRO_CONFIG, self.MPU6050_GCONFIG_FS_SEL_BIT, self.MPU6050_GCONFIG_FS_SEL_LENGTH, range) 629 | 630 | def getAccelXSelfTest(self): 631 | return self.i2c.readBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_XA_ST_BIT) 632 | 633 | def setAccelXSelfTest(self, enabled): 634 | self.i2c.writeBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_XA_ST_BIT, enabled) 635 | 636 | def getAccelYSelfTest(self): 637 | return self.readBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_YA_ST_BIT) 638 | 639 | def setAccelYSelfTest(self, enabled): 640 | self.i2c.writeBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_YA_ST_BIT, enabled) 641 | 642 | def getAccelZSelfTest(self): 643 | return self.i2c.readBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_ZA_ST_BIT) 644 | 645 | def setAccelZSelfTest(self, enabled): 646 | self.i2c.writeBit(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_ZA_ST_BIT, enabled) 647 | 648 | def getFullScaleAccelRange(self): 649 | return self.i2c.readBits(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_AFS_SEL_BIT, self.MPU6050_ACONFIG_AFS_SEL_LENGTH) 650 | 651 | def setFullScaleAccelRange(self, value): 652 | self.i2c.writeBits(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_AFS_SEL_BIT, self.MPU6050_ACONFIG_AFS_SEL_LENGTH, value) 653 | 654 | def getDHPFMode(self): 655 | return self.i2c.readBits(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_ACCEL_HPF_BIT, self.MPU6050_ACONFIG_ACCEL_HPF_LENGTH) 656 | 657 | def setDHPFMode(self, bandwith): 658 | self.i2c.writeBits(self.MPU6050_RA_ACCEL_CONFIG, self.MPU6050_ACONFIG_ACCEL_HPF_BIT, self.MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth) 659 | 660 | def getFreefallDetectionThreshold(self): 661 | return self.i2c.readU8(self.MPU6050_RA_FF_THR) 662 | 663 | def setFreefallDetectionThreshold(self, treshold): 664 | self.i2c.write8(self.MPU6050_RA_FF_THR, treshold) 665 | 666 | def getFreefallDetectionDuration(self): 667 | return self.i2c.readU8(self.MPU6050_RA_FF_DUR) 668 | 669 | def setFreefallDetectionDuration(self, duration): 670 | self.i2c.write8(self.MPU6050_RA_FF_DUR) 671 | 672 | def getMotionDetectionThreshold(self): 673 | return self.i2c.readU8(self.MPU6050_RA_MOT_THR) 674 | 675 | def setMotionDetectionThreshold(self, treshold): 676 | self.i2c.write8(self.MPU6050_RA_MOT_THR, treshold) 677 | 678 | def getMotionDetectionDuration(self): 679 | return self.i2c.readU8(self.MPU6050_RA_MOT_DUR) 680 | 681 | def setMotionDetectionDuration(self, duration): 682 | self.i2c.write8(self.MPU6050_RA_MOT_DUR, duration) 683 | 684 | def getZeroMotionDetectionThreshold(self): 685 | return self.i2c.readU8(self.MPU6050_RA_ZRMOT_THR) 686 | 687 | def setZeroMotionDetectionThreshold(self, treshold): 688 | self.i2c.write8(self.MPU6050_RA_ZRMOT_THR, treshold) 689 | 690 | def getZeroMotionDetectionDuration(self): 691 | return self.i2c.readU8(self.MPU6050_RA_ZRMOT_DUR) 692 | 693 | def setZeroMotionDetectionDuration(self, duration): 694 | self.i2c.write8(self.MPU6050_RA_ZRMOT_DUR, duration) 695 | 696 | def getTempFIFOEnabled(self): 697 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_TEMP_FIFO_EN_BIT) 698 | 699 | def setTempFIFOEnabled(self, enabled): 700 | self.i2c.write8(self.MPU6050_RA_FIFO_EN, self.MPU6050_TEMP_FIFO_EN_BIT, enabled) 701 | 702 | def getXGyroFIFOEnabled(self): 703 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_XG_FIFO_EN_BIT) 704 | 705 | def setXGyroFIFOEnabled(self, enabled): 706 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_XG_FIFO_EN_BIT, enabled) 707 | 708 | def getYGyroFIFOEnabled(self): 709 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_YG_FIFO_EN_BIT) 710 | 711 | def setYGyroFIFOEnabled(self, enabled): 712 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_YG_FIFO_EN_BIT, enabled) 713 | 714 | def getZGyroFIFOEnabled(self): 715 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_ZG_FIFO_EN_BIT) 716 | 717 | def setZGyroFIFOEnabled(self, enabled): 718 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_ZG_FIFO_EN_BIT, enabled) 719 | 720 | def getAccelFIFOEnabled(self): 721 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_ACCEL_FIFO_EN_BIT) 722 | 723 | def setAccelFIFOEnabled(self, enabled): 724 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_ACCEL_FIFO_EN_BIT, enabled) 725 | 726 | def getSlave2FIFOEnabled(self): 727 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV2_FIFO_EN_BIT) 728 | 729 | def setSlave2FIFOEnabled(self, enabled): 730 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV2_FIFO_EN_BIT, enabled) 731 | 732 | def getSlave1FIFOEnabled(self): 733 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV1_FIFO_EN_BIT) 734 | 735 | def setSlave1FIFOEnabled(self, enabled): 736 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV1_FIFO_EN_BIT, enabled) 737 | 738 | def getSlave0FIFOEnabled(self): 739 | return self.i2c.readBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV0_FIFO_EN_BIT) 740 | 741 | def setSlave0FIFOEnabled(self, enabled): 742 | self.i2c.writeBit(self.MPU6050_RA_FIFO_EN, self.MPU6050_SLV0_FIFO_EN_BIT, enabled) 743 | 744 | def getMultiMasterEnabled(self): 745 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_MULT_MST_EN_BIT) 746 | 747 | def setMultiMasterEnabled(self, enabled): 748 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_MULT_MST_EN_BIT, enabled) 749 | 750 | def getWaitForExternalSensorEnabled(self): 751 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_WAIT_FOR_ES_BIT) 752 | 753 | def setWaitForExternalSensorEnabled(self, value): 754 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_WAIT_FOR_ES_BIT, enabled) 755 | 756 | def getSlave3FIFOEnabled(self): 757 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_SLV_3_FIFO_EN_BIT) 758 | 759 | def setSlave3FIFOEnabled(self, enabled): 760 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_SLV_3_FIFO_EN_BIT, enabled) 761 | 762 | def getSlaveReadWriteTransitionEnabled(self): 763 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_I2C_MST_P_NSR_BIT) 764 | 765 | def setSlaveReadWriteTransitionEnabled(self, enabled): 766 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_I2C_MST_P_NSR_BIT, enabled) 767 | 768 | def getMasterClockSpeed(self): 769 | return self.i2c.readBits(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_I2C_MST_CLK_BIT, self.MPU6050_I2C_MST_CLK_LENGTH) 770 | 771 | def setMasterClockSpeed(self, speed): 772 | self.i2c.writeBits(self.MPU6050_RA_I2C_MST_CTRL, self.MPU6050_I2C_MST_CLK_BIT, self.MPU6050_I2C_MST_CLK_LENGTH, speed) 773 | 774 | def getSlaveAddress(self, num): 775 | if num > 3: 776 | return 0 777 | 778 | return self.i2c.readU8(self.MPU6050_RA_I2C_SLV0_ADDR + num * 3) 779 | 780 | def setSlaveAddress(self, num, address): 781 | if num > 3: 782 | return 783 | self.i2c.write8(self.MPU6050_RA_I2C_SLV0_ADDR + num * 3, address) 784 | 785 | def getSlaveRegister(self, num): 786 | if num > 3: 787 | return 0 788 | 789 | return self.i2c.readU8(self.MPU6050_RA_I2C_SLV0_REG + num * 3) 790 | 791 | def setSlaveRegister(self, num, reg): 792 | if num > 3: 793 | return 794 | self.i2c.write8(self.MPU6050_RA_I2C_SLV0_REG + num * 3, reg) 795 | 796 | def getSlaveEnabled(self, num): 797 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_EN_BIT) 798 | 799 | def setSlaveEnabled(self, num, enabled): 800 | if num > 3: 801 | return 802 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_EN_BIT, enabled) 803 | 804 | def getSlaveWordByteSwap(self, num): 805 | if num > 3: 806 | return 0 807 | 808 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_BYTE_SW_BIT) 809 | 810 | def setSlaveWordByteSwap(self, num, enabled): 811 | if num > 3: 812 | return 813 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_BYTE_SW_BIT, enabled) 814 | 815 | 816 | def getSlaveWriteMode(self, num): 817 | if num > 3: 818 | return 0 819 | 820 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_REG_DIS_BIT) 821 | 822 | def setSlaveWriteMode(self, num, mode): 823 | if num > 3: 824 | return 825 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_REG_DIS_BIT, mode) 826 | 827 | def getSlaveWordGroupOffset(self, num): 828 | if num > 3: 829 | return 0 830 | 831 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_GRP_BIT) 832 | 833 | def setSlaveWordGroupOffset(self, num, enabled): 834 | if num > 3: 835 | return 836 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_GRP_BIT, enabled) 837 | 838 | def getSlaveDataLength(self, num): 839 | if num > 3: 840 | return 0 841 | 842 | return self.i2c.readBits(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_LEN_BIT, self.MPU6050_I2C_SLV_LEN_LENGTH) 843 | 844 | def setSlaveDataLength(self, num, length): 845 | if num > 3: 846 | return 847 | self.i2c.writeBits(self.MPU6050_RA_I2C_SLV0_CTRL + num * 3, self.MPU6050_I2C_SLV_LEN_BIT, self.MPU6050_I2C_SLV_LEN_LENGTH, length) 848 | 849 | def getSlave4Address(self): 850 | return self.i2c.readU8(self.MPU6050_RA_I2C_SLV4_ADDR) 851 | 852 | def setSlave4Address(self, address): 853 | self.i2c.write8(self.MPU6050_RA_I2C_SLV4_ADDR, address) 854 | 855 | def getSlave4Register(self): 856 | return self.i2c.readU8(self.MPU6050_RA_I2C_SLV4_REG) 857 | 858 | def setSlave4Register(self, reg): 859 | self.i2c.write8(self.MPU6050_RA_I2C_SLV4_REG, reg) 860 | 861 | def setSlave4OutputByte(self, data): 862 | self.i2c.write8(self.MPU6050_RA_I2C_SLV4_DO, data) 863 | 864 | def getSlave4Enabled(self): 865 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_EN_BIT) 866 | 867 | def setSlave4Enabled(self, enabled): 868 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_EN_BIT, enabled) 869 | 870 | def getSlave4InterruptEnabled(self): 871 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_INT_EN_BIT) 872 | 873 | def setSlave4InterruptEnabled(self, enabled): 874 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_INT_EN_BIT, enabled) 875 | 876 | def getSlave4WriteMode(self): 877 | return self.i2c.readBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_REG_DIS_BIT) 878 | 879 | def setSlave4WriteMode(self, mode): 880 | self.i2c.writeBit(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_REG_DIS_BIT, mode) 881 | 882 | def getSlave4MasterDelay(self): 883 | return self.i2c.readBits(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_MST_DLY_BIT, self.MPU6050_I2C_SLV4_MST_DLY_LENGTH) 884 | 885 | def setSlave4MasterDelay(self, delay): 886 | self.i2c.writeBits(self.MPU6050_RA_I2C_SLV4_CTRL, self.MPU6050_I2C_SLV4_MST_DLY_BIT, self.MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay) 887 | 888 | def getSlate4InputByte(self): 889 | return self.i2c.readU8(self.MPU6050_RA_I2C_SLV4_DI) 890 | 891 | def getPassthroughStatus(self): 892 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_PASS_THROUGH_BIT) 893 | 894 | def getSlave4IsDone(self): 895 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV4_DONE_BIT) 896 | 897 | def getLostArbitration(self): 898 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_LOST_ARB_BIT) 899 | 900 | def getSlave4Nack(self): 901 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV4_NACK_BIT) 902 | 903 | def getSlave3Nack(self): 904 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV3_NACK_BIT) 905 | 906 | def getSlave2Nack(self): 907 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV2_NACK_BIT) 908 | 909 | def getSlave1Nack(self): 910 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV1_NACK_BIT) 911 | 912 | def getSlave0Nack(self): 913 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_STATUS, self.MPU6050_MST_I2C_SLV0_NACK_BIT) 914 | 915 | def getInterruptMode(self): 916 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_LEVEL_BIT) 917 | 918 | def setInterruptMode(self, mode): 919 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_LEVEL_BIT, mode) 920 | 921 | def getInterruptDrive(self): 922 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_OPEN_BIT) 923 | 924 | def setInterruptDrive(self, drive): 925 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_OPEN_BIT, drive) 926 | 927 | def getInterruptLatch(self): 928 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_LATCH_INT_EN_BIT) 929 | 930 | def setInterruptLatch(self, latch): 931 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_LATCH_INT_EN_BIT, latch) 932 | 933 | def getInterruptLatchClear(self): 934 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_RD_CLEAR_BIT) 935 | 936 | def setInterruptLatchClear(self, clear): 937 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear) 938 | 939 | def getFSyncInterruptLevel(self): 940 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT) 941 | 942 | def setFSyncInterruptLevel(self, level): 943 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level) 944 | 945 | def getFSyncInterruptEnabled(self): 946 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_FSYNC_INT_EN_BIT) 947 | 948 | def setFSyncInterruptEnabled(self, enabled): 949 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled) 950 | 951 | def getI2CBypassEnabled(self): 952 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_I2C_BYPASS_EN_BIT) 953 | 954 | def setI2CBypassEnabled(self, enabled): 955 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled) 956 | 957 | def getClockOutputEnabled(self): 958 | return self.i2c.readBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_CLKOUT_EN_BIT) 959 | 960 | def setClockOutputEnabled(self, enabled): 961 | self.i2c.writeBit(self.MPU6050_RA_INT_PIN_CFG, self.MPU6050_INTCFG_CLKOUT_EN_BIT, enabled) 962 | 963 | def getIntEnabled(self): 964 | return self.i2c.readU8(self.MPU6050_RA_INT_ENABLE) 965 | 966 | def setIntEnabled(self, status): 967 | self.i2c.write8(self.MPU6050_RA_INT_ENABLE, status) 968 | 969 | def getIntFreefallEnabled(self): 970 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_FF_BIT) 971 | 972 | def setIntFreefallEnabled(self, enabled): 973 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_FF_BIT, enabled) 974 | 975 | def getIntMotionEnabled(self): 976 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_MOT_BIT) 977 | 978 | def setIntMotionEnabled(self, enabled): 979 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_MOT_BIT, enabled) 980 | 981 | def getIntZeroMotionEnabled(self): 982 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_ZMOT_BIT) 983 | 984 | def setIntZeroMotionEnabled(self, enabled): 985 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_ZMOT_BIT, enabled) 986 | 987 | def getIntFIFOBufferOverflowEnabled(self): 988 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_FIFO_OFLOW_BIT) 989 | 990 | def setIntFIFOBufferOverflowEnabled(self, enabled): 991 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled) 992 | 993 | def getIntI2CMasterEnabled(self): 994 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_I2C_MST_INT_BIT) 995 | 996 | def setIntI2CMasterEnabled(self, enabled): 997 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled) 998 | 999 | def getIntDataReadyEnabled(self): 1000 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_DATA_RDY_BIT) 1001 | 1002 | def setIntDataReadyEnabled(self, enabled): 1003 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_DATA_RDY_BIT, enabled) 1004 | 1005 | def getIntStatus(self): 1006 | return self.i2c.readU8(self.MPU6050_RA_INT_STATUS) 1007 | 1008 | def getIntFreefallStatus(self): 1009 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_FF_BIT) 1010 | 1011 | def getIntMotionStatus(self): 1012 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_MOT_BIT) 1013 | 1014 | def getIntZeroMotionStatus(self): 1015 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_ZMOT_BIT) 1016 | 1017 | def getIntFIFOBufferOverflowStatus(self): 1018 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_FIFO_OFLOW_BIT) 1019 | 1020 | def getIntI2CMasterStatus(self): 1021 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_I2C_MST_INT_BIT) 1022 | 1023 | def getIntDataReadyStatus(self): 1024 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_DATA_RDY_BIT) 1025 | 1026 | def getMotion9(self): 1027 | # unknown 1028 | pass 1029 | 1030 | def getMotion6(self): 1031 | pass 1032 | 1033 | def getAcceleration(self): 1034 | pass 1035 | 1036 | def getAccelerationX(self): 1037 | pass 1038 | 1039 | def getAccelerationY(self): 1040 | pass 1041 | 1042 | def getAccelerationZ(self): 1043 | pass 1044 | 1045 | def getTemperature(self): 1046 | pass 1047 | 1048 | def getRotation(self): 1049 | pass 1050 | 1051 | def getRotationX(self): 1052 | pass 1053 | 1054 | def getRotationY(self): 1055 | pass 1056 | 1057 | def getRotationZ(self): 1058 | pass 1059 | 1060 | def getExternalSensorByte(self, position): 1061 | return self.i2c.readU8(self.MPU6050_RA_EXT_SENS_DATA_00 + position) 1062 | 1063 | def getExternalSensorWord(self, position): 1064 | pass 1065 | 1066 | def getExternalSensorDWord(self, position): 1067 | pass 1068 | 1069 | def getXNegMotionDetected(self): 1070 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_XNEG_BIT) 1071 | 1072 | def getXPosMotionDetected(self): 1073 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_XPOS_BIT) 1074 | 1075 | def getYNegMotionDetected(self): 1076 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_YNEG_BIT) 1077 | 1078 | def getYPosMotionDetected(self): 1079 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_YPOS_BIT) 1080 | 1081 | def getZNegMotionDetected(self): 1082 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_ZNEG_BIT) 1083 | 1084 | def getZPosMotionDetected(self): 1085 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_ZPOS_BIT) 1086 | 1087 | def getZeroMotionDetected(self): 1088 | return self.i2c.readBit(self.MPU6050_RA_MOT_DETECT_STATUS, self.MPU6050_MOTION_MOT_ZRMOT_BIT) 1089 | 1090 | def setSlaveOutputByte(self, num, data): 1091 | if num > 3: 1092 | return 1093 | self.i2c.write8(self.MPU6050_RA_I2C_SLV0_DO + num, data) 1094 | 1095 | def getExternalShadowDelayEnabled(self): 1096 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_DELAY_CTRL, self.MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT) 1097 | 1098 | def setExternalShadowDelayEnabled(self, enabled): 1099 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_DELAY_CTRL, self.MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled) 1100 | 1101 | def getSlaveDelayEnabled(self, num): 1102 | # // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. 1103 | if num > 4: 1104 | return 0 1105 | 1106 | return self.i2c.readBit(self.MPU6050_RA_I2C_MST_DELAY_CTRL, num) 1107 | 1108 | def setSlaveDelayEnabled(self, num, enabled): 1109 | self.i2c.writeBit(self.MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled) 1110 | 1111 | def resetGyroscopePath(self): 1112 | self.i2c.writeBit(self.MPU6050_RA_SIGNAL_PATH_RESET, self.MPU6050_PATHRESET_GYRO_RESET_BIT, True) 1113 | 1114 | def resetAccelerometerPath(self): 1115 | self.i2c.writeBit(self.MPU6050_RA_SIGNAL_PATH_RESET, self.MPU6050_PATHRESET_ACCEL_RESET_BIT, True) 1116 | 1117 | def resetTemperaturePath(self): 1118 | self.i2c.writeBit(self.MPU6050_RA_SIGNAL_PATH_RESET, self.MPU6050_PATHRESET_TEMP_RESET_BIT, True) 1119 | 1120 | def getAccelerometerPowerOnDelay(self): 1121 | return self.i2c.readBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_ACCEL_ON_DELAY_BIT, self.MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH) 1122 | 1123 | def setAccelerometerPowerOnDelay(self, delay): 1124 | self.i2c.writeBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_ACCEL_ON_DELAY_BIT, self.MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay) 1125 | 1126 | def getFreefallDetectionCounterDecrement(self): 1127 | return self.i2c.readBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_FF_COUNT_BIT, self.MPU6050_DETECT_FF_COUNT_LENGTH) 1128 | 1129 | def setFreefallDetectionCounterDecrement(self, decrement): 1130 | self.i2c.writeBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_FF_COUNT_BIT, self.MPU6050_DETECT_FF_COUNT_LENGTH, decrement) 1131 | 1132 | def getMotionDetectionCounterDecrement(self): 1133 | return self.i2c.readBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_MOT_COUNT_BIT, self.MPU6050_DETECT_MOT_COUNT_LENGTH) 1134 | 1135 | def setMotionDetectionCounterDecrement(self, decrement): 1136 | self.i2c.writeBits(self.MPU6050_RA_MOT_DETECT_CTRL, self.MPU6050_DETECT_MOT_COUNT_BIT, self.MPU6050_DETECT_MOT_COUNT_LENGTH, decrement) 1137 | 1138 | def getFIFOEnabled(self): 1139 | return self.i2c.readBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_FIFO_EN_BIT) 1140 | 1141 | def setFIFOEnabled(self, status): 1142 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_FIFO_EN_BIT, status) 1143 | 1144 | def getI2CMasterModeEnabled(self): 1145 | return self.i2c.readBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_I2C_MST_EN_BIT) 1146 | 1147 | def setI2CMasterModeEnabled(self, status): 1148 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_I2C_MST_EN_BIT, status) 1149 | 1150 | def switchSPIEnabled(self, enabled): 1151 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled) 1152 | 1153 | def resetFIFO(self): 1154 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_FIFO_RESET_BIT, True) 1155 | 1156 | def resetI2CMaster(self): 1157 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_I2C_MST_RESET_BIT, True) 1158 | 1159 | def resetSensors(self): 1160 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_SIG_COND_RESET_BIT, True) 1161 | 1162 | def reset(self): 1163 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_DEVICE_RESET_BIT, True) 1164 | 1165 | def getSleepEnabled(self): 1166 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_SLEEP_BIT) 1167 | 1168 | def setSleepEnabled(self, status): 1169 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_SLEEP_BIT, status) 1170 | 1171 | def getWakeCycleEnabled(self): 1172 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_CYCLE_BIT) 1173 | 1174 | def setWakeCycleEnabled(self, enabled): 1175 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_CYCLE_BIT, enabled) 1176 | 1177 | def getTempSensorEnabled(self): 1178 | result = self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_TEMP_DIS_BIT) 1179 | return result == 0 # 1 is actually disabled here 1180 | 1181 | def setTempSensorEnabled(self, enabled): 1182 | # 1 is actually disabled here 1183 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_TEMP_DIS_BIT, enabled != enabled) 1184 | 1185 | def getClockSource(self): 1186 | return self.i2c.readBits(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_CLKSEL_BIT, self.MPU6050_PWR1_CLKSEL_LENGTH) 1187 | 1188 | def setClockSource(self, source): 1189 | self.i2c.writeBits(self.MPU6050_RA_PWR_MGMT_1, self.MPU6050_PWR1_CLKSEL_BIT, self.MPU6050_PWR1_CLKSEL_LENGTH, source) 1190 | 1191 | def getWakeFrequency(self): 1192 | return self.i2c.readBits(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_LP_WAKE_CTRL_BIT, self.MPU6050_PWR2_LP_WAKE_CTRL_LENGTH) 1193 | 1194 | def setWakeFrequency(self, frequency): 1195 | self.i2c.writeBits(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_LP_WAKE_CTRL_BIT, self.MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency) 1196 | 1197 | def getStandbyXAccelEnabled(self): 1198 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_XA_BIT) 1199 | 1200 | def setStandbyXAccelEnabled(self, enabled): 1201 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_XA_BIT, enabled) 1202 | 1203 | def getStandbyYAccelEnabled(self): 1204 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_YA_BIT) 1205 | 1206 | def setStandbyYAccelEnabled(self, enabled): 1207 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_YA_BIT, enabled) 1208 | 1209 | def getStandbyZAccelEnabled(self): 1210 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_ZA_BIT) 1211 | 1212 | def setStandbyZAccelEnabled(self, enabled): 1213 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_ZA_BIT, enabled) 1214 | 1215 | def getStandbyXGyroEnabled(self): 1216 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_XG_BIT) 1217 | 1218 | def setStandbyXGyroEnabled(self, enabled): 1219 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_XG_BIT, enabled) 1220 | 1221 | def getStandbyYGyroEnabled(self): 1222 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_YG_BIT) 1223 | 1224 | def setStandbyYGyroEnabled(self, enabled): 1225 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_YG_BIT, enabled) 1226 | 1227 | def getStandbyZGyroEnabled(self): 1228 | return self.i2c.readBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_ZG_BIT) 1229 | 1230 | def setStandbyZGyroEnabled(self, enabled): 1231 | self.i2c.writeBit(self.MPU6050_RA_PWR_MGMT_2, self.MPU6050_PWR2_STBY_ZG_BIT, enabled) 1232 | 1233 | def getFIFOCount(self): 1234 | return self.i2c.readU16(self.MPU6050_RA_FIFO_COUNTH) 1235 | 1236 | def getFIFOByte(self): 1237 | return self.i2c.readU8(self.MPU6050_RA_FIFO_R_W) 1238 | 1239 | def getFIFOBytes(self,length): 1240 | return self.i2c.readBytes(self.MPU6050_RA_FIFO_R_W, length) 1241 | 1242 | def setFIFOByte(self, data): 1243 | self.i2c.write8(self.MPU6050_RA_FIFO_R_W, data) 1244 | 1245 | def getDeviceID(self): 1246 | return self.i2c.readBits(self.MPU6050_RA_WHO_AM_I, self.MPU6050_WHO_AM_I_BIT, self.MPU6050_WHO_AM_I_LENGTH) 1247 | 1248 | def setDeviceID(self, id): 1249 | self.i2c.writeBits(self.MPU6050_RA_WHO_AM_I, self.MPU6050_WHO_AM_I_BIT, self.MPU6050_WHO_AM_I_LENGTH, id) 1250 | 1251 | def getOTPBankValid(self): 1252 | result = self.i2c.readBit(self.MPU6050_RA_XG_OFFS_TC, self.MPU6050_TC_OTP_BNK_VLD_BIT) 1253 | return result 1254 | 1255 | def setOTPBankValid(self, status): 1256 | self.i2c.writeBit(self.MPU6050_RA_XG_OFFS_TC, self.MPU6050_TC_OTP_BNK_VLD_BIT, status) 1257 | 1258 | def getXGyroOffset(self): 1259 | return self.i2c.readBits(self.MPU6050_RA_XG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH) 1260 | 1261 | def setXGyroOffset(self, offset): 1262 | self.i2c.writeBits(self.MPU6050_RA_XG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH, offset) 1263 | 1264 | def getYGyroOffset(self): 1265 | return self.i2c.readBits(self.MPU6050_RA_YG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH) 1266 | 1267 | def setYGyroOffset(self, offset): 1268 | self.i2c.writeBits(self.MPU6050_RA_YG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH, offset) 1269 | 1270 | def getZGyroOffset(self): 1271 | return self.i2c.readBits(self.MPU6050_RA_ZG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH) 1272 | 1273 | def setZGyroOffset(self, offset): 1274 | self.i2c.writeBits(self.MPU6050_RA_ZG_OFFS_TC, self.MPU6050_TC_OFFSET_BIT, self.MPU6050_TC_OFFSET_LENGTH, offset) 1275 | 1276 | def getXFineGain(self): 1277 | return self.i2c.readU8(self.MPU6050_RA_X_FINE_GAIN) 1278 | 1279 | def setXFineGain(self, gain): 1280 | self.i2c.write8(self.MPU6050_RA_X_FINE_GAIN, gain) 1281 | 1282 | def getYFineGain(self): 1283 | return self.i2c.readU8(self.MPU6050_RA_Y_FINE_GAIN) 1284 | 1285 | def setYFineGain(self, gain): 1286 | self.i2c.write8(self.MPU6050_RA_Y_FINE_GAIN, gain) 1287 | 1288 | def getZFineGain(self): 1289 | return self.i2c.readU8(self.MPU6050_RA_Z_FINE_GAIN) 1290 | 1291 | def setZFineGain(self, gain): 1292 | self.i2c.write8(self.MPU6050_RA_Z_FINE_GAIN, gain) 1293 | 1294 | def getXAccelOffset(self): 1295 | pass 1296 | 1297 | def setXAccelOffset(self, offset): 1298 | pass 1299 | 1300 | def getYAccelOffset(self): 1301 | pass 1302 | 1303 | def setYAccelOffset(self, offset): 1304 | pass 1305 | 1306 | def getZAccelOffset(self): 1307 | pass 1308 | 1309 | def setZAccelOffset(self, offset): 1310 | pass 1311 | 1312 | def getXGyroOffsetUser(self): 1313 | pass 1314 | 1315 | def setXGyroOffsetUser(self, value): 1316 | self.i2c.write8(self.MPU6050_RA_XG_OFFS_USRH, value >> 8) 1317 | self.i2c.write8(self.MPU6050_RA_XG_OFFS_USRL, value & 0xFF) 1318 | return True 1319 | 1320 | def getYGyroOffsetUser(self): 1321 | pass 1322 | 1323 | def setYGyroOffsetUser(self, value): 1324 | self.i2c.write8(self.MPU6050_RA_YG_OFFS_USRH, value >> 8) 1325 | self.i2c.write8(self.MPU6050_RA_YG_OFFS_USRL, value & 0xFF) 1326 | return True 1327 | 1328 | def getZGyroOffsetUser(self): 1329 | pass 1330 | 1331 | def setZGyroOffsetUser(self, value): 1332 | self.i2c.write8(self.MPU6050_RA_ZG_OFFS_USRH, value >> 8) 1333 | self.i2c.write8(self.MPU6050_RA_ZG_OFFS_USRL, value & 0xFF) 1334 | return True 1335 | 1336 | def getIntPLLReadyEnabled(self): 1337 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_PLL_RDY_INT_BIT) 1338 | 1339 | def setIntPLLReadyEnabled(self, enabled): 1340 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled) 1341 | 1342 | def getIntDMPEnabled(self): 1343 | return self.i2c.readBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_DMP_INT_BIT) 1344 | 1345 | def setIntDMPEnabled(self, enabled): 1346 | self.i2c.writeBit(self.MPU6050_RA_INT_ENABLE, self.MPU6050_INTERRUPT_DMP_INT_BIT, enabled) 1347 | 1348 | def getDMPInt5Status(self): 1349 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_5_BIT) 1350 | 1351 | def getDMPInt4Status(self): 1352 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_4_BIT) 1353 | 1354 | def getDMPInt3Status(self): 1355 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_3_BIT) 1356 | 1357 | def getDMPInt2Status(self): 1358 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_2_BIT) 1359 | 1360 | def getDMPInt1Status(self): 1361 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_1_BIT) 1362 | 1363 | def getDMPInt0Status(self): 1364 | return self.i2c.readBit(self.MPU6050_RA_DMP_INT_STATUS, self.MPU6050_DMPINT_0_BIT) 1365 | 1366 | def getIntPLLReadyStatus(self): 1367 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_PLL_RDY_INT_BIT) 1368 | 1369 | def getIntDMPStatus(self): 1370 | return self.i2c.readBit(self.MPU6050_RA_INT_STATUS, self.MPU6050_INTERRUPT_DMP_INT_BIT) 1371 | 1372 | def getDMPEnabled(self): 1373 | return self.i2c.readBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_DMP_EN_BIT) 1374 | 1375 | def setDMPEnabled(self, status): 1376 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_DMP_EN_BIT, status) 1377 | 1378 | def resetDMP(self): 1379 | self.i2c.writeBit(self.MPU6050_RA_USER_CTRL, self.MPU6050_USERCTRL_DMP_RESET_BIT, True) 1380 | 1381 | def setMemoryBank(self, bank, prefetchEnabled = False, userBank = False): 1382 | bank &= 0x1F 1383 | 1384 | if userBank: 1385 | bank |= 0x20 1386 | if prefetchEnabled: 1387 | bank |= 0x40 1388 | 1389 | self.i2c.write8(self.MPU6050_RA_BANK_SEL, bank) 1390 | return True 1391 | 1392 | def setMemoryStartAddress(self, address): 1393 | self.i2c.write8(self.MPU6050_RA_MEM_START_ADDR, address) 1394 | 1395 | def readMemoryByte(self): 1396 | result = self.i2c.readU8(self.MPU6050_RA_MEM_R_W) 1397 | return result 1398 | 1399 | def writeMemoryByte(self, data): 1400 | self.i2c.write8(self.MPU6050_RA_MEM_R_W, data) 1401 | 1402 | def readMemoryBlock(self): 1403 | pass 1404 | 1405 | def writeMemoryBlock(self, data, dataSize, bank = 0, address = 0, verify = False): 1406 | self.setMemoryBank(bank) 1407 | self.setMemoryStartAddress(address) 1408 | 1409 | i = 0 1410 | while i < dataSize: 1411 | self.i2c.write8(self.MPU6050_RA_MEM_R_W, data[i]) 1412 | 1413 | # Verify 1414 | if verify: 1415 | self.setMemoryBank(bank) 1416 | self.setMemoryStartAddress(address) 1417 | result = self.i2c.readU8(self.MPU6050_RA_MEM_R_W) 1418 | 1419 | if result != data[i]: 1420 | print(data[i]), 1421 | print(result), 1422 | print(address) 1423 | 1424 | # reset adress to 0 after reaching 255 1425 | if address == 255: 1426 | address = 0 1427 | bank += 1 1428 | 1429 | self.setMemoryBank(bank) 1430 | else: 1431 | address += 1 1432 | 1433 | self.setMemoryStartAddress(address) 1434 | 1435 | # increase byte index 1436 | i += 1 1437 | 1438 | 1439 | def writeDMPConfigurationSet(self, data, dataSize, bank = 0, address = 0, verify = False): 1440 | # config set data is a long string of blocks with the following structure: 1441 | # [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] 1442 | pos = 0 1443 | while pos < dataSize: 1444 | j = 0 1445 | dmpConfSet = [] 1446 | while ((j < 4) or (j < dmpConfSet[2] + 3)): 1447 | dmpConfSet.append(data[pos]) 1448 | j += 1 1449 | pos += 1 1450 | 1451 | # write data or perform special action 1452 | if dmpConfSet[2] > 0: 1453 | # regular block of data to write 1454 | self.writeMemoryBlock(dmpConfSet[3:], dmpConfSet[2], dmpConfSet[0], dmpConfSet[1], verify) 1455 | else: 1456 | # special instruction 1457 | # NOTE: this kind of behavior (what and when to do certain things) 1458 | # is totally undocumented. This code is in here based on observed 1459 | # behavior only, and exactly why (or even whether) it has to be here 1460 | # is anybody's guess for now. 1461 | if dmpConfSet[3] == 0x01: 1462 | # enable DMP-related interrupts 1463 | 1464 | #setIntZeroMotionEnabled(true); 1465 | #setIntFIFOBufferOverflowEnabled(true); 1466 | #setIntDMPEnabled(true); 1467 | self.i2c.write8(self.MPU6050_RA_INT_ENABLE, 0x32); # single operation 1468 | 1469 | 1470 | def getDMPConfig1(self): 1471 | self.i2c.readU8(self.MPU6050_RA_DMP_CFG_1) 1472 | 1473 | def setDMPConfig1(self, config): 1474 | self.i2c.write8(self.MPU6050_RA_DMP_CFG_1, config) 1475 | 1476 | def getDMPConfig2(self): 1477 | return self.i2c.readU8(self.MPU6050_RA_DMP_CFG_2) 1478 | 1479 | def setDMPConfig2(self, config): 1480 | self.i2c.write8(self.MPU6050_RA_DMP_CFG_2, config) 1481 | 1482 | def dmpPacketAvailable(self): 1483 | return self.getFIFOCount() >= self.dmpGetFIFOPacketSize() 1484 | 1485 | def dmpGetFIFOPacketSize(self): 1486 | return self.dmpPacketSize 1487 | 1488 | def dmpGetAccel(self): 1489 | pass 1490 | 1491 | def dmpGetQuaternion(self, packet): 1492 | # We are dealing with signed bytes 1493 | if packet[0] > 127: 1494 | packet[0] -= 256 1495 | 1496 | if packet[4] > 127: 1497 | packet[4] -= 256 1498 | 1499 | if packet[8] > 127: 1500 | packet[8] -= 256 1501 | 1502 | if packet[12] > 127: 1503 | packet[12] -= 256 1504 | 1505 | data = { 1506 | 'w' : ((packet[0] << 8) + packet[1]) / 16384.0, 1507 | 'x' : ((packet[4] << 8) + packet[5]) / 16384.0, 1508 | 'y' : ((packet[8] << 8) + packet[9]) / 16384.0, 1509 | 'z' : ((packet[12] << 8) + packet[13]) / 16384.0} 1510 | 1511 | return data 1512 | 1513 | def dmpGetGyro(self): 1514 | pass 1515 | 1516 | def dmpGetLinearAccel(self): 1517 | pass 1518 | 1519 | def dmpGetLinearAccelInWorld(self): 1520 | pass 1521 | 1522 | def dmpGetGravity(self, q): 1523 | data = { 1524 | 'x' : float(2 * (q['x'] * q['z'] - q['w'] * q['y'])), 1525 | 'y' : float(2 * (q['w'] * q['x'] + q['y'] * q['z'])), 1526 | 'z' : float(q['w'] * q['w'] - q['x'] * q['x'] - q['y'] * q['y'] + q['z'] * q['z'])} 1527 | 1528 | return data 1529 | 1530 | def dmpGetEuler(self, q): 1531 | pass 1532 | 1533 | def dmpGetYawPitchRoll(self, q, g): 1534 | data = { 1535 | # yaw: (about Z axis) 1536 | 'yaw' : atan2(2 * q['x'] * q['y'] - 2 * q['w'] * q['z'], 2 * q['w'] * q['w'] + 2 * q['x'] * q['x'] - 1), 1537 | # pitch: (nose up/down, about Y axis) 1538 | 'pitch' : atan(g['x'] / sqrt(g['y'] * g['y'] + g['z'] * g['z'])), 1539 | # roll: (tilt left/right, about X axis) 1540 | 'roll' : atan(g['y'] / sqrt(g['x'] * g['x'] + g['z'] * g['z']))} 1541 | 1542 | return data 1543 | 1544 | def dmpProcessFIFOPacket(self): 1545 | pass 1546 | 1547 | def dmpReadAndProcessFIFOPacket(self): 1548 | pass 1549 | 1550 | def dmpInitialize(self): 1551 | # Resetting MPU6050 1552 | self.reset() 1553 | sleep(0.05) # wait after reset 1554 | 1555 | # Disable sleep mode 1556 | self.setSleepEnabled(False) 1557 | 1558 | # get MPU hardware revision 1559 | self.setMemoryBank(0x10, True, True) # Selecting user bank 16 1560 | self.setMemoryStartAddress(0x06) # Selecting memory byte 6 1561 | hwRevision = self.readMemoryByte() # Checking hardware revision 1562 | #print('Revision @ user[16][6] ='), 1563 | #print(hex(hwRevision)) 1564 | self.setMemoryBank(0, False, False) # Resetting memory bank selection to 0 1565 | 1566 | # get X/Y/Z gyro offsets 1567 | xgOffset = self.getXGyroOffset() 1568 | ygOffset = self.getYGyroOffset() 1569 | zgOffset = self.getZGyroOffset() 1570 | 1571 | # Enable pass through mode 1572 | self.setI2CBypassEnabled(True) 1573 | 1574 | # load DMP code into memory banks 1575 | self.writeMemoryBlock(self.dmpMemory, self.MPU6050_DMP_CODE_SIZE, 0, 0, False) 1576 | #print('Success! DMP code written and verified') 1577 | 1578 | # write DMP configuration 1579 | self.writeDMPConfigurationSet(self.dmpConfig, self.MPU6050_DMP_CONFIG_SIZE, 0, 0, False) 1580 | #print('Success! DMP configuration written and verified') 1581 | 1582 | # Setting clock source to Z Gyro 1583 | self.setClockSource(self.MPU6050_CLOCK_PLL_ZGYRO) 1584 | 1585 | # Setting DMP and FIFO_OFLOW interrupts enabled 1586 | self.setIntEnabled(0x12) 1587 | 1588 | # Setting sample rate to 200Hz 1589 | self.setRate(4) # 1khz / (1 + 4) = 200 Hz [9 = 100 Hz] 1590 | 1591 | # Setting external frame sync to TEMP_OUT_L[0] 1592 | self.setExternalFrameSync(self.MPU6050_EXT_SYNC_TEMP_OUT_L) 1593 | 1594 | # Setting DLPF bandwidth to 42Hz 1595 | self.setDLPFMode(self.MPU6050_DLPF_BW_42) 1596 | 1597 | # Setting gyro sensitivity to +/- 2000 deg/sec 1598 | self.setFullScaleGyroRange(self.MPU6050_GYRO_FS_2000) 1599 | 1600 | # Setting DMP configuration bytes (function unknown) 1601 | self.setDMPConfig1(0x03) 1602 | self.setDMPConfig2(0x00) 1603 | 1604 | # Clearing OTP Bank flag 1605 | self.setOTPBankValid(False) 1606 | 1607 | # Setting X/Y/Z gyro offsets to previous values 1608 | #self.setXGyroOffset(xgOffset); 1609 | #self.setYGyroOffset(ygOffset); 1610 | #self.setZGyroOffset(zgOffset); 1611 | 1612 | # Setting X/Y/Z gyro user offsets to zero 1613 | self.setXGyroOffsetUser(0) 1614 | self.setYGyroOffsetUser(0) 1615 | self.setZGyroOffsetUser(0) 1616 | 1617 | # Writing final memory update 1/7 (function unknown) 1618 | pos = 0 1619 | j = 0 1620 | dmpUpdate = [] 1621 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1622 | dmpUpdate.append(self.dmpUpdates[pos]) 1623 | j += 1 1624 | pos += 1 1625 | 1626 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1627 | 1628 | # Writing final memory update 2/7 (function unknown) 1629 | j = 0 1630 | dmpUpdate = [] 1631 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1632 | dmpUpdate.append(self.dmpUpdates[pos]) 1633 | j += 1 1634 | pos += 1 1635 | 1636 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1637 | 1638 | # Resetting FIFO 1639 | self.resetFIFO() 1640 | 1641 | # Reading FIFO count 1642 | fifoCount = self.getFIFOCount() 1643 | #print('Current FIFO count = %s' % fifoCount) 1644 | 1645 | # Setting motion detection threshold to 2 1646 | self.setMotionDetectionThreshold(2) 1647 | 1648 | # Setting zero-motion detection threshold to 156 1649 | self.setZeroMotionDetectionThreshold(156) 1650 | 1651 | # Setting motion detection duration to 80 1652 | self.setMotionDetectionDuration(80) 1653 | 1654 | # Setting zero-motion detection duration to 0 1655 | self.setZeroMotionDetectionDuration(0) 1656 | 1657 | # Resetting FIFO 1658 | self.resetFIFO() 1659 | 1660 | # Enabling FIFO 1661 | self.setFIFOEnabled(True) 1662 | 1663 | # Enabling DMP 1664 | self.setDMPEnabled(True) 1665 | 1666 | # Resetting DMP 1667 | self.resetDMP() 1668 | 1669 | # Writing final memory update 3/7 (function unknown) 1670 | j = 0 1671 | dmpUpdate = [] 1672 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1673 | dmpUpdate.append(self.dmpUpdates[pos]) 1674 | j += 1 1675 | pos += 1 1676 | 1677 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1678 | 1679 | # Writing final memory update 4/7 (function unknown) 1680 | j = 0 1681 | dmpUpdate = [] 1682 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1683 | dmpUpdate.append(self.dmpUpdates[pos]) 1684 | j += 1 1685 | pos += 1 1686 | 1687 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1688 | 1689 | # Writing final memory update 5/7 (function unknown) 1690 | j = 0 1691 | dmpUpdate = [] 1692 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1693 | dmpUpdate.append(self.dmpUpdates[pos]) 1694 | j += 1 1695 | pos += 1 1696 | 1697 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1698 | 1699 | # Waiting for FIFO count > 2 1700 | while (self.getFIFOCount() < 3): 1701 | fifoCount = self.getFIFOCount() 1702 | #print('Current FIFO count ='), 1703 | #print(fifoCount) 1704 | 1705 | # Reading FIFO data 1706 | self.getFIFOBytes(fifoCount) 1707 | 1708 | # Writing final memory update 6/7 (function unknown) 1709 | j = 0 1710 | dmpUpdate = [] 1711 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1712 | dmpUpdate.append(self.dmpUpdates[pos]) 1713 | j += 1 1714 | pos += 1 1715 | 1716 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1717 | 1718 | # Writing final memory update 7/7 (function unknown) 1719 | j = 0 1720 | dmpUpdate = [] 1721 | while ((j < 4) or (j < dmpUpdate[2] + 3)): 1722 | dmpUpdate.append(self.dmpUpdates[pos]) 1723 | j += 1 1724 | pos += 1 1725 | 1726 | self.writeMemoryBlock(dmpUpdate[3:], dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], True) 1727 | 1728 | # Disabling DMP (you turn it on later) 1729 | self.setDMPEnabled(False) 1730 | 1731 | # Setting up internal 42-byte (default) DMP packet buffer 1732 | self.dmpPacketSize = 42 1733 | 1734 | # Resetting FIFO and clearing INT status one last time 1735 | self.resetFIFO() 1736 | self.getIntStatus() -------------------------------------------------------------------------------- /PCA9685/pca9685.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Python Standard Library Imports 4 | import time 5 | import math 6 | 7 | # External Imports 8 | pass 9 | 10 | # Custom Imports 11 | from pycomms import PyComms 12 | 13 | # ============================================================================ 14 | # Adafruit PCA9685 16-Channel PWM Servo Driver (slightly modified) 15 | # ============================================================================ 16 | 17 | class PCA9685(): 18 | i2c = None 19 | 20 | # Registers/etc. 21 | __SUBADR1 = 0x02 22 | __SUBADR2 = 0x03 23 | __SUBADR3 = 0x04 24 | __MODE1 = 0x00 25 | __PRESCALE = 0xFE 26 | __LED0_ON_L = 0x06 27 | __LED0_ON_H = 0x07 28 | __LED0_OFF_L = 0x08 29 | __LED0_OFF_H = 0x09 30 | __ALLLED_ON_L = 0xFA 31 | __ALLLED_ON_H = 0xFB 32 | __ALLLED_OFF_L = 0xFC 33 | __ALLLED_OFF_H = 0xFD 34 | 35 | def __init__(self, address = 0x40): 36 | self.i2c = PyComms(address) 37 | self.address = address 38 | self.i2c.write8(self.__MODE1, 0x00) 39 | 40 | def setPWMFreq(self, freq): 41 | # Sets the PWM frequency" 42 | prescaleval = 25000000.0 # 25MHz 43 | prescaleval /= 4096.0 # 12-bit 44 | prescaleval /= float(freq) 45 | prescaleval -= 1.0 46 | prescale = math.floor(prescaleval + 0.5) 47 | 48 | oldmode = self.i2c.readU8(self.__MODE1); 49 | newmode = (oldmode & 0x7F) | 0x10 # sleep 50 | self.i2c.write8(self.__MODE1, newmode) # go to sleep 51 | self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) 52 | self.i2c.write8(self.__MODE1, oldmode) 53 | time.sleep(0.005) 54 | self.i2c.write8(self.__MODE1, oldmode | 0x80) 55 | 56 | def setPWM(self, channel, on, off): 57 | # Sets a single PWM channel 58 | self.i2c.write8(self.__LED0_ON_L + 4 * channel, on & 0xFF) 59 | self.i2c.write8(self.__LED0_ON_H + 4 * channel, on >> 8) 60 | self.i2c.write8(self.__LED0_OFF_L + 4 * channel, off & 0xFF) 61 | self.i2c.write8(self.__LED0_OFF_H + 4 * channel, off >> 8) -------------------------------------------------------------------------------- /PyComms/pycomms.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Python Standard Library Imports 4 | import smbus 5 | 6 | # External Imports 7 | pass 8 | 9 | # Custom Imports 10 | pass 11 | 12 | # =========================================================================== 13 | # PyComms I2C Base Class (an rewriten Adafruit_I2C pythone class clone) 14 | # =========================================================================== 15 | 16 | class PyComms: 17 | def __init__(self, address, bus = smbus.SMBus(0)): 18 | self.address = address 19 | self.bus = bus 20 | 21 | def reverseByteOrder(self, data): 22 | # Reverses the byte order of an int (16-bit) or long (32-bit) value 23 | # Courtesy Vishal Sapre 24 | dstr = hex(data)[2:].replace('L','') 25 | byteCount = len(dstr[::2]) 26 | val = 0 27 | for i, n in enumerate(range(byteCount)): 28 | d = data & 0xFF 29 | val |= (d << (8 * (byteCount - i - 1))) 30 | data >>= 8 31 | return val 32 | 33 | def readBit(self, reg, bitNum): 34 | b = self.readU8(reg) 35 | data = b & (1 << bitNum) 36 | return data 37 | 38 | def writeBit(self, reg, bitNum, data): 39 | b = self.readU8(reg) 40 | 41 | if data != 0: 42 | b = (b | (1 << bitNum)) 43 | else: 44 | b = (b & ~(1 << bitNum)) 45 | 46 | return self.write8(reg, b) 47 | 48 | def readBits(self, reg, bitStart, length): 49 | # 01101001 read byte 50 | # 76543210 bit numbers 51 | # xxx args: bitStart=4, length=3 52 | # 010 masked 53 | # -> 010 shifted 54 | 55 | b = self.readU8(reg) 56 | mask = ((1 << length) - 1) << (bitStart - length + 1) 57 | b &= mask 58 | b >>= (bitStart - length + 1) 59 | 60 | return b 61 | 62 | 63 | def writeBits(self, reg, bitStart, length, data): 64 | # 010 value to write 65 | # 76543210 bit numbers 66 | # xxx args: bitStart=4, length=3 67 | # 00011100 mask byte 68 | # 10101111 original value (sample) 69 | # 10100011 original & ~mask 70 | # 10101011 masked | value 71 | 72 | b = self.readU8(reg) 73 | mask = ((1 << length) - 1) << (bitStart - length + 1) 74 | data <<= (bitStart - length + 1) 75 | data &= mask 76 | b &= ~(mask) 77 | b |= data 78 | 79 | return self.write8(reg, b) 80 | 81 | def readBytes(self, reg, length): 82 | output = [] 83 | 84 | i = 0 85 | while i < length: 86 | output.append(self.readU8(reg)) 87 | i += 1 88 | 89 | return output 90 | 91 | def readBytesListU(self, reg, length): 92 | output = [] 93 | 94 | i = 0 95 | while i < length: 96 | output.append(self.readU8(reg + i)) 97 | i += 1 98 | 99 | return output 100 | 101 | def readBytesListS(self, reg, length): 102 | output = [] 103 | 104 | i = 0 105 | while i < length: 106 | output.append(self.readS8(reg + i)) 107 | i += 1 108 | 109 | return output 110 | 111 | def writeList(self, reg, list): 112 | # Writes an array of bytes using I2C format" 113 | try: 114 | self.bus.write_i2c_block_data(self.address, reg, list) 115 | except (IOError): 116 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 117 | return -1 118 | 119 | def write8(self, reg, value): 120 | # Writes an 8-bit value to the specified register/address 121 | try: 122 | self.bus.write_byte_data(self.address, reg, value) 123 | except (IOError): 124 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 125 | return -1 126 | 127 | def readU8(self, reg): 128 | # Read an unsigned byte from the I2C device 129 | try: 130 | result = self.bus.read_byte_data(self.address, reg) 131 | return result 132 | except (IOError): 133 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 134 | return -1 135 | 136 | def readS8(self, reg): 137 | # Reads a signed byte from the I2C device 138 | try: 139 | result = self.bus.read_byte_data(self.address, reg) 140 | if result > 127: 141 | return result - 256 142 | else: 143 | return result 144 | except (IOError): 145 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 146 | return -1 147 | 148 | def readU16(self, reg): 149 | # Reads an unsigned 16-bit value from the I2C device 150 | try: 151 | hibyte = self.bus.read_byte_data(self.address, reg) 152 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg + 1) 153 | return result 154 | except (IOError): 155 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 156 | return -1 157 | 158 | def readS16(self, reg): 159 | # Reads a signed 16-bit value from the I2C device 160 | try: 161 | hibyte = self.bus.read_byte_data(self.address, reg) 162 | if hibyte > 127: 163 | hibyte -= 256 164 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg + 1) 165 | return result 166 | except (IOError): 167 | print ("Error accessing 0x%02X: Check your I2C address" % self.address) 168 | return -1 -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | PyComms is a simple wrapper class written around the smbus i2c python extension. 2 | 3 | I am personally trying to port a few sensor classes from arduino to python, because using i2c sensors became very popular on the Raspberry Pi platform, but there is no "pure python" implementation for any of the common i2c sensors like MPU6050, HMC5883L and many others, thats what this repo is for. 4 | 5 | Please bear in mind that some of the classes aren't fully ported (yet), so there might be bugs. 6 | Feel free to fork & modify the code in any way you see fit, if you extended support of any sensor class, please make a pull request so your changes will be distributed to others. 7 | 8 | Thank you --------------------------------------------------------------------------------