diff --git a/640imutest.py b/640imutest.py new file mode 100644 index 0000000..c783798 --- /dev/null +++ b/640imutest.py @@ -0,0 +1,65 @@ +""" +MS5611 driver code is placed under the BSD license. +Copyright (c) 2014, Emlid Limited, www.emlid.com +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Emlid Limited nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +""" + +import spidev +import time +import sys +from darkwater_640.mpu9250 import MPU9250 + +imu = MPU9250() + +if imu.testConnection(): + print "Connection established: True" +else: + sys.exit("Connection established: False") + +imu.initialize() + +time.sleep(1) + +while True: + # imu.read_all() + # imu.read_gyro() + # imu.read_acc() + # imu.read_temp() + # imu.read_mag() + + # print "Accelerometer: ", imu.accelerometer_data + # print "Gyroscope: ", imu.gyroscope_data + # print "Temperature: ", imu.temperature + # print "Magnetometer: ", imu.magnetometer_data + + # time.sleep(0.1) + + m9a, m9g, m9m = imu.getMotion9() + + print "Acc:", "{:+7.3f}".format(m9a[0]), "{:+7.3f}".format(m9a[1]), "{:+7.3f}".format(m9a[2]), + print " Gyr:", "{:+8.3f}".format(m9g[0]), "{:+8.3f}".format(m9g[1]), "{:+8.3f}".format(m9g[2]), + print " Mag:", "{:+7.3f}".format(m9m[0]), "{:+7.3f}".format(m9m[1]), "{:+7.3f}".format(m9m[2]) + + time.sleep(0.5) diff --git a/640motortest.py b/640motortest.py index 568d40f..c3d41ea 100644 --- a/640motortest.py +++ b/640motortest.py @@ -18,64 +18,64 @@ m6.off() time.sleep(1) ##time.sleep(10) -print "Set forward - " -print "Motor 1" +print("Set forward - ") +print("Motor 1") m1.setMotorSpeed(255) time.sleep(1) -print "Motor 2" +print("Motor 2") m2.setMotorSpeed(255) time.sleep(1) -print "Motor 3" +print("Motor 3") m3.setMotorSpeed(255) time.sleep(1) -print "Motor 4" +print("Motor 4") m4.setMotorSpeed(255) time.sleep(1) -print "Motor 5" +print("Motor 5") m5.setMotorSpeed(255) time.sleep(1) -print "Motor 6" +print("Motor 6") m6.setMotorSpeed(255) time.sleep(1) -print "Stopping - " -print "Motor 1" +print("Stopping - ") +print("Motor 1") m1.setMotorSpeed(0) time.sleep(1) -print "Motor 2" +print("Motor 2") m2.setMotorSpeed(0) time.sleep(1) -print "Motor 3" +print("Motor 3") m3.setMotorSpeed(0) time.sleep(1) -print "Motor 4" +print("Motor 4") m4.setMotorSpeed(0) time.sleep(1) -print "Motor 5" +print("Motor 5") m5.setMotorSpeed(0) time.sleep(1) -print "Motor 6" +print("Motor 6") m6.setMotorSpeed(0) time.sleep(1) -print "Set reverse - " -print "Motor 1" +print("Set reverse - ") +print("Motor 1") m1.setMotorSpeed(-255) time.sleep(1) -print "Motor 2" +print("Motor 2") m2.setMotorSpeed(-255) time.sleep(1) -print "Motor 3" +print("Motor 3") m3.setMotorSpeed(-255) time.sleep(1) -print "Motor 4" +print("Motor 4") m4.setMotorSpeed(-255) time.sleep(1) -print "Motor 5" +print("Motor 5") m5.setMotorSpeed(-255) time.sleep(1) -print "Motor 6" +print("Motor 6") m6.setMotorSpeed(-255) time.sleep(1) -print "All off" +print("All off") m1.off() m2.off() m3.off() diff --git a/640servotest.py b/640servotest.py index 73facdb..3b715ef 100644 --- a/640servotest.py +++ b/640servotest.py @@ -9,27 +9,27 @@ s1.off() s2.off() time.sleep(1) -print "Set 2000uS - " -print "Servo 1" +print("Set 2000uS - ") +print("Servo 1") s1.setPWMuS(2000) time.sleep(1) -print "Servo 2" +print("Servo 2") s2.setPWMuS(2000) time.sleep(1) -print "Set 1500uS - " -print "Servo 1" +print("Set 1500uS - ") +print("Servo 1") s1.setPWMuS(1500) time.sleep(1) -print "Servo 2" +print("Servo 2") s2.setPWMuS(1500) time.sleep(1) -print "Set 1000uS - " -print "Servo 1" +print("Set 1000uS - ") +print("Servo 1") s1.setPWMuS(1000) time.sleep(1) -print "Servo 2" +print("Servo 2") s2.setPWMuS(1000) time.sleep(1) -print "All off" +print("All off") s1.off() s2.off() diff --git a/darkwater_640/Adafruit_I2C.py b/darkwater_640/Adafruit_I2C.py deleted file mode 100644 index 2b24b67..0000000 --- a/darkwater_640/Adafruit_I2C.py +++ /dev/null @@ -1,161 +0,0 @@ -#!/usr/bin/python -import re -import smbus - -# =========================================================================== -# Adafruit_I2C Class -# =========================================================================== - -class Adafruit_I2C(object): - - @staticmethod - def getPiRevision(): - "Gets the version number of the Raspberry Pi board" - # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History - try: - with open('/proc/cpuinfo', 'r') as infile: - for line in infile: - # Match a line of the form "Revision : 0002" while ignoring extra - # info in front of the revsion (like 1000 when the Pi was over-volted). - match = re.match('Revision\s+:\s+.*(\w{4})$', line) - if match and match.group(1) in ['0000', '0002', '0003']: - # Return revision 1 if revision ends with 0000, 0002 or 0003. - return 1 - elif match: - # Assume revision 2 if revision ends with any other 4 chars. - return 2 - # Couldn't find the revision, assume revision 0 like older code for compatibility. - return 0 - except: - return 0 - - @staticmethod - def getPiI2CBusNumber(): - # Gets the I2C bus number /dev/i2c# - return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 - - def __init__(self, address, busnum=-1, debug=False): - self.address = address - # By default, the correct I2C bus is auto-detected using /proc/cpuinfo - # Alternatively, you can hard-code the bus version below: - # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) - # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) - self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber()) - self.debug = debug - - def reverseByteOrder(self, data): - "Reverses the byte order of an int (16-bit) or long (32-bit) value" - # Courtesy Vishal Sapre - byteCount = len(hex(data)[2:].replace('L','')[::2]) - val = 0 - for i in range(byteCount): - val = (val << 8) | (data & 0xff) - data >>= 8 - return val - - def errMsg(self): - print "Error accessing 0x%02X: Check your I2C address" % self.address - return -1 - - def write8(self, reg, value): - "Writes an 8-bit value to the specified register/address" - try: - self.bus.write_byte_data(self.address, reg, value) - if self.debug: - print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) - except IOError, err: - return self.errMsg() - - def write16(self, reg, value): - "Writes a 16-bit value to the specified register/address pair" - try: - self.bus.write_word_data(self.address, reg, value) - if self.debug: - print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % - (value, reg, reg+1)) - except IOError, err: - return self.errMsg() - - def writeRaw8(self, value): - "Writes an 8-bit value on the bus" - try: - self.bus.write_byte(self.address, value) - if self.debug: - print "I2C: Wrote 0x%02X" % value - except IOError, err: - return self.errMsg() - - def writeList(self, reg, list): - "Writes an array of bytes using I2C format" - try: - if self.debug: - print "I2C: Writing list to register 0x%02X:" % reg - print list - self.bus.write_i2c_block_data(self.address, reg, list) - except IOError, err: - return self.errMsg() - - def readList(self, reg, length): - "Read a list of bytes from the I2C device" - try: - results = self.bus.read_i2c_block_data(self.address, reg, length) - if self.debug: - print ("I2C: Device 0x%02X returned the following from reg 0x%02X" % - (self.address, reg)) - print results - return results - except IOError, err: - return self.errMsg() - - def readU8(self, reg): - "Read an unsigned byte from the I2C device" - try: - result = self.bus.read_byte_data(self.address, reg) - if self.debug: - print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % - (self.address, result & 0xFF, reg)) - return result - except IOError, err: - return self.errMsg() - - def readS8(self, reg): - "Reads a signed byte from the I2C device" - try: - result = self.bus.read_byte_data(self.address, reg) - if result > 127: result -= 256 - if self.debug: - print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % - (self.address, result & 0xFF, reg)) - return result - except IOError, err: - return self.errMsg() - - def readU16(self, reg, little_endian=True): - "Reads an unsigned 16-bit value from the I2C device" - try: - result = self.bus.read_word_data(self.address,reg) - # Swap bytes if using big endian because read_word_data assumes little - # endian on ARM (little endian) systems. - if not little_endian: - result = ((result << 8) & 0xFF00) + (result >> 8) - if (self.debug): - print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) - return result - except IOError, err: - return self.errMsg() - - def readS16(self, reg, little_endian=True): - "Reads a signed 16-bit value from the I2C device" - try: - result = self.readU16(reg,little_endian) - if result > 32767: result -= 65536 - return result - except IOError, err: - return self.errMsg() - -if __name__ == '__main__': - try: - bus = Adafruit_I2C(address=0) - print "Default I2C bus is accessible" - except: - print "Error accessing default I2C bus" diff --git a/darkwater_640/Adafruit_PWM_Servo_Driver.py b/darkwater_640/Adafruit_PWM_Servo_Driver.py deleted file mode 100644 index 6cf4566..0000000 --- a/darkwater_640/Adafruit_PWM_Servo_Driver.py +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/python - -import time -import math -from Adafruit_I2C import Adafruit_I2C - -# ============================================================================ -# Adafruit PCA9685 16-Channel PWM Servo Driver -# ============================================================================ - -class PWM : - # Registers/etc. - __MODE1 = 0x00 - __MODE2 = 0x01 - __SUBADR1 = 0x02 - __SUBADR2 = 0x03 - __SUBADR3 = 0x04 - __PRESCALE = 0xFE - __LED0_ON_L = 0x06 - __LED0_ON_H = 0x07 - __LED0_OFF_L = 0x08 - __LED0_OFF_H = 0x09 - __ALL_LED_ON_L = 0xFA - __ALL_LED_ON_H = 0xFB - __ALL_LED_OFF_L = 0xFC - __ALL_LED_OFF_H = 0xFD - - # Bits - __RESTART = 0x80 - __SLEEP = 0x10 - __ALLCALL = 0x01 - __INVRT = 0x10 - __OUTDRV = 0x04 - - general_call_i2c = Adafruit_I2C(0x00) - - @classmethod - def softwareReset(cls): - "Sends a software reset (SWRST) command to all the servo drivers on the bus" - cls.general_call_i2c.writeRaw8(0x06) # SWRST - - def __init__(self, address=0x40, debug=False): - self.i2c = Adafruit_I2C(address) - self.i2c.debug = debug - self.address = address - self.debug = debug - if (self.debug): - print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2" - self.setAllPWM(0, 0) - self.i2c.write8(self.__MODE2, self.__OUTDRV) - self.i2c.write8(self.__MODE1, self.__ALLCALL) - time.sleep(0.005) # wait for oscillator - - mode1 = self.i2c.readU8(self.__MODE1) - mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep) - self.i2c.write8(self.__MODE1, mode1) - time.sleep(0.005) # wait for oscillator - - def setPWMFreq(self, freq, correctionFactor=1.0): - "Sets the PWM frequency" - prescaleval = 25000000.0 # 25MHz - prescaleval /= 4096.0 # 12-bit - prescaleval /= float(freq) - prescaleval -= 1.0 - if (self.debug): - print "Setting PWM frequency to %d Hz" % freq - print "Estimated pre-scale: %d" % prescaleval - prescale = round(prescaleval * correctionFactor + 0.5) - if (self.debug): - print "Final pre-scale: %d" % prescale - - oldmode = self.i2c.readU8(self.__MODE1); - newmode = (oldmode & 0x7F) | 0x10 # sleep - self.i2c.write8(self.__MODE1, newmode) # go to sleep - self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) - self.i2c.write8(self.__MODE1, oldmode) - time.sleep(0.005) - self.i2c.write8(self.__MODE1, oldmode | 0x80) - - def setPWMFreqMin(self, freq, correctionFactor=1.0): - "Sets the PWM frequency" - prescaleval = 25000000.0 # 25MHz - prescaleval /= 4096.0 # 12-bit - prescaleval /= float(freq) - prescaleval -= 1.0 - if (self.debug): - print "Setting PWM frequency to %d Hz" % freq - print "Estimated pre-scale: %d" % prescaleval - prescale = math.floor(prescaleval * correctionFactor + 0.5) - if (self.debug): - print "Final pre-scale: %d" % prescale - - oldmode = self.i2c.readU8(self.__MODE1); - newmode = (oldmode & 0x7F) | 0x10 # sleep - self.i2c.write8(self.__MODE1, newmode) # go to sleep - self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) - self.i2c.write8(self.__MODE1, oldmode) - time.sleep(0.005) - self.i2c.write8(self.__MODE1, oldmode | 0x80) - - def setPWMFreqMax(self, freq, correctionFactor=1.0): - "Sets the PWM frequency" - prescaleval = 25000000.0 # 25MHz - prescaleval /= 4096.0 # 12-bit - prescaleval /= float(freq) - prescaleval -= 1.0 - if (self.debug): - print "Setting PWM frequency to %d Hz" % freq - print "Estimated pre-scale: %d" % prescaleval - prescale = math.ceil(prescaleval * correctionFactor + 0.5) - if (self.debug): - print "Final pre-scale: %d" % prescale - - oldmode = self.i2c.readU8(self.__MODE1); - newmode = (oldmode & 0x7F) | 0x10 # sleep - self.i2c.write8(self.__MODE1, newmode) # go to sleep - self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) - self.i2c.write8(self.__MODE1, oldmode) - time.sleep(0.005) - self.i2c.write8(self.__MODE1, oldmode | 0x80) - - def getPWMFreq(self): - prescale = self.i2c.readU8(self.__PRESCALE) - calcfreq = 25000000.0 / 4096.0 / ( float(prescale) + 1 ) - if (self.debug): - print "Got pre-scale: %d" % prescale - print "Calculated Frequency: %d" % calcfreq - - return calcfreq - - def setPWM(self, channel, on, off): - "Sets a single PWM channel" - self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) - self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) - self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) - self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) - - def setAllPWM(self, on, off): - "Sets a all PWM channels" - self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF) - self.i2c.write8(self.__ALL_LED_ON_H, on >> 8) - self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF) - self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8) diff --git a/darkwater_640/__init__.py b/darkwater_640/__init__.py index 9d5f80d..4b8c2a7 100644 --- a/darkwater_640/__init__.py +++ b/darkwater_640/__init__.py @@ -1 +1 @@ -from .darkwater_640 import dw_Motor, dw_Servo, dw_Controller +from .darkwater_640 import dw_Motor, dw_Servo, dw_Stepper, dw_Controller diff --git a/darkwater_640/mpu9250.py b/darkwater_640/mpu9250.py new file mode 100644 index 0000000..ae9b4ac --- /dev/null +++ b/darkwater_640/mpu9250.py @@ -0,0 +1,614 @@ +""" +MPU9250 driver code is placed under the BSD license. +Copyright (c) 2014, Emlid Limited, www.emlid.com +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Emlid Limited nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +""" + +import spidev +import time +import struct +import array + +class MPU9250: + + G_SI = 9.80665 + PI = 3.14159 + + # MPU9250 registers + __MPUREG_XG_OFFS_TC = 0x00 + __MPUREG_YG_OFFS_TC = 0x01 + __MPUREG_ZG_OFFS_TC = 0x02 + __MPUREG_X_FINE_GAIN = 0x03 + __MPUREG_Y_FINE_GAIN = 0x04 + __MPUREG_Z_FINE_GAIN = 0x05 + __MPUREG_XA_OFFS_H = 0x06 + __MPUREG_XA_OFFS_L = 0x07 + __MPUREG_YA_OFFS_H = 0x08 + __MPUREG_YA_OFFS_L = 0x09 + __MPUREG_ZA_OFFS_H = 0x0A + __MPUREG_ZA_OFFS_L = 0x0B + __MPUREG_PRODUCT_ID = 0x0C + __MPUREG_SELF_TEST_X = 0x0D + __MPUREG_SELF_TEST_Y = 0x0E + __MPUREG_SELF_TEST_Z = 0x0F + __MPUREG_SELF_TEST_A = 0x10 + __MPUREG_XG_OFFS_USRH = 0x13 + __MPUREG_XG_OFFS_USRL = 0x14 + __MPUREG_YG_OFFS_USRH = 0x15 + __MPUREG_YG_OFFS_USRL = 0x16 + __MPUREG_ZG_OFFS_USRH = 0x17 + __MPUREG_ZG_OFFS_USRL = 0x18 + __MPUREG_SMPLRT_DIV = 0x19 + __MPUREG_CONFIG = 0x1A + __MPUREG_GYRO_CONFIG = 0x1B + __MPUREG_ACCEL_CONFIG = 0x1C + __MPUREG_ACCEL_CONFIG_2 = 0x1D + __MPUREG_LP_ACCEL_ODR = 0x1E + __MPUREG_MOT_THR = 0x1F + __MPUREG_FIFO_EN = 0x23 + __MPUREG_I2C_MST_CTRL = 0x24 + __MPUREG_I2C_SLV0_ADDR = 0x25 + __MPUREG_I2C_SLV0_REG = 0x26 + __MPUREG_I2C_SLV0_CTRL = 0x27 + __MPUREG_I2C_SLV1_ADDR = 0x28 + __MPUREG_I2C_SLV1_REG = 0x29 + __MPUREG_I2C_SLV1_CTRL = 0x2A + __MPUREG_I2C_SLV2_ADDR = 0x2B + __MPUREG_I2C_SLV2_REG = 0x2C + __MPUREG_I2C_SLV2_CTRL = 0x2D + __MPUREG_I2C_SLV3_ADDR = 0x2E + __MPUREG_I2C_SLV3_REG = 0x2F + __MPUREG_I2C_SLV3_CTRL = 0x30 + __MPUREG_I2C_SLV4_ADDR = 0x31 + __MPUREG_I2C_SLV4_REG = 0x32 + __MPUREG_I2C_SLV4_DO = 0x33 + __MPUREG_I2C_SLV4_CTRL = 0x34 + __MPUREG_I2C_SLV4_DI = 0x35 + __MPUREG_I2C_MST_STATUS = 0x36 + __MPUREG_INT_PIN_CFG = 0x37 + __MPUREG_INT_ENABLE = 0x38 + __MPUREG_ACCEL_XOUT_H = 0x3B + __MPUREG_ACCEL_XOUT_L = 0x3C + __MPUREG_ACCEL_YOUT_H = 0x3D + __MPUREG_ACCEL_YOUT_L = 0x3E + __MPUREG_ACCEL_ZOUT_H = 0x3F + __MPUREG_ACCEL_ZOUT_L = 0x40 + __MPUREG_TEMP_OUT_H = 0x41 + __MPUREG_TEMP_OUT_L = 0x42 + __MPUREG_GYRO_XOUT_H = 0x43 + __MPUREG_GYRO_XOUT_L = 0x44 + __MPUREG_GYRO_YOUT_H = 0x45 + __MPUREG_GYRO_YOUT_L = 0x46 + __MPUREG_GYRO_ZOUT_H = 0x47 + __MPUREG_GYRO_ZOUT_L = 0x48 + __MPUREG_EXT_SENS_DATA_00 = 0x49 + __MPUREG_EXT_SENS_DATA_01 = 0x4A + __MPUREG_EXT_SENS_DATA_02 = 0x4B + __MPUREG_EXT_SENS_DATA_03 = 0x4C + __MPUREG_EXT_SENS_DATA_04 = 0x4D + __MPUREG_EXT_SENS_DATA_05 = 0x4E + __MPUREG_EXT_SENS_DATA_06 = 0x4F + __MPUREG_EXT_SENS_DATA_07 = 0x50 + __MPUREG_EXT_SENS_DATA_08 = 0x51 + __MPUREG_EXT_SENS_DATA_09 = 0x52 + __MPUREG_EXT_SENS_DATA_10 = 0x53 + __MPUREG_EXT_SENS_DATA_11 = 0x54 + __MPUREG_EXT_SENS_DATA_12 = 0x55 + __MPUREG_EXT_SENS_DATA_13 = 0x56 + __MPUREG_EXT_SENS_DATA_14 = 0x57 + __MPUREG_EXT_SENS_DATA_15 = 0x58 + __MPUREG_EXT_SENS_DATA_16 = 0x59 + __MPUREG_EXT_SENS_DATA_17 = 0x5A + __MPUREG_EXT_SENS_DATA_18 = 0x5B + __MPUREG_EXT_SENS_DATA_19 = 0x5C + __MPUREG_EXT_SENS_DATA_20 = 0x5D + __MPUREG_EXT_SENS_DATA_21 = 0x5E + __MPUREG_EXT_SENS_DATA_22 = 0x5F + __MPUREG_EXT_SENS_DATA_23 = 0x60 + __MPUREG_I2C_SLV0_DO = 0x63 + __MPUREG_I2C_SLV1_DO = 0x64 + __MPUREG_I2C_SLV2_DO = 0x65 + __MPUREG_I2C_SLV3_DO = 0x66 + __MPUREG_I2C_MST_DELAY_CTRL = 0x67 + __MPUREG_SIGNAL_PATH_RESET = 0x68 + __MPUREG_MOT_DETECT_CTRL = 0x69 + __MPUREG_USER_CTRL = 0x6A + __MPUREG_PWR_MGMT_1 = 0x6B + __MPUREG_PWR_MGMT_2 = 0x6C + __MPUREG_BANK_SEL = 0x6D + __MPUREG_MEM_START_ADDR = 0x6E + __MPUREG_MEM_R_W = 0x6F + __MPUREG_DMP_CFG_1 = 0x70 + __MPUREG_DMP_CFG_2 = 0x71 + __MPUREG_FIFO_COUNTH = 0x72 + __MPUREG_FIFO_COUNTL = 0x73 + __MPUREG_FIFO_R_W = 0x74 + __MPUREG_WHOAMI = 0x75 + __MPUREG_XA_OFFSET_H = 0x77 + __MPUREG_XA_OFFSET_L = 0x78 + __MPUREG_YA_OFFSET_H = 0x7A + __MPUREG_YA_OFFSET_L = 0x7B + __MPUREG_ZA_OFFSET_H = 0x7D + __MPUREG_ZA_OFFSET_L = 0x7E + + # ---- AK8963 Reg In MPU9250 ----------------------------------------------- + + __AK8963_I2C_ADDR = 0x0c #0x18 + __AK8963_Device_ID = 0x48 + + # Read-only Reg + __AK8963_WIA = 0x00 + __AK8963_INFO = 0x01 + __AK8963_ST1 = 0x02 + __AK8963_HXL = 0x03 + __AK8963_HXH = 0x04 + __AK8963_HYL = 0x05 + __AK8963_HYH = 0x06 + __AK8963_HZL = 0x07 + __AK8963_HZH = 0x08 + __AK8963_ST2 = 0x09 + + # Write/Read Reg + __AK8963_CNTL1 = 0x0A + __AK8963_CNTL2 = 0x0B + __AK8963_ASTC = 0x0C + __AK8963_TS1 = 0x0D + __AK8963_TS2 = 0x0E + __AK8963_I2CDIS = 0x0F + + # Read-only Reg ( ROM ) + __AK8963_ASAX = 0x10 + __AK8963_ASAY = 0x11 + __AK8963_ASAZ = 0x12 + + # Configuration bits MPU9250 + __BIT_SLEEP = 0x40 + __BIT_H_RESET = 0x80 + __BITS_CLKSEL = 0x07 + __MPU_CLK_SEL_PLLGYROX = 0x01 + __MPU_CLK_SEL_PLLGYROZ = 0x03 + __MPU_EXT_SYNC_GYROX = 0x02 + __BITS_FS_250DPS = 0x00 + __BITS_FS_500DPS = 0x08 + __BITS_FS_1000DPS = 0x10 + __BITS_FS_2000DPS = 0x18 + __BITS_FS_2G = 0x00 + __BITS_FS_4G = 0x08 + __BITS_FS_8G = 0x10 + __BITS_FS_16G = 0x18 + __BITS_FS_MASK = 0x18 + __BITS_DLPF_CFG_256HZ_NOLPF2 = 0x00 + __BITS_DLPF_CFG_188HZ = 0x01 + __BITS_DLPF_CFG_98HZ = 0x02 + __BITS_DLPF_CFG_42HZ = 0x03 + __BITS_DLPF_CFG_20HZ = 0x04 + __BITS_DLPF_CFG_10HZ = 0x05 + __BITS_DLPF_CFG_5HZ = 0x06 + __BITS_DLPF_CFG_2100HZ_NOLPF = 0x07 + __BITS_DLPF_CFG_MASK = 0x07 + __BIT_INT_ANYRD_2CLEAR = 0x10 + __BIT_RAW_RDY_EN = 0x01 + __BIT_I2C_IF_DIS = 0x10 + + __READ_FLAG = 0x80 + + # ---- Sensitivity --------------------------------------------------------- + + __MPU9250A_2g = (0.000061035156) # 0.000061035156 g/LSB + __MPU9250A_4g = (0.000122070312) # 0.000122070312 g/LSB + __MPU9250A_8g = (0.000244140625) # 0.000244140625 g/LSB + __MPU9250A_16g = (0.000488281250) # 0.000488281250 g/LSB + + __MPU9250G_250dps = (0.007633587786) # 0.007633587786 dps/LSB + __MPU9250G_500dps = (0.015267175572) # 0.015267175572 dps/LSB + __MPU9250G_1000dps = (0.030487804878) # 0.030487804878 dps/LSB + __MPU9250G_2000dps = (0.060975609756) # 0.060975609756 dps/LSB + + __MPU9250M_4800uT = (0.6) # 0.6 uT/LSB + + __MPU9250T_85degC = (0.002995177763) # 0.002995177763 degC/LSB + + __Magnetometer_Sensitivity_Scale_Factor = (0.15) + + def __init__(self, spi_bus_number = 0, spi_dev_number = 1): + self.bus = spidev.SpiDev() + self.spi_bus_number = spi_bus_number + self.spi_dev_number = spi_dev_number + self.gyro_divider = 0.0 + self.acc_divider = 0.0 + self.calib_data = [0.0, 0.0, 0.0] + self.magnetometer_ASA = [0.0, 0.0, 0.0] + self.temperature = 0.0 + self.gyroscope_data = [0.0, 0.0, 0.0] + self.accelerometer_data = [0.0, 0.0, 0.0] + self.magnetometer_data = [0.0, 0.0, 0.0] + +# ----------------------------------------------------------------------------------------------- +# REGISTER READ & WRITE +# usage: use these methods to read and write MPU9250 registers over SPI +# ----------------------------------------------------------------------------------------------- + + def WriteReg(self, reg_address, data): + self.bus.open(self.spi_bus_number, self.spi_dev_number) + tx = [reg_address, data] + rx = self.bus.xfer2(tx) + self.bus.close() + return rx + +# ----------------------------------------------------------------------------------------------- + + def ReadReg(self, reg_address): + self.bus.open(self.spi_bus_number, self.spi_dev_number) + tx = [reg_address | self.__READ_FLAG, 0x00] + rx = self.bus.xfer2(tx) + self.bus.close() + return rx[1] + +# ----------------------------------------------------------------------------------------------- + + def ReadRegs(self, reg_address, length): + self.bus.open(self.spi_bus_number, self.spi_dev_number) + tx = [0] * (length + 1) + tx[0] = reg_address | self.__READ_FLAG + + rx = self.bus.xfer2(tx) + + self.bus.close() + return rx[1:len(rx)] + +# ----------------------------------------------------------------------------------------------- +# TEST CONNECTION +# usage: call this function to know if SPI and MPU9250 are working correctly. +# returns true if mpu9250 answers +# ----------------------------------------------------------------------------------------------- + + def testConnection(self): + response = self.ReadReg(self.__MPUREG_WHOAMI) + if (response == 0x71): + return True + else: + return False + +# ----------------------------------------------------------------------------------------------- +# INITIALIZATION +# usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and +# low pass filter value; suitable values are: +# BITS_DLPF_CFG_256HZ_NOLPF2 +# BITS_DLPF_CFG_188HZ +# BITS_DLPF_CFG_98HZ +# BITS_DLPF_CFG_42HZ +# BITS_DLPF_CFG_20HZ +# BITS_DLPF_CFG_10HZ +# BITS_DLPF_CFG_5HZ +# BITS_DLPF_CFG_2100HZ_NOLPF +# returns 1 if an error occurred +# ----------------------------------------------------------------------------------------------- + + def initialize(self, sample_rate_div = 1, low_pass_filter = 0x01): + MPU_InitRegNum = 17 + MPU_Init_Data = [[0, 0]] * MPU_InitRegNum + + MPU_Init_Data = [ + [0x80, self.__MPUREG_PWR_MGMT_1], # Reset Device + [0x01, self.__MPUREG_PWR_MGMT_1], # Clock Source + [0x00, self.__MPUREG_PWR_MGMT_2], # Enable Acc & Gyro + [low_pass_filter, self.__MPUREG_CONFIG], # Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz + [0x18, self.__MPUREG_GYRO_CONFIG], # +-2000dps + [0x08, self.__MPUREG_ACCEL_CONFIG], # +-4G + [0x09, self.__MPUREG_ACCEL_CONFIG_2], # Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz + [0x30, self.__MPUREG_INT_PIN_CFG], + #[0x40, self.__MPUREG_I2C_MST_CTRL], # I2C Speed 348 kHz + #[0x20, self.__MPUREG_USER_CTRL], # Enable AUX + [0x20, self.__MPUREG_USER_CTRL], # I2C Master mode + [0x0D, self.__MPUREG_I2C_MST_CTRL], # I2C configuration multi-master IIC 400KHz + + [self.__AK8963_I2C_ADDR, self.__MPUREG_I2C_SLV0_ADDR], #Set the I2C slave addres of AK8963 and set for write. + #[0x09, self.__MPUREG_I2C_SLV4_CTRL], + #[0x81, self.__MPUREG_I2C_MST_DELAY_CTRL], #Enable I2C delay + + [self.__AK8963_CNTL2, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer + [0x01, self.__MPUREG_I2C_SLV0_DO], # Reset AK8963 + [0x81, self.__MPUREG_I2C_SLV0_CTRL], #Enable I2C and set 1 byte + + [self.__AK8963_CNTL1, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer + [0x12, self.__MPUREG_I2C_SLV0_DO], # Register value to continuous measurement in 16bit + [0x81, self.__MPUREG_I2C_SLV0_CTRL] #Enable I2C and set 1 byte + ] + + for i in range(0, MPU_InitRegNum): + self.WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]) + time.sleep(0.01) # I2C must slow down the write speed, otherwise it won't work + + self.set_acc_scale(self.__BITS_FS_16G) + self.set_gyro_scale(self.__BITS_FS_2000DPS) + + self.calib_mag() + +# ----------------------------------------------------------------------------------------------- +# ACCELEROMETER SCALE +# usage: call this function at startup, after initialization, to set the right range for the +# accelerometers. Suitable ranges are: +# BITS_FS_2G +# BITS_FS_4G +# BITS_FS_8G +# BITS_FS_16G +# returns the range set (2,4,8 or 16) +# ----------------------------------------------------------------------------------------------- + + def set_acc_scale(self, scale): + self.WriteReg(self.__MPUREG_ACCEL_CONFIG, scale) + if (scale == self.__BITS_FS_2G): + self.acc_divider = 16384.0 + elif (scale == self.__BITS_FS_4G): + self.acc_divider = 8192.0 + elif (scale == self.__BITS_FS_8G): + self.acc_divider = 4096.0 + elif (scale == self.__BITS_FS_16G): + self.acc_divider = 2048.0 + + temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG) + if (temp_scale == self.__BITS_FS_2G): + temp_scale = 2 + elif (temp_scale == self.__BITS_FS_4G): + temp_scale = 4 + elif (temp_scale == self.__BITS_FS_8G): + temp_scale = 8 + elif (temp_scale == self.__BITS_FS_16G): + temp_scale = 16 + + return temp_scale + +# ----------------------------------------------------------------------------------------------- +# GYROSCOPE SCALE +# usage: call this function at startup, after initialization, to set the right range for the +# gyroscopes. Suitable ranges are: +# BITS_FS_250DPS +# BITS_FS_500DPS +# BITS_FS_1000DPS +# BITS_FS_2000DPS +# returns the range set (250,500,1000 or 2000) +# ----------------------------------------------------------------------------------------------- + + def set_gyro_scale(self, scale): + self.WriteReg(self.__MPUREG_GYRO_CONFIG, scale) + if (scale == self.__BITS_FS_250DPS): + self.gyro_divider = 131.0 + elif (scale == self.__BITS_FS_500DPS): + self.gyro_divider = 65.6 + elif (scale == self.__BITS_FS_1000DPS): + self.gyro_divider = 32.8 + elif (scale == self.__BITS_FS_2000DPS): + self.gyro_divider = 16.4 + + temp_scale = self.ReadReg(self.__MPUREG_GYRO_CONFIG) + if (temp_scale == self.__BITS_FS_250DPS): + temp_scale = 250 + elif (temp_scale == self.__BITS_FS_500DPS): + temp_scale = 500 + elif (temp_scale == self.__BITS_FS_1000DPS): + temp_scale = 1000 + elif (temp_scale == self.__BITS_FS_2000DPS): + temp_scale = 2000 + + return temp_scale + +# ----------------------------------------------------------------------------------------------- +# WHO AM I? +# usage: call this function to know if SPI is working correctly. It checks the I2C address of the +# mpu9250 which should be 104 when in SPI mode. +# returns the I2C address (104) +# ----------------------------------------------------------------------------------------------- + + def whoami(self): + return self.ReadReg(self.__MPUREG_WHOAMI) + +# ----------------------------------------------------------------------------------------------- +# READ ACCELEROMETER +# usage: call this function to read accelerometer data. Axis represents selected axis: +# 0 -> X axis +# 1 -> Y axis +# 2 -> Z axis +# ----------------------------------------------------------------------------------------------- + + def read_acc(self): + response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 6) + + for i in range(0, 3): + data = self.byte_to_float(response[i*2:i*2+2]) + self.accelerometer_data[i] = self.G_SI * data / self.acc_divider + +# ----------------------------------------------------------------------------------------------- +# READ GYROSCOPE +# usage: call this function to read gyroscope data. Axis represents selected axis: +# 0 -> X axis +# 1 -> Y axis +# 2 -> Z axis +# ----------------------------------------------------------------------------------------------- + + def read_gyro(self): + response = self.ReadRegs(self.__MPUREG_GYRO_XOUT_H, 6) + + for i in range(0, 3): + data = self.byte_to_float(response[i*2:i*2+2]) + self.gyroscope_data[i] = (self.PI/180) * data / self.gyro_divider + +# ----------------------------------------------------------------------------------------------- +# READ TEMPERATURE +# usage: call this function to read temperature data. +# returns the value in Celsius +# ----------------------------------------------------------------------------------------------- + + def read_temp(self): + response = self.ReadRegs(self.__MPUREG_TEMP_OUT_H, 2) + + #temp = response[0]*256.0 + response[1] + temp = self.byte_to_float(response) + self.temperature = (temp/340.0)+36.53 + +# ----------------------------------------------------------------------------------------------- +# READ ACCELEROMETER CALIBRATION +# usage: call this function to read accelerometer data. Axis represents selected axis: +# 0 -> X axis +# 1 -> Y axis +# 2 -> Z axis +# returns Factory Trim value +# ----------------------------------------------------------------------------------------------- + + def calib_acc(self): + temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG) + self.set_acc_scale(self.__BITS_FS_8G) + + response = self.ReadRegs(self.__MPUREG_SELF_TEST_X, 4) + + self.calib_data[0] = ((response[0] & 11100000) >> 3) | ((response[3] & 00110000) >> 4) + self.calib_data[1] = ((response[1] & 11100000) >> 3) | ((response[3] & 00001100) >> 2) + self.calib_data[2] = ((response[2] & 11100000) >> 3) | ((response[3] & 00000011)) + + self.set_acc_scale(temp_scale) + +# ----------------------------------------------------------------------------------------------- + + def AK8963_whoami(self): + self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) #Set the I2C slave addres of AK8963 and set for read. + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_WIA) #I2C slave 0 register address from where to begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) #Read 1 byte from the magnetometer + + #self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) # Enable I2C and set bytes + time.sleep(0.01) + + return self.ReadReg(self.__MPUREG_EXT_SENS_DATA_00) # Read I2C + +# ----------------------------------------------------------------------------------------------- + + def calib_mag(self): + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) + # I2C slave 0 register address from where to begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_ASAX) + # Read 3 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x83) + + time.sleep(0.01) + + response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 3) + + for i in range(0, 3): + self.magnetometer_ASA[i] = ((float(response[i]) - 128)/256 + 1) * self.__Magnetometer_Sensitivity_Scale_Factor + +# ----------------------------------------------------------------------------------------------- + + def read_mag(self): + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) + # I2C slave 0 register address from where to begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) + # Read 6 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) + + time.sleep(0.01) + + response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 7) + #must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. + for i in range(0, 3): + data = self.byte_to_float_le(response[i*2:i*2+2]) + self.magnetometer_data[i] = data * self.magnetometer_ASA[i] + +# ----------------------------------------------------------------------------------------------- + + def read_all(self): + # Send I2C command at first + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) + # I2C slave 0 register address from where ; //Read 7 bytes from the magnetometerto begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) + # Read 7 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) + # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. + + # time.sleep(0.001) + response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 21); + + # Get Accelerometer values + for i in range(0, 3): + data = self.byte_to_float(response[i*2:i*2+2]) + self.accelerometer_data[i] = self.G_SI * data / self.acc_divider + + # Get temperature + i = 3 + temp = self.byte_to_float(response[i*2:i*2+2]) + self.temperature = (temp/340.0)+36.53 + + # Get gyroscope values + for i in range(4, 7): + data = self.byte_to_float(response[i*2:i*2+2]) + self.gyroscope_data[i-4] = (self.PI/180) * data / self.gyro_divider + + # Get magnetometer values + for i in range(7, 10): + data = self.byte_to_float_le(response[i*2:i*2+2]) + self.magnetometer_data[i-7] = data * self.magnetometer_ASA[i-7] + +# ----------------------------------------------------------------------------------------------- +# GET VALUES +# usage: call this functions to read and get values +# returns accel, gyro and mag values +# ----------------------------------------------------------------------------------------------- + + def getMotion9(self): + self.read_all() + m9a = self.accelerometer_data + m9g = self.gyroscope_data + m9m = self.magnetometer_data + + return m9a, m9g, m9m + +# ----------------------------------------------------------------------------------------------- + + def getMotion6(self): + self.read_acc() + self.read_gyro() + + m6a = self.accelerometer_data + m6g = self.gyroscope_data + + return m6a, m6g + +# ----------------------------------------------------------------------------------------------- + + def byte_to_float(self, input_buffer): + byte_array = array.array("B", input_buffer) + signed_16_bit_int, = struct.unpack(">h", byte_array) + return float(signed_16_bit_int) + +# ----------------------------------------------------------------------------------------------- + + def byte_to_float_le(self, input_buffer): + byte_array = array.array("B", input_buffer) + signed_16_bit_int, = struct.unpack("