From d08e939f40b344c69edf4d7b3c694d9ac81adae4 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:19:25 +0100 Subject: [PATCH 01/10] updating for python 3 --- darkwater_640/I2C.py | 8 ++++---- darkwater_640/PCA9685.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/darkwater_640/I2C.py b/darkwater_640/I2C.py index a45a21d..87fcee8 100644 --- a/darkwater_640/I2C.py +++ b/darkwater_640/I2C.py @@ -19,10 +19,10 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -import logging -import subprocess +import .logging +import .subprocess -import Platform as Platform +import .Platform as Platform def reverseByteOrder(data): @@ -93,7 +93,7 @@ class Device(object): self._address = address if i2c_interface is None: # Use pure python I2C interface if none is specified. - import smbus + import .smbus self._bus = smbus.SMBus(busnum) else: # Otherwise use the provided class to create an smbus interface. diff --git a/darkwater_640/PCA9685.py b/darkwater_640/PCA9685.py index f982701..f08a679 100644 --- a/darkwater_640/PCA9685.py +++ b/darkwater_640/PCA9685.py @@ -71,7 +71,7 @@ class PCA9685(object): """Initialize the PCA9685.""" # Setup I2C interface for the device. if i2c is None: - import I2C as I2C + from .I2C import I2C as I2C i2c = I2C self._device = i2c.get_i2c_device(address, **kwargs) self.set_all_pwm(0, 0) From 1c5f1d160f0e73c53a367d3815801a329bdaf79a Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:23:31 +0100 Subject: [PATCH 02/10] test script update --- 640motortest.py | 46 +++++++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/640motortest.py b/640motortest.py index 568d40f..10d2d32 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" -m3.setMotorSpeed(255) +print("Motor 3") +m3.se9tMotorSpeed(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() From 6d3bb50e50f47f20d9bb1cefab47bb449a28f57d Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:29:04 +0100 Subject: [PATCH 03/10] removed dodgy . --- darkwater_640/I2C.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/darkwater_640/I2C.py b/darkwater_640/I2C.py index 87fcee8..d2bc375 100644 --- a/darkwater_640/I2C.py +++ b/darkwater_640/I2C.py @@ -19,8 +19,8 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -import .logging -import .subprocess +import logging +import subprocess import .Platform as Platform @@ -93,7 +93,7 @@ class Device(object): self._address = address if i2c_interface is None: # Use pure python I2C interface if none is specified. - import .smbus + import smbus self._bus = smbus.SMBus(busnum) else: # Otherwise use the provided class to create an smbus interface. From 865f8a258879cc5c2bcf7946c73b9c078cdce254 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:30:27 +0100 Subject: [PATCH 04/10] grrr --- darkwater_640/I2C.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darkwater_640/I2C.py b/darkwater_640/I2C.py index d2bc375..373929a 100644 --- a/darkwater_640/I2C.py +++ b/darkwater_640/I2C.py @@ -22,7 +22,7 @@ import logging import subprocess -import .Platform as Platform +from .Platform import Platform as Platform def reverseByteOrder(data): From ac31d90e5f72a0164e1759d02e4f9732cc87bcae Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:32:19 +0100 Subject: [PATCH 05/10] grr --- darkwater_640/I2C.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darkwater_640/I2C.py b/darkwater_640/I2C.py index 373929a..c9f5db6 100644 --- a/darkwater_640/I2C.py +++ b/darkwater_640/I2C.py @@ -22,7 +22,7 @@ import logging import subprocess -from .Platform import Platform as Platform +from .Platform import Platform def reverseByteOrder(data): From 7b86aca61fb79a152d1afb1d8520ec4161379136 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:36:20 +0100 Subject: [PATCH 06/10] remove legacy --- darkwater_640/Adafruit_I2C.py | 161 --------------------- darkwater_640/Adafruit_PWM_Servo_Driver.py | 143 ------------------ darkwater_640/I2C.py | 2 +- darkwater_640/__init__.py | 2 +- 4 files changed, 2 insertions(+), 306 deletions(-) delete mode 100644 darkwater_640/Adafruit_I2C.py delete mode 100644 darkwater_640/Adafruit_PWM_Servo_Driver.py 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/I2C.py b/darkwater_640/I2C.py index c9f5db6..a45a21d 100644 --- a/darkwater_640/I2C.py +++ b/darkwater_640/I2C.py @@ -22,7 +22,7 @@ import logging import subprocess -from .Platform import Platform +import Platform as Platform def reverseByteOrder(data): 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 From f72925a04a8e8b1ee5139bacc9d93b3e161f1dc8 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:40:05 +0100 Subject: [PATCH 07/10] back update --- darkwater_640/PCA9685.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darkwater_640/PCA9685.py b/darkwater_640/PCA9685.py index f08a679..f982701 100644 --- a/darkwater_640/PCA9685.py +++ b/darkwater_640/PCA9685.py @@ -71,7 +71,7 @@ class PCA9685(object): """Initialize the PCA9685.""" # Setup I2C interface for the device. if i2c is None: - from .I2C import I2C as I2C + import I2C as I2C i2c = I2C self._device = i2c.get_i2c_device(address, **kwargs) self.set_all_pwm(0, 0) From a6281a309fb41734c33fff99ed909ef3c676b55f Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:41:17 +0100 Subject: [PATCH 08/10] typo --- 640motortest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/640motortest.py b/640motortest.py index 10d2d32..c3d41ea 100644 --- a/640motortest.py +++ b/640motortest.py @@ -26,7 +26,7 @@ print("Motor 2") m2.setMotorSpeed(255) time.sleep(1) print("Motor 3") -m3.se9tMotorSpeed(255) +m3.setMotorSpeed(255) time.sleep(1) print("Motor 4") m4.setMotorSpeed(255) From 243af588da273eaeb7b2456a448dab56504c62c8 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 22:46:26 +0100 Subject: [PATCH 09/10] servo update --- 640servotest.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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() From 358444741709dd272d9adced78202e58a98bff49 Mon Sep 17 00:00:00 2001 From: shrkey Date: Thu, 29 Sep 2016 23:37:08 +0100 Subject: [PATCH 10/10] initial imu --- 640imutest.py | 65 +++++ darkwater_640/mpu9250.py | 614 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 679 insertions(+) create mode 100644 640imutest.py create mode 100644 darkwater_640/mpu9250.py 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/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("