├── README.md ├── esc-calibration.py ├── pid.py ├── hmc5883l.py ├── motor.py ├── MS5611.py ├── sensor.py └── MPU6050.py /README.md: -------------------------------------------------------------------------------- 1 | pyDrone 2 | ======= 3 | 4 | 5 | Set of Python classes for building a drone using the Raspberry Pi and list of hardware like: 6 | - 4 brushless motors 7 | - 4 ESC (Electronical speed control) 8 | - 10 DOF IMU (GY-86 10DOF MS5611 HMC5883L MPU6050, 3-axis gyro + 3 axis accelerometer + 3-axis magnetic field + atmospheric pressure) 9 | - GPS 10 | - ADC for monitoring the battery -------------------------------------------------------------------------------- /esc-calibration.py: -------------------------------------------------------------------------------- 1 | 2 | from motor import motor 3 | 4 | motor1 = motor('m1', 17, simulation=False) 5 | motor2 = motor('m1', 18, simulation=False) 6 | motor3 = motor('m1', 25, simulation=False) 7 | motor4 = motor('m1', 22, simulation=False) 8 | 9 | motors = [motor1, motor2, motor3, motor4] 10 | 11 | print('***Disconnect ESC power') 12 | print('***then press ENTER') 13 | res = raw_input() 14 | try: 15 | for mitour in motors: 16 | mitour.start() 17 | mitour.setW(100) 18 | 19 | print('***Connect ESC Power') 20 | print('***Wait beep-beep') 21 | 22 | res = raw_input() 23 | for mitour in motors: 24 | mitour.start() 25 | mitour.setW(0) 26 | print('***Wait N beep for battery cell') 27 | print('***Wait beeeeeep for ready') 28 | print('***then press ENTER') 29 | res = raw_input() 30 | 31 | for mitour in motors: 32 | mitour.start() 33 | mitour.setW(10) 34 | res = raw_input() 35 | finally: 36 | # shut down cleanly 37 | for mitour in motors: 38 | mitour.stop() 39 | 40 | print ("well done!") 41 | exit() 42 | -------------------------------------------------------------------------------- /pid.py: -------------------------------------------------------------------------------- 1 | from time import time,sleep 2 | 3 | 4 | class pid(object): 5 | 6 | def __init__(self,kp,ki,kd,maxCorr=5): 7 | self.kp = kp 8 | self.ki = ki 9 | self.kd = kd 10 | self.previousTime = 0.0 11 | self.I = 0 12 | self.P =0 13 | self.D = 0 14 | self.previousError = 0.0 15 | self.init=True 16 | self.maxCorr=maxCorr 17 | 18 | 19 | def calc(self, error): 20 | if self.init: 21 | #the first time do not calculate pid correction, but init the time data 22 | self.previousTime = time() 23 | self.init=False 24 | return 0 25 | else: 26 | currentTime = time() 27 | stepTime = currentTime - self.previousTime 28 | 29 | self.P = error * self.kp 30 | self.I += (error * stepTime) * self.ki 31 | self.D = (error - self.previousError) / stepTime * self.kd 32 | 33 | 34 | correction = self.P + self.I + self.D 35 | self.previousTime = currentTime 36 | self.previousError = error 37 | #since W is an integer, correction is rounded 38 | correction = round(correction) 39 | 40 | if correction>self.maxCorr: 41 | correction=self.maxCorr 42 | if correction<-self.maxCorr: 43 | correction=-self.maxCorr 44 | return correction, self.P, self.I, self.D 45 | -------------------------------------------------------------------------------- /hmc5883l.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # vim: set fileencoding=UTF-8 : 3 | 4 | # HMC5888L Magnetometer (Digital Compass) wrapper class 5 | # Based on https://bitbucket.org/thinkbowl/i2clibraries/src/14683feb0f96, 6 | # but uses smbus rather than quick2wire and sets some different init 7 | # params. 8 | 9 | import smbus 10 | import math 11 | import time 12 | import sys 13 | 14 | class hmc5883l: 15 | 16 | __scales = { 17 | 0.88: [0, 0.73], 18 | 1.30: [1, 0.92], 19 | 1.90: [2, 1.22], 20 | 2.50: [3, 1.52], 21 | 4.00: [4, 2.27], 22 | 4.70: [5, 2.56], 23 | 5.60: [6, 3.03], 24 | 8.10: [7, 4.35], 25 | } 26 | 27 | def __init__(self, port=1, address=0x1E, gauss=1.3, declination=(0,0)): 28 | self.bus = smbus.SMBus(port) 29 | self.address = address 30 | 31 | (degrees, minutes) = declination 32 | self.__declDegrees = degrees 33 | self.__declMinutes = minutes 34 | self.__declination = (degrees + minutes / 60) * math.pi / 180 35 | 36 | (reg, self.__scale) = self.__scales[gauss] 37 | self.bus.write_byte_data(self.address, 0x00, 0x70) # 8 Average, 15 Hz, normal measurement 38 | self.bus.write_byte_data(self.address, 0x01, reg << 5) # Scale 39 | self.bus.write_byte_data(self.address, 0x02, 0x00) # Continuous measurement 40 | 41 | def declination(self): 42 | return (self.__declDegrees, self.__declMinutes) 43 | 44 | def twos_complement(self, val, len): 45 | # Convert twos compliment to integer 46 | if (val & (1 << len - 1)): 47 | val = val - (1< 2 * math.pi: 74 | headingRad -= 2 * math.pi 75 | 76 | # Convert to degrees from radians 77 | headingDeg = headingRad * 180 / math.pi 78 | return headingDeg 79 | 80 | def degrees(self, headingDeg): 81 | degrees = math.floor(headingDeg) 82 | minutes = round((headingDeg - degrees) * 60) 83 | return (degrees, minutes) 84 | 85 | def __str__(self): 86 | (x, y, z) = self.axes() 87 | return "Axis X: " + str(x) + "\n" \ 88 | "Axis Y: " + str(y) + "\n" \ 89 | "Axis Z: " + str(z) + "\n" \ 90 | "Declination: " + self.degrees(self.declination()) + "\n" \ 91 | "Heading: " + self.degrees(self.heading()) + "\n" 92 | 93 | if __name__ == "__main__": 94 | # http://magnetic-declination.com/Great%20Britain%20(UK)/Harrogate# 95 | compass = hmc5883l(gauss = 4.7, declination = (-2,5)) 96 | while True: 97 | sys.stdout.write("\rHeading: " + compass.degrees(compass.heading()) + " ") 98 | sys.stdout.flush() 99 | time.sleep(0.5) 100 | 101 | -------------------------------------------------------------------------------- /motor.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | class motor(object): 5 | """Manages the currect Angular rotation 6 | Implements the IO interface using the RPIO lib 7 | __init_(self, name, pin, kv=1000, RPMMin=1, RPMMax=100, debug=True, simulation=True): 8 | More info on RPIO in http://pythonhosted.org/RPIO/index.html""" 9 | 10 | 11 | def __init__(self, name, pin, kv=1000, WMin=0, WMax=100, debug=True, simulation=True): 12 | self.name = name 13 | self.powered = False 14 | self.simulation = simulation 15 | self.__pin = pin 16 | self.__kv = kv 17 | self.setWLimits(WMin, WMax) 18 | self.setDebug(debug) 19 | 20 | self.__W = self.__WMin 21 | self.__Wh = 10 22 | 23 | try: 24 | from RPIO import PWM 25 | self.__IO = PWM.Servo() 26 | except ImportError: 27 | self.simulation = True 28 | 29 | def setDebug(self, debug): 30 | self.__debug = debug 31 | #if self.__debug: 32 | #self.__logger.setLevel(logging.DEBUG) 33 | #else: 34 | #self.__logger.setLevel(logging.WARNING) 35 | 36 | def getDebug(self): 37 | return self.__debug 38 | 39 | def setPin(self, pin): 40 | "set the pin for each motor" 41 | self.__pin = pin 42 | 43 | def setKv(self, kv): 44 | "set the kv for each motor" 45 | self.__kv = kv 46 | 47 | def setWLimits(self, WMin, WMax): 48 | "set the pin for each motor" 49 | if WMin < 0: 50 | WMin = 0 51 | self.__WMin = WMin 52 | if WMax > 100: 53 | WMax = 100 54 | self.__WMax = WMax 55 | 56 | def saveWh(self): 57 | "Save Wh = current W%" 58 | self.__Wh = self.__W 59 | 60 | def setWh(self): 61 | "Sets current W% =Wh" 62 | self.__W = self.__Wh 63 | self.setW(self.__W) 64 | 65 | def getWh(self): 66 | "returns current W% =Wh" 67 | return self.__Wh 68 | 69 | def start(self): 70 | "Run the procedure to init the PWM" 71 | if not self.simulation: 72 | try: 73 | from RPIO import PWM 74 | self.__IO = PWM.Servo() 75 | self.powered = True 76 | #TODO Decide How to manage the WMax < 100 77 | #to keep anyhow the throttle range 0-100 78 | except ImportError: 79 | self.simulation = True 80 | self.powered = False 81 | 82 | def stop(self): 83 | "Stop PWM signal" 84 | 85 | self.setW(0) 86 | if self.powered: 87 | self.__IO.stop_servo(self.__pin) 88 | self.powered = False 89 | 90 | def increaseW(self, step=1): 91 | "increases W% for the motor" 92 | 93 | self.__W = self.__W + step 94 | self.setW(self.__W) 95 | 96 | def decreaseW(self, step=1): 97 | "decreases W% for the motor" 98 | 99 | self.__W = self.__W - step 100 | self.setW(self.__W) 101 | 102 | def getW(self): 103 | "retuns current W%" 104 | return self.__W 105 | 106 | def setW(self, W): 107 | "Checks W% is between limits than sets it" 108 | 109 | PW = 0 110 | self.__W = W 111 | if self.__W < self.__WMin: 112 | self.__W = self.__WMin 113 | if self.__W > self.__WMax: 114 | self.__W = self.__WMax 115 | PW = (1000 + (self.__W) * 10) 116 | # Set servo to xxx us 117 | if self.powered: 118 | self.__IO.set_servo(self.__pin, PW) 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /MS5611.py: -------------------------------------------------------------------------------- 1 | ## I recieved my MS5611 Pressure/Barometer Sensor not knowing what I was doing. 2 | ## After much trial and error and pooring over the datasheet quite extensively I figured out how to communicate with it using Python 3 | ## Below are my poor attempts a script to get the Pressure and Tempurature data from the MS5611 4 | ## The datasheet that I used can be found here: http://www.embeddedadventures.com/datasheets/ms5611.pdf 5 | ## All of the following values and calculations were taken from that datasheet. 6 | 7 | ## Note this script does require Root to run. 8 | 9 | ## Created By: Jonny Fosnight 10 | ## Date: 2013-03-18 11 | ## Email: jfosnight@live.com 12 | 13 | ## Feel free to use the script however you want. If you would like to use it for a commercial project please contact me at the listed email address. 14 | 15 | 16 | import time 17 | 18 | ## Import Libraries that let python talk to I2C devices 19 | from smbus import SMBus 20 | 21 | ## Initialize the bus. (If you have a Rev 2 Raspberry Pi the 0 needs to be changed to a 1) 22 | bus = SMBus(0) 23 | 24 | ## This writes the command to the Sensor to calculate the digital value of pressure. (This is a raw value and will 25 | # be used in a calculation later on in the script to get the actual real pressure.) 26 | bus.write_byte(0x76, 0x48) 27 | time.sleep(0.05) 28 | 29 | ## This command reads the data that we just calculated on the sensor 30 | ## I had to use the read_i2c_block_data rather than the read_word or read_byte because the values being returned by the 31 | # sensor were 24 bit and was being truncated by the other read methods. Reading the whole data block was the only way 32 | # that I knew to get all of the data. 33 | D1 = bus.read_i2c_block_data(0x76, 0x00) 34 | time.sleep(0.05) 35 | 36 | 37 | ## This is much like above only it is getting the raw temperature. 38 | bus.write_byte(0x76, 0x58) 39 | time.sleep(0.05) 40 | D2 = bus.read_i2c_block_data(0x76, 0x00) 41 | time.sleep(0.05) 42 | 43 | 44 | ## The data being read in blocks are in an array in 8 bit pieces, so we need to convert the first 24 bits into decimal, 45 | # which is what this statement does. 46 | D1 = D1[0] * 65536 + D1[1] * 256.0 + D1[2] 47 | D2 = D2[0] * 65536 + D2[1] * 256.0 + D2[2] 48 | 49 | 50 | ## The MS6511 Sensor stores 6 values in the EPROM memory that we need in order to calculate the actual temperature and pressure 51 | ## These values are calculated/stored at the factory when the sensor is calibrated. 52 | ## I probably could have used the read word function instead of the whole block, but I wanted to keep things consistent. 53 | C1 = bus.read_i2c_block_data(0x76, 0xA2) #Pressure Sensitivity 54 | time.sleep(0.05) 55 | C2 = bus.read_i2c_block_data(0x76, 0xA4) #Pressure Offset 56 | time.sleep(0.05) 57 | C3 = bus.read_i2c_block_data(0x76, 0xA6) #Temperature coefficient of pressure sensitivity 58 | time.sleep(0.05) 59 | C4 = bus.read_i2c_block_data(0x76, 0xA8) #Temperature coefficient of pressure offset 60 | time.sleep(0.05) 61 | C5 = bus.read_i2c_block_data(0x76, 0xAA) #Reference temperature 62 | time.sleep(0.05) 63 | C6 = bus.read_i2c_block_data(0x76, 0xAC) #Temperature coefficient of the temperature 64 | time.sleep(0.05) 65 | 66 | ## Again here we are converting the 2 8bit packages into a single decimal 67 | C1 = C1[0] * 256.0 + C1[1] 68 | C2 = C2[0] * 256.0 + C2[1] 69 | C3 = C3[0] * 256.0 + C3[1] 70 | C4 = C4[0] * 256.0 + C4[1] 71 | C5 = C5[0] * 256.0 + C5[1] 72 | C6 = C6[0] * 256.0 + C6[1] 73 | 74 | ## These are the calculations provided in the datasheet for the sensor. 75 | ## TEMP is the actual temperature reported in Centigrade. It is reported to the 1/100ths, so it needs to be divided by 100 to be used by us. 76 | dT = D2 - C5 * 2**8 77 | TEMP = 2000 + dT * C6 / 2**23 78 | 79 | ## I also included Fahrenheit for the rest of us normal people 80 | TEMP_F = TEMP/100.0 * 9.0/5 + 32 81 | 82 | print "Temperature: ", TEMP/100.0 83 | print "Temperature F: ", round(TEMP_F, 2) 84 | 85 | ## These calculations are all used to produce the final pressure value (just as before the Pressure value needs to be divided by 100 to shift the decimal place where it belongs.) 86 | OFF = C2 * 2**16 + (C4 * dT) / 2**7 87 | SENS = C1 * 2**15 + (C3 * dT) / 2**8 88 | P = (D1 * SENS / 2**21 - OFF) / 2**15 89 | 90 | 91 | print "Pressure: ", P/100.0 92 | 93 | ## Now one curious thing I found was that the sensor needs to be adjusted to your current eleivation. I couldn't find an automatic way to do it, but I did find 94 | # this website with a chart. http://www.novalynx.com/manuals/bp-elevation-correction-tables.pdf 95 | # Just replace the 55.5 below with whatever elevation is approapriate for you. 96 | print "Pressure Adjusted: ", round(P/100.0 + 55.5, 2) 97 | -------------------------------------------------------------------------------- /sensor.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | from MPU6050 import MPU6050 4 | from time import time,sleep 5 | import threading 6 | 7 | class sensor(threading.Thread): 8 | 9 | #TODO gestire logger 10 | 11 | def __init__(self,address=0x68,updateoffset=True, cycletime=0.01, savelog=False): 12 | 13 | threading.Thread.__init__(self) 14 | 15 | self.address=address 16 | #MPU6050 is an specific interface to the hw used. 17 | #if the imu is different from MPU6050 it is enough to call the 18 | #correct interface here 19 | self.IMU=MPU6050(address) 20 | print 'IMU calibrating...' 21 | if updateoffset: 22 | self.IMU.updateOffsets('IMU_offset.txt') 23 | self.IMU.readOffsets('IMU_offset.txt') 24 | 25 | self.roll=0 26 | self.pitch=0 27 | self.yaw=0 28 | self.x_acc=0 29 | self.y_acc=0 30 | self.z_acc=0 31 | self.r_rate=0 32 | self.p_rate=0 33 | self.y_rate=0 34 | self.cycling=True 35 | self.cycletime=cycletime 36 | self.savelog = savelog 37 | self.datalog='' 38 | 39 | 40 | def run(self): 41 | 42 | self.datalog='' 43 | self.datalog='|time|dt' 44 | self.datalog +='|roll|pitch|yaw' 45 | self.datalog +='|r_rate|p_rate|y_rate' 46 | self.datalog +='|x_acc|y_acc|z_acc' 47 | self.datalog +='\n' 48 | 49 | 50 | 51 | inittime=time() 52 | tottime=0 53 | print 'IMU running...' 54 | while self.cycling: 55 | tottime_old=tottime 56 | tottime=time()-inittime 57 | steptime=tottime-tottime_old 58 | self.update(steptime) 59 | if self.savelog is True: 60 | self.datalog +=self.getDataString(tottime,steptime) 61 | 62 | #comment this for cycling as fast as possible 63 | #if steptime tau the acc takes precedence 122 | 123 | new_r,new_p,new_y=self.getAngleAcc() 124 | a=tau/(tau+dt) 125 | self.roll=a*(self.roll+self.r_rate*dt)+(1-a)*new_r 126 | self.pitch=a*(self.pitch+self.p_rate*dt)+(1-a)*new_p 127 | #note the yaw angle can be calculated only using the 128 | # gyro data, so a=1 for yaw.(it means that yaw value is affected by drift) 129 | a=1 130 | self.yaw=a*(self.yaw+self.y_rate*dt)+(1-a)*new_y 131 | #Rounding the results 132 | self.roll=(round(self.roll,2)) 133 | self.pitch=(round(self.pitch,2)) 134 | self.yaw=(round(self.yaw,2)) 135 | 136 | 137 | -------------------------------------------------------------------------------- /MPU6050.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | ##solenero.tech@gmail.com 3 | #solenerotech.wordpress.com 4 | 5 | #solenerotech 2013.07.31 6 | 7 | #2013.09.30 see SNT modifications 8 | # respect original code from www.pistuffing.co.uk 9 | 10 | from __future__ import division 11 | import signal 12 | import socket 13 | import time 14 | import string 15 | import sys 16 | import getopt 17 | import time 18 | import math 19 | import threading 20 | from array import * 21 | import smbus 22 | import select 23 | import os 24 | import struct 25 | import logging 26 | from RPIO import PWM 27 | import RPIO 28 | import subprocess 29 | from datetime import datetime 30 | 31 | ############################################################################################ 32 | # 33 | # Adafruit i2c interface plus bug fix 34 | # 35 | ############################################################################################ 36 | class I2C: 37 | 38 | def __init__(self, address, bus=smbus.SMBus(1)): 39 | self.address = address 40 | self.bus = bus 41 | 42 | def reverseByteOrder(self, data): 43 | "Reverses the byte order of an int (16-bit) or long (32-bit) value" 44 | # Courtesy Vishal Sapre 45 | dstr = hex(data)[2:].replace('L','') 46 | byteCount = len(dstr[::2]) 47 | val = 0 48 | for i, n in enumerate(range(byteCount)): 49 | d = data & 0xFF 50 | val |= (d << (8 * (byteCount - i - 1))) 51 | data >>= 8 52 | return val 53 | 54 | def write8(self, reg, value): 55 | "Writes an 8-bit value to the specified register/address" 56 | while True: 57 | try: 58 | self.bus.write_byte_data(self.address, reg, value) 59 | #logger.debug('I2C: Wrote 0x%02X to register 0x%02X', value, reg) 60 | break 61 | except IOError, err: 62 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 63 | time.sleep(0.001) 64 | 65 | def writeList(self, reg, list): 66 | "Writes an array of bytes using I2C format" 67 | while True: 68 | try: 69 | self.bus.write_i2c_block_data(self.address, reg, list) 70 | break 71 | except IOError, err: 72 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 73 | time.sleep(0.001) 74 | 75 | def readU8(self, reg): 76 | "Read an unsigned byte from the I2C device" 77 | while True: 78 | try: 79 | result = self.bus.read_byte_data(self.address, reg) 80 | #logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg) 81 | return result 82 | except IOError, err: 83 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 84 | time.sleep(0.001) 85 | 86 | def readS8(self, reg): 87 | "Reads a signed byte from the I2C device" 88 | while True: 89 | try: 90 | result = self.bus.read_byte_data(self.address, reg) 91 | #logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg) 92 | if (result > 127): 93 | return result - 256 94 | else: 95 | return result 96 | except IOError, err: 97 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 98 | time.sleep(0.001) 99 | 100 | def readU16(self, reg): 101 | "Reads an unsigned 16-bit value from the I2C device" 102 | while True: 103 | try: 104 | hibyte = self.bus.read_byte_data(self.address, reg) 105 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) 106 | #logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg) 107 | if result == 0x7FFF or result == 0x8000: 108 | #logger.critical('I2C read max value') 109 | time.sleep(0.001) 110 | else: 111 | return result 112 | except IOError, err: 113 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 114 | time.sleep(0.001) 115 | 116 | def readS16(self, reg): 117 | "Reads a signed 16-bit value from the I2C device" 118 | while True: 119 | try: 120 | hibyte = self.bus.read_byte_data(self.address, reg) 121 | if (hibyte > 127): 122 | hibyte -= 256 123 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) 124 | #logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg) 125 | if result == 0x7FFF or result == 0x8000: 126 | #logger.critical('I2C read max value') 127 | time.sleep(0.001) 128 | else: 129 | return result 130 | except IOError, err: 131 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 132 | time.sleep(0.001) 133 | 134 | def readList(self, reg, length): 135 | "Reads a a byte array value from the I2C device" 136 | while True: 137 | try: 138 | result = self.bus.read_i2c_block_data(self.address, reg, length) 139 | #logger.debug('I2C: Device 0x%02X from reg 0x%02X', self.address, reg) 140 | return result 141 | except IOError, err: 142 | #logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) 143 | time.sleep(0.001) 144 | 145 | 146 | ############################################################################################ 147 | # 148 | # Gyroscope / Accelerometer class for reading position / movement 149 | # 150 | ############################################################################################ 151 | class MPU6050 : 152 | i2c = None 153 | 154 | 155 | # Registers/etc. 156 | __MPU6050_RA_XG_OFFS_TC= 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 157 | __MPU6050_RA_YG_OFFS_TC= 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 158 | __MPU6050_RA_ZG_OFFS_TC= 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 159 | __MPU6050_RA_X_FINE_GAIN= 0x03 # [7:0] X_FINE_GAIN 160 | __MPU6050_RA_Y_FINE_GAIN= 0x04 # [7:0] Y_FINE_GAIN 161 | __MPU6050_RA_Z_FINE_GAIN= 0x05 # [7:0] Z_FINE_GAIN 162 | __MPU6050_RA_XA_OFFS_H= 0x06 # [15:0] XA_OFFS 163 | __MPU6050_RA_XA_OFFS_L_TC= 0x07 164 | __MPU6050_RA_YA_OFFS_H= 0x08 # [15:0] YA_OFFS 165 | __MPU6050_RA_YA_OFFS_L_TC= 0x09 166 | __MPU6050_RA_ZA_OFFS_H= 0x0A # [15:0] ZA_OFFS 167 | __MPU6050_RA_ZA_OFFS_L_TC= 0x0B 168 | __MPU6050_RA_XG_OFFS_USRH= 0x13 # [15:0] XG_OFFS_USR 169 | __MPU6050_RA_XG_OFFS_USRL= 0x14 170 | __MPU6050_RA_YG_OFFS_USRH= 0x15 # [15:0] YG_OFFS_USR 171 | __MPU6050_RA_YG_OFFS_USRL= 0x16 172 | __MPU6050_RA_ZG_OFFS_USRH= 0x17 # [15:0] ZG_OFFS_USR 173 | __MPU6050_RA_ZG_OFFS_USRL= 0x18 174 | __MPU6050_RA_SMPLRT_DIV= 0x19 175 | __MPU6050_RA_CONFIG= 0x1A 176 | __MPU6050_RA_GYRO_CONFIG= 0x1B 177 | __MPU6050_RA_ACCEL_CONFIG= 0x1C 178 | __MPU6050_RA_FF_THR= 0x1D 179 | __MPU6050_RA_FF_DUR= 0x1E 180 | __MPU6050_RA_MOT_THR= 0x1F 181 | __MPU6050_RA_MOT_DUR= 0x20 182 | __MPU6050_RA_ZRMOT_THR= 0x21 183 | __MPU6050_RA_ZRMOT_DUR= 0x22 184 | __MPU6050_RA_FIFO_EN= 0x23 185 | __MPU6050_RA_I2C_MST_CTRL= 0x24 186 | __MPU6050_RA_I2C_SLV0_ADDR= 0x25 187 | __MPU6050_RA_I2C_SLV0_REG= 0x26 188 | __MPU6050_RA_I2C_SLV0_CTRL= 0x27 189 | __MPU6050_RA_I2C_SLV1_ADDR= 0x28 190 | __MPU6050_RA_I2C_SLV1_REG= 0x29 191 | __MPU6050_RA_I2C_SLV1_CTRL= 0x2A 192 | __MPU6050_RA_I2C_SLV2_ADDR= 0x2B 193 | __MPU6050_RA_I2C_SLV2_REG= 0x2C 194 | __MPU6050_RA_I2C_SLV2_CTRL= 0x2D 195 | __MPU6050_RA_I2C_SLV3_ADDR= 0x2E 196 | __MPU6050_RA_I2C_SLV3_REG= 0x2F 197 | __MPU6050_RA_I2C_SLV3_CTRL= 0x30 198 | __MPU6050_RA_I2C_SLV4_ADDR= 0x31 199 | __MPU6050_RA_I2C_SLV4_REG= 0x32 200 | __MPU6050_RA_I2C_SLV4_DO= 0x33 201 | __MPU6050_RA_I2C_SLV4_CTRL= 0x34 202 | __MPU6050_RA_I2C_SLV4_DI= 0x35 203 | __MPU6050_RA_I2C_MST_STATUS= 0x36 204 | __MPU6050_RA_INT_PIN_CFG= 0x37 205 | __MPU6050_RA_INT_ENABLE= 0x38 206 | __MPU6050_RA_DMP_INT_STATUS= 0x39 207 | __MPU6050_RA_INT_STATUS= 0x3A 208 | __MPU6050_RA_ACCEL_XOUT_H= 0x3B 209 | __MPU6050_RA_ACCEL_XOUT_L= 0x3C 210 | __MPU6050_RA_ACCEL_YOUT_H= 0x3D 211 | __MPU6050_RA_ACCEL_YOUT_L= 0x3E 212 | __MPU6050_RA_ACCEL_ZOUT_H= 0x3F 213 | __MPU6050_RA_ACCEL_ZOUT_L= 0x40 214 | __MPU6050_RA_TEMP_OUT_H= 0x41 215 | __MPU6050_RA_TEMP_OUT_L= 0x42 216 | __MPU6050_RA_GYRO_XOUT_H= 0x43 217 | __MPU6050_RA_GYRO_XOUT_L= 0x44 218 | __MPU6050_RA_GYRO_YOUT_H= 0x45 219 | __MPU6050_RA_GYRO_YOUT_L= 0x46 220 | __MPU6050_RA_GYRO_ZOUT_H= 0x47 221 | __MPU6050_RA_GYRO_ZOUT_L= 0x48 222 | __MPU6050_RA_EXT_SENS_DATA_00= 0x49 223 | __MPU6050_RA_EXT_SENS_DATA_01= 0x4A 224 | __MPU6050_RA_EXT_SENS_DATA_02= 0x4B 225 | __MPU6050_RA_EXT_SENS_DATA_03= 0x4C 226 | __MPU6050_RA_EXT_SENS_DATA_04= 0x4D 227 | __MPU6050_RA_EXT_SENS_DATA_05= 0x4E 228 | __MPU6050_RA_EXT_SENS_DATA_06= 0x4F 229 | __MPU6050_RA_EXT_SENS_DATA_07= 0x50 230 | __MPU6050_RA_EXT_SENS_DATA_08= 0x51 231 | __MPU6050_RA_EXT_SENS_DATA_09= 0x52 232 | __MPU6050_RA_EXT_SENS_DATA_10= 0x53 233 | __MPU6050_RA_EXT_SENS_DATA_11= 0x54 234 | __MPU6050_RA_EXT_SENS_DATA_12= 0x55 235 | __MPU6050_RA_EXT_SENS_DATA_13= 0x56 236 | __MPU6050_RA_EXT_SENS_DATA_14= 0x57 237 | __MPU6050_RA_EXT_SENS_DATA_15= 0x58 238 | __MPU6050_RA_EXT_SENS_DATA_16= 0x59 239 | __MPU6050_RA_EXT_SENS_DATA_17= 0x5A 240 | __MPU6050_RA_EXT_SENS_DATA_18= 0x5B 241 | __MPU6050_RA_EXT_SENS_DATA_19= 0x5C 242 | __MPU6050_RA_EXT_SENS_DATA_20= 0x5D 243 | __MPU6050_RA_EXT_SENS_DATA_21= 0x5E 244 | __MPU6050_RA_EXT_SENS_DATA_22= 0x5F 245 | __MPU6050_RA_EXT_SENS_DATA_23= 0x60 246 | __MPU6050_RA_MOT_DETECT_STATUS= 0x61 247 | __MPU6050_RA_I2C_SLV0_DO= 0x63 248 | __MPU6050_RA_I2C_SLV1_DO= 0x64 249 | __MPU6050_RA_I2C_SLV2_DO= 0x65 250 | __MPU6050_RA_I2C_SLV3_DO= 0x66 251 | __MPU6050_RA_I2C_MST_DELAY_CTRL= 0x67 252 | __MPU6050_RA_SIGNAL_PATH_RESET= 0x68 253 | __MPU6050_RA_MOT_DETECT_CTRL= 0x69 254 | __MPU6050_RA_USER_CTRL= 0x6A 255 | __MPU6050_RA_PWR_MGMT_1= 0x6B 256 | __MPU6050_RA_PWR_MGMT_2= 0x6C 257 | __MPU6050_RA_BANK_SEL= 0x6D 258 | __MPU6050_RA_MEM_START_ADDR= 0x6E 259 | __MPU6050_RA_MEM_R_W= 0x6F 260 | __MPU6050_RA_DMP_CFG_1= 0x70 261 | __MPU6050_RA_DMP_CFG_2= 0x71 262 | __MPU6050_RA_FIFO_COUNTH= 0x72 263 | __MPU6050_RA_FIFO_COUNTL= 0x73 264 | __MPU6050_RA_FIFO_R_W= 0x74 265 | __MPU6050_RA_WHO_AM_I= 0x75 266 | 267 | __CALIBRATION_ITERATIONS = 100 268 | __k_norm = 0.0 269 | 270 | def CheckSetting(self): 271 | if self.i2c.readU8(self.__MPU6050_RA_SMPLRT_DIV)is not 0x04: 272 | print('IMU Error: __MPU6050_RA_SMPLRT_DIV Failed:' + str(self.i2c.readU8(self.__MPU6050_RA_SMPLRT_DIV))) 273 | time.sleep(2) 274 | 275 | if self.i2c.readU8(self.__MPU6050_RA_PWR_MGMT_1)is not 0x03: 276 | print('IMU Error: __MPU6050_RA_PWR_MGMT_1 Failed: ' + str(self.i2c.readU8(self.__MPU6050_RA_PWR_MGMT_1))) 277 | time.sleep(2) 278 | 279 | if self.i2c.readU8(self.__MPU6050_RA_CONFIG)is not 0x05: 280 | print('IMU Error: __MPU6050_RA_CONFIG Failed: ' + str(self.i2c.readU8(self.__MPU6050_RA_CONFIG))) 281 | time.sleep(2) 282 | 283 | if self.i2c.readU8(self.__MPU6050_RA_GYRO_CONFIG)is not 0x00: 284 | print('IMU Error: __MPU6050_RA_GYRO_CONFIG Failed: ' + str(self.i2c.readU8(self.__MPU6050_RA_GYRO_CONFIG))) 285 | time.sleep(2) 286 | 287 | def __init__(self, address=0x68): 288 | 289 | print ('IMU initializing...') 290 | self.i2c = I2C(address) 291 | self.address = address 292 | self.ax_offset = 0 293 | self.ay_offset = 0 294 | self.az_offset = 0 295 | self.gx_offset = 0 296 | self.gy_offset = 0 297 | self.gz_offset = 0 298 | self.sensor_data = array('B', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) 299 | self.result_array = array('h', [0, 0, 0, 0, 0, 0, 0]) 300 | ###logger = logging.get##logger(__name__) 301 | ##logger.info('Reseting MPU-6050') 302 | 303 | #--------------------------------------------------------------------------- 304 | # Reset all registers 305 | #--------------------------------------------------------------------------- 306 | ##logger.debug('Reset all registers') 307 | #SNT note: corrected the param 308 | self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1,0x80) 309 | #self.i2c.write8(self.__MPU6050_RA_CONFIG, 0x80) 310 | time.sleep(5) 311 | 312 | #--------------------------------------------------------------------------- 313 | # ********************************: Experimental :************************** 314 | 315 | 316 | #--------------------------------------------------------------------------- 317 | # Sets clock source to gyro reference w/ PLL 318 | #--------------------------------------------------------------------------- 319 | ##logger.debug('Clock gyro PLL con x reference') 320 | #SNT: 0x02 >> 0x03 (pll con z gyro reference) 321 | self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0x03) 322 | time.sleep(0.005) 323 | 324 | 325 | # Sets sample rate to 1000/1+4 = 200Hz 326 | #--------------------------------------------------------------------------- 327 | ##logger.debug('Sample rate 100Hz') 328 | self.i2c.write8(self.__MPU6050_RA_SMPLRT_DIV, 0x04) 329 | time.sleep(0.005) 330 | 331 | #SNT note: moved up to solve a bug in MPU6050: 332 | #CONFIG has to set just after PWR_MGMT_1 333 | #--------------------------------------------------------------------------- 334 | # ********************************: Experimental :************************** 335 | # Disable FSync, 5Hz DLPF => 1kHz sample frequency used above divided by the 336 | # sample divide factor. 337 | #--------------------------------------------------------------------------- 338 | ##logger.debug('5Hz DLPF') 339 | # 0x02 => 98Hz 2ms delay 340 | # 0x03 => 40Hz 4 341 | # 0x04 => 20Hz 8 342 | # 0x05 => 10Hz 15 343 | self.i2c.write8(self.__MPU6050_RA_CONFIG, 0x05) 344 | time.sleep(0.005) 345 | 346 | #print(str(self.i2c.readU8(self.__MPU6050_RA_CONFIG))) 347 | 348 | #--------------------------------------------------------------------------- 349 | # ********************************: Experimental :************************** 350 | # Disable gyro self tests, scale of +/- 500 degrees/s 351 | #--------------------------------------------------------------------------- 352 | #0x00=+/-250 0x08=+/- 500 0x10=+/-1000 0x18=+/-2000 353 | #logger.debug('Gyro +/-500 degrees/s') 354 | #SNT modified in 0x00 355 | self.i2c.write8(self.__MPU6050_RA_GYRO_CONFIG, 0x00) 356 | self.gyro_scale=250 357 | time.sleep(0.005) 358 | 359 | #--------------------------------------------------------------------------- 360 | # ********************************: Experimental :************************** 361 | # Disable accel self tests, scale of +/-2g 362 | #--------------------------------------------------------------------------- 363 | #0x00=+/-2 0x08=+/- 4 0x10=+/-8 0x18=+/-16 364 | #logger.debug('Accel +/- 2g') 365 | self.i2c.write8(self.__MPU6050_RA_ACCEL_CONFIG, 0x00) 366 | self.accel_scale=2 367 | time.sleep(0.005) 368 | 369 | #-------------------------------------------------------------------------- 370 | # Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 371 | #--------------------------------------------------------------------------- 372 | self.i2c.write8(self.__MPU6050_RA_USER_CTRL, 0x00) 373 | time.sleep(0.005) 374 | 375 | #--------------------------------------------------------------------------- 376 | # Setup INT pin to latch and AUX I2C pass through 377 | #--------------------------------------------------------------------------- 378 | ##logger.debug('Enable interrupt') 379 | #SNT 0x20>0x02 380 | self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG, 0x02) 381 | time.sleep(0.005) 382 | 383 | #--------------------------------------------------------------------------- 384 | # Controls frequency of wakeups in accel low power mode plus the sensor standby modes 385 | #--------------------------------------------------------------------------- 386 | ##logger.debug('Disable low-power') 387 | self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_2, 0x00) 388 | time.sleep(0.005) 389 | 390 | #--------------------------------------------------------------------------- 391 | # ********************************: Experimental :************************** 392 | # Enable data ready interrupt 393 | #--------------------------------------------------------------------------- 394 | ##logger.debug('Interrupt data ready') 395 | self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x01) 396 | time.sleep(0.005) 397 | 398 | ##logger.debug('Gumph hereafter...') 399 | 400 | #--------------------------------------------------------------------------- 401 | # Freefall threshold of |0mg| 402 | #--------------------------------------------------------------------------- 403 | self.i2c.write8(self.__MPU6050_RA_FF_THR, 0x00) 404 | time.sleep(0.005) 405 | 406 | #--------------------------------------------------------------------------- 407 | # Freefall duration limit of 0 408 | #--------------------------------------------------------------------------- 409 | self.i2c.write8(self.__MPU6050_RA_FF_DUR, 0x00) 410 | time.sleep(0.005) 411 | 412 | #--------------------------------------------------------------------------- 413 | # Motion threshold of 0mg 414 | #--------------------------------------------------------------------------- 415 | self.i2c.write8(self.__MPU6050_RA_MOT_THR, 0x00) 416 | time.sleep(0.005) 417 | 418 | #--------------------------------------------------------------------------- 419 | # Motion duration of 0s 420 | #--------------------------------------------------------------------------- 421 | self.i2c.write8(self.__MPU6050_RA_MOT_DUR, 0x00) 422 | time.sleep(0.005) 423 | 424 | #--------------------------------------------------------------------------- 425 | # Zero motion threshold 426 | #--------------------------------------------------------------------------- 427 | self.i2c.write8(self.__MPU6050_RA_ZRMOT_THR, 0x00) 428 | time.sleep(0.005) 429 | 430 | #--------------------------------------------------------------------------- 431 | # Zero motion duration threshold 432 | #--------------------------------------------------------------------------- 433 | self.i2c.write8(self.__MPU6050_RA_ZRMOT_DUR, 0x00) 434 | time.sleep(0.005) 435 | 436 | #--------------------------------------------------------------------------- 437 | # Disable sensor output to FIFO buffer 438 | #--------------------------------------------------------------------------- 439 | self.i2c.write8(self.__MPU6050_RA_FIFO_EN, 0x00) 440 | time.sleep(0.005) 441 | 442 | #--------------------------------------------------------------------------- 443 | # AUX I2C setup 444 | # Sets AUX I2C to single master control, plus other config 445 | #--------------------------------------------------------------------------- 446 | self.i2c.write8(self.__MPU6050_RA_I2C_MST_CTRL, 0x00) 447 | time.sleep(0.005) 448 | 449 | #--------------------------------------------------------------------------- 450 | # Setup AUX I2C slaves 451 | #--------------------------------------------------------------------------- 452 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_ADDR, 0x00) 453 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_REG, 0x00) 454 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_CTRL, 0x00) 455 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_ADDR, 0x00) 456 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_REG, 0x00) 457 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_CTRL, 0x00) 458 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_ADDR, 0x00) 459 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_REG, 0x00) 460 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_CTRL, 0x00) 461 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_ADDR, 0x00) 462 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_REG, 0x00) 463 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_CTRL, 0x00) 464 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_ADDR, 0x00) 465 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_REG, 0x00) 466 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_DO, 0x00) 467 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_CTRL, 0x00) 468 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_DI, 0x00) 469 | 470 | #--------------------------------------------------------------------------- 471 | # Slave out, dont care 472 | #--------------------------------------------------------------------------- 473 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_DO, 0x00) 474 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_DO, 0x00) 475 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_DO, 0x00) 476 | self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_DO, 0x00) 477 | 478 | #--------------------------------------------------------------------------- 479 | # More slave config 480 | #--------------------------------------------------------------------------- 481 | self.i2c.write8(self.__MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00) 482 | time.sleep(0.005) 483 | 484 | #--------------------------------------------------------------------------- 485 | # Reset sensor signal paths 486 | #--------------------------------------------------------------------------- 487 | self.i2c.write8(self.__MPU6050_RA_SIGNAL_PATH_RESET, 0x00) 488 | time.sleep(0.005) 489 | 490 | #--------------------------------------------------------------------------- 491 | # Motion detection control 492 | #--------------------------------------------------------------------------- 493 | self.i2c.write8(self.__MPU6050_RA_MOT_DETECT_CTRL, 0x00) 494 | time.sleep(0.005) 495 | 496 | 497 | 498 | #--------------------------------------------------------------------------- 499 | # Data transfer to and from the FIFO buffer 500 | #--------------------------------------------------------------------------- 501 | self.i2c.write8(self.__MPU6050_RA_FIFO_R_W, 0x00) 502 | time.sleep(0.005) 503 | 504 | 505 | self.CheckSetting() 506 | 507 | def readSensorsRaw(self): 508 | #--------------------------------------------------------------------------- 509 | # Clear the interrupt by reading the interrupt status register, 510 | #--------------------------------------------------------------------------- 511 | self.i2c.readU8(self.__MPU6050_RA_INT_STATUS) 512 | time.sleep(0.001) 513 | #--------------------------------------------------------------------------- 514 | # Hard loop on the data ready interrupt until it gets set high 515 | #--------------------------------------------------------------------------- 516 | while not (self.i2c.readU8(self.__MPU6050_RA_INT_STATUS) == 0x01): 517 | time.sleep(0.001) 518 | continue 519 | 520 | #--------------------------------------------------------------------------- 521 | # Disable the interrupt while we read the data 522 | #--------------------------------------------------------------------------- 523 | self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x00) 524 | 525 | # #--------------------------------------------------------------------------- 526 | # # Read the sensor data, keeping the interrupt latched 527 | # #--------------------------------------------------------------------------- 528 | # ax = self.i2c.readS16(self.__MPU6050_RA_ACCEL_XOUT_H) 529 | # ay = self.i2c.readS16(self.__MPU6050_RA_ACCEL_YOUT_H) 530 | # az = self.i2c.readS16(self.__MPU6050_RA_ACCEL_ZOUT_H) 531 | # gx = self.i2c.readS16(self.__MPU6050_RA_GYRO_XOUT_H) 532 | # gy = self.i2c.readS16(self.__MPU6050_RA_GYRO_YOUT_H) 533 | # gz = self.i2c.readS16(self.__MPU6050_RA_GYRO_ZOUT_H) 534 | # 535 | # self.result_array = [ax, ay, az, 0, gx, gy, gz] 536 | 537 | #--------------------------------------------------------------------------- 538 | # For speed of reading, read all the sensors and parse to USHORTs after 539 | #--------------------------------------------------------------------------- 540 | sensor_data = self.i2c.readList(self.__MPU6050_RA_ACCEL_XOUT_H, 14) 541 | 542 | for index in range(0, 14, 2): 543 | if (sensor_data[index] > 127): 544 | sensor_data[index] -= 256 545 | self.result_array[int(index / 2)] = (sensor_data[index] << 8) + sensor_data[index + 1] 546 | 547 | #--------------------------------------------------------------------------- 548 | # Reenable the interrupt 549 | #--------------------------------------------------------------------------- 550 | self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x01) 551 | 552 | return self.result_array 553 | 554 | 555 | def readSensors(self): 556 | #--------------------------------------------------------------------------- 557 | # +/- 2g 2 * 16 bit range for the accelerometer 558 | # +/- 500 degrees * 16 bit range for the gyroscope 559 | #--------------------------------------------------------------------------- 560 | [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw() 561 | fax = float(ax / self.__k_norm) 562 | fay = float(ay / self.__k_norm) 563 | faz = float(az / self.__k_norm) 564 | 565 | fgx = float(gx * self.__CALIBRATION_ITERATIONS - self.gx_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 566 | fgy = float(gy * self.__CALIBRATION_ITERATIONS - self.gy_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 567 | fgz = float(gz * self.__CALIBRATION_ITERATIONS - self.gz_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 568 | return fax, fay, faz, fgx, fgy, fgz 569 | 570 | 571 | def readSensors_ORIGINAL(self): 572 | #--------------------------------------------------------------------------- 573 | # +/- 2g 2 * 16 bit range for the accelerometer 574 | # +/- 500 degrees * 16 bit range for the gyroscope 575 | #--------------------------------------------------------------------------- 576 | [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw() 577 | fax = float(ax * self.__CALIBRATION_ITERATIONS - self.ax_offset) * (2*self.accel_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 578 | fay = float(ay * self.__CALIBRATION_ITERATIONS - self.ay_offset) * (2*self.accel_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 579 | faz = float(az * self.__CALIBRATION_ITERATIONS - self.az_offset) * (2*self.accel_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 580 | fgx = float(gx * self.__CALIBRATION_ITERATIONS - self.gx_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 581 | fgy = float(gy * self.__CALIBRATION_ITERATIONS - self.gy_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 582 | fgz = float(gz * self.__CALIBRATION_ITERATIONS - self.gz_offset) * (2*self.gyro_scale) / float(65536 * self.__CALIBRATION_ITERATIONS) 583 | return fax, fay, faz, fgx, fgy, fgz 584 | 585 | 586 | def updateOffsets(self, file_name): 587 | g=9.8 #m/s^2 588 | ax_offset = 0 589 | ay_offset = 0 590 | az_offset = 0 591 | gx_offset = 0 592 | gy_offset = 0 593 | gz_offset = 0 594 | 595 | for loop_count in range(0, self.__CALIBRATION_ITERATIONS): 596 | [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw() 597 | ax_offset += ax 598 | ay_offset += ay 599 | az_offset += az 600 | gx_offset += gx 601 | gy_offset += gy 602 | gz_offset += gz 603 | 604 | time.sleep(0.05) 605 | 606 | self.ax_offset = ax_offset/self.__CALIBRATION_ITERATIONS 607 | self.ay_offset = ay_offset/self.__CALIBRATION_ITERATIONS 608 | self.az_offset = az_offset/self.__CALIBRATION_ITERATIONS 609 | 610 | 611 | self.__k_norm = math.pow((self.ax_offset),2) + math.pow((self.ay_offset),2) + math.pow((self.az_offset),2) 612 | self.__k_norm = self.__k_norm/math.pow((g),2) 613 | self.__k_norm =float(math.pow((self.__k_norm),0.5)) 614 | self.gx_offset = gx_offset 615 | self.gy_offset = gy_offset 616 | self.gz_offset = gz_offset 617 | #print 'k write:' + str(self.__k_norm) 618 | 619 | #--------------------------------------------------------------------------- 620 | # Open the offset config file 621 | #--------------------------------------------------------------------------- 622 | cfg_rc = True 623 | try: 624 | with open(file_name, 'w+') as cfg_file: 625 | cfg_file.write('%d\n' % self.ax_offset) 626 | cfg_file.write('%d\n' % self.ay_offset) 627 | cfg_file.write('%d\n' % self.az_offset) 628 | cfg_file.write('%d\n' % gx_offset) 629 | cfg_file.write('%d\n' % gy_offset) 630 | cfg_file.write('%d\n' % gz_offset) 631 | cfg_file.write('%f\n' % self.__k_norm) 632 | cfg_file.flush() 633 | 634 | except IOError, err: 635 | ##logger.critical('Could not open offset config file: %s for writing', file_name) 636 | cfg_rc = False 637 | 638 | return cfg_rc 639 | 640 | def updateOffsets_ORIGINAL(self, file_name): 641 | ax_offset = 0 642 | ay_offset = 0 643 | az_offset = 0 644 | gx_offset = 0 645 | gy_offset = 0 646 | gz_offset = 0 647 | 648 | for loop_count in range(0, self.__CALIBRATION_ITERATIONS): 649 | [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw() 650 | ax_offset += ax 651 | ay_offset += ay 652 | az_offset += az 653 | gx_offset += gx 654 | gy_offset += gy 655 | gz_offset += gz 656 | 657 | time.sleep(0.05) 658 | 659 | self.ax_offset = ax_offset 660 | self.ay_offset = ay_offset 661 | self.az_offset = az_offset 662 | 663 | self.gx_offset = gx_offset 664 | self.gy_offset = gy_offset 665 | self.gz_offset = gz_offset 666 | 667 | #--------------------------------------------------------------------------- 668 | # Open the offset config file 669 | #--------------------------------------------------------------------------- 670 | cfg_rc = True 671 | try: 672 | with open(file_name, 'w+') as cfg_file: 673 | cfg_file.write('%d\n' % ax_offset) 674 | cfg_file.write('%d\n' % ay_offset) 675 | cfg_file.write('%d\n' % az_offset) 676 | cfg_file.write('%d\n' % gx_offset) 677 | cfg_file.write('%d\n' % gy_offset) 678 | cfg_file.write('%d\n' % gz_offset) 679 | cfg_file.flush() 680 | 681 | except IOError, err: 682 | ##logger.critical('Could not open offset config file: %s for writing', file_name) 683 | cfg_rc = False 684 | 685 | return cfg_rc 686 | 687 | 688 | def readOffsets(self, file_name): 689 | #--------------------------------------------------------------------------- 690 | # Open the Offsets config file, and read the contents 691 | #--------------------------------------------------------------------------- 692 | cfg_rc = True 693 | try: 694 | with open(file_name, 'r') as cfg_file: 695 | str_ax_offset = cfg_file.readline() 696 | str_ay_offset = cfg_file.readline() 697 | str_az_offset = cfg_file.readline() 698 | str_gx_offset = cfg_file.readline() 699 | str_gy_offset = cfg_file.readline() 700 | str_gz_offset = cfg_file.readline() 701 | str_k_norm = cfg_file.readline() 702 | 703 | self.ax_offset = int(str_ax_offset) 704 | self.ay_offset = int(str_ay_offset) 705 | self.az_offset = int(str_az_offset) 706 | self.gx_offset = int(str_gx_offset) 707 | self.gy_offset = int(str_gy_offset) 708 | self.gz_offset = int(str_gz_offset) 709 | self.__k_norm = float(str_k_norm) 710 | 711 | #print 'k read: ' + str(self.__k_norm) 712 | except IOError, err: 713 | ##logger.critical('Could not open offset config file: %s for reading', file_name) 714 | cfg_rc = False 715 | 716 | return cfg_rc 717 | 718 | def getEulerAngles(self, fax, fay, faz): 719 | #--------------------------------------------------------------------------- 720 | # What's the angle in the x and y plane from horizonal? 721 | #--------------------------------------------------------------------------- 722 | 723 | roll= math.atan2(fax, math.pow(math.pow(fay, 2) + math.pow(faz, 2), 0.5)) 724 | pitch = math.atan2(fay, math.pow(math.pow(fax, 2) + math.pow(faz, 2), 0.5)) 725 | yaw = math.atan2(math.pow(math.pow(fax, 2) + math.pow(fay, 2), 0.5), faz) 726 | 727 | roll *= (180 / math.pi) 728 | pitch *= (180 / math.pi) 729 | yaw *= (180 / math.pi) 730 | 731 | return roll,pitch,yaw 732 | 733 | def readTemp(self): 734 | temp = self.i2c.readS16(self.__MPU6050_RA_TEMP_OUT_H) 735 | temp = (float(temp) / 340) + 36.53 736 | ##logger.debug('temp = %s oC', temp) 737 | return temp 738 | 739 | 740 | 741 | 742 | 743 | 744 | 745 | --------------------------------------------------------------------------------