#!/usr/bin/env python from __future__ import division import signal import socket import time import string import sys from RPIO import PWM import RPIO as GPIO import time import math import threading import logging from array import * import smbus import select import os import struct ############################################################################################ # # Gyroscope / Accelerometer class for reading position / movement # ############################################################################################ class MPU6050 : i2c = None # Registers/etc. __MPU6050_RA_XG_OFFS_TC= 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_YG_OFFS_TC= 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_ZG_OFFS_TC= 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_X_FINE_GAIN= 0x03 # [7:0] X_FINE_GAIN __MPU6050_RA_Y_FINE_GAIN= 0x04 # [7:0] Y_FINE_GAIN __MPU6050_RA_Z_FINE_GAIN= 0x05 # [7:0] Z_FINE_GAIN __MPU6050_RA_XA_OFFS_H= 0x06 # [15:0] XA_OFFS __MPU6050_RA_XA_OFFS_L_TC= 0x07 __MPU6050_RA_YA_OFFS_H= 0x08 # [15:0] YA_OFFS __MPU6050_RA_YA_OFFS_L_TC= 0x09 __MPU6050_RA_ZA_OFFS_H= 0x0A # [15:0] ZA_OFFS __MPU6050_RA_ZA_OFFS_L_TC= 0x0B __MPU6050_RA_XG_OFFS_USRH= 0x13 # [15:0] XG_OFFS_USR __MPU6050_RA_XG_OFFS_USRL= 0x14 __MPU6050_RA_YG_OFFS_USRH= 0x15 # [15:0] YG_OFFS_USR __MPU6050_RA_YG_OFFS_USRL= 0x16 __MPU6050_RA_ZG_OFFS_USRH= 0x17 # [15:0] ZG_OFFS_USR __MPU6050_RA_ZG_OFFS_USRL= 0x18 __MPU6050_RA_SMPLRT_DIV= 0x19 __MPU6050_RA_CONFIG= 0x1A __MPU6050_RA_GYRO_CONFIG= 0x1B __MPU6050_RA_ACCEL_CONFIG= 0x1C __MPU6050_RA_FF_THR= 0x1D __MPU6050_RA_FF_DUR= 0x1E __MPU6050_RA_MOT_THR= 0x1F __MPU6050_RA_MOT_DUR= 0x20 __MPU6050_RA_ZRMOT_THR= 0x21 __MPU6050_RA_ZRMOT_DUR= 0x22 __MPU6050_RA_FIFO_EN= 0x23 __MPU6050_RA_I2C_MST_CTRL= 0x24 __MPU6050_RA_I2C_SLV0_ADDR= 0x25 __MPU6050_RA_I2C_SLV0_REG= 0x26 __MPU6050_RA_I2C_SLV0_CTRL= 0x27 __MPU6050_RA_I2C_SLV1_ADDR= 0x28 __MPU6050_RA_I2C_SLV1_REG= 0x29 __MPU6050_RA_I2C_SLV1_CTRL= 0x2A __MPU6050_RA_I2C_SLV2_ADDR= 0x2B __MPU6050_RA_I2C_SLV2_REG= 0x2C __MPU6050_RA_I2C_SLV2_CTRL= 0x2D __MPU6050_RA_I2C_SLV3_ADDR= 0x2E __MPU6050_RA_I2C_SLV3_REG= 0x2F __MPU6050_RA_I2C_SLV3_CTRL= 0x30 __MPU6050_RA_I2C_SLV4_ADDR= 0x31 __MPU6050_RA_I2C_SLV4_REG= 0x32 __MPU6050_RA_I2C_SLV4_DO= 0x33 __MPU6050_RA_I2C_SLV4_CTRL= 0x34 __MPU6050_RA_I2C_SLV4_DI= 0x35 __MPU6050_RA_I2C_MST_STATUS= 0x36 __MPU6050_RA_INT_PIN_CFG= 0x37 __MPU6050_RA_INT_ENABLE= 0x38 __MPU6050_RA_DMP_INT_STATUS= 0x39 __MPU6050_RA_INT_STATUS= 0x3A __MPU6050_RA_ACCEL_XOUT_H= 0x3B __MPU6050_RA_ACCEL_XOUT_L= 0x3C __MPU6050_RA_ACCEL_YOUT_H= 0x3D __MPU6050_RA_ACCEL_YOUT_L= 0x3E __MPU6050_RA_ACCEL_ZOUT_H= 0x3F __MPU6050_RA_ACCEL_ZOUT_L= 0x40 __MPU6050_RA_TEMP_OUT_H= 0x41 __MPU6050_RA_TEMP_OUT_L= 0x42 __MPU6050_RA_GYRO_XOUT_H= 0x43 __MPU6050_RA_GYRO_XOUT_L= 0x44 __MPU6050_RA_GYRO_YOUT_H= 0x45 __MPU6050_RA_GYRO_YOUT_L= 0x46 __MPU6050_RA_GYRO_ZOUT_H= 0x47 __MPU6050_RA_GYRO_ZOUT_L= 0x48 __MPU6050_RA_EXT_SENS_DATA_00= 0x49 __MPU6050_RA_EXT_SENS_DATA_01= 0x4A __MPU6050_RA_EXT_SENS_DATA_02= 0x4B __MPU6050_RA_EXT_SENS_DATA_03= 0x4C __MPU6050_RA_EXT_SENS_DATA_04= 0x4D __MPU6050_RA_EXT_SENS_DATA_05= 0x4E __MPU6050_RA_EXT_SENS_DATA_06= 0x4F __MPU6050_RA_EXT_SENS_DATA_07= 0x50 __MPU6050_RA_EXT_SENS_DATA_08= 0x51 __MPU6050_RA_EXT_SENS_DATA_09= 0x52 __MPU6050_RA_EXT_SENS_DATA_10= 0x53 __MPU6050_RA_EXT_SENS_DATA_11= 0x54 __MPU6050_RA_EXT_SENS_DATA_12= 0x55 __MPU6050_RA_EXT_SENS_DATA_13= 0x56 __MPU6050_RA_EXT_SENS_DATA_14= 0x57 __MPU6050_RA_EXT_SENS_DATA_15= 0x58 __MPU6050_RA_EXT_SENS_DATA_16= 0x59 __MPU6050_RA_EXT_SENS_DATA_17= 0x5A __MPU6050_RA_EXT_SENS_DATA_18= 0x5B __MPU6050_RA_EXT_SENS_DATA_19= 0x5C __MPU6050_RA_EXT_SENS_DATA_20= 0x5D __MPU6050_RA_EXT_SENS_DATA_21= 0x5E __MPU6050_RA_EXT_SENS_DATA_22= 0x5F __MPU6050_RA_EXT_SENS_DATA_23= 0x60 __MPU6050_RA_MOT_DETECT_STATUS= 0x61 __MPU6050_RA_I2C_SLV0_DO= 0x63 __MPU6050_RA_I2C_SLV1_DO= 0x64 __MPU6050_RA_I2C_SLV2_DO= 0x65 __MPU6050_RA_I2C_SLV3_DO= 0x66 __MPU6050_RA_I2C_MST_DELAY_CTRL= 0x67 __MPU6050_RA_SIGNAL_PATH_RESET= 0x68 __MPU6050_RA_MOT_DETECT_CTRL= 0x69 __MPU6050_RA_USER_CTRL= 0x6A __MPU6050_RA_PWR_MGMT_1= 0x6B __MPU6050_RA_PWR_MGMT_2= 0x6C __MPU6050_RA_BANK_SEL= 0x6D __MPU6050_RA_MEM_START_ADDR= 0x6E __MPU6050_RA_MEM_R_W= 0x6F __MPU6050_RA_DMP_CFG_1= 0x70 __MPU6050_RA_DMP_CFG_2= 0x71 __MPU6050_RA_FIFO_COUNTH= 0x72 __MPU6050_RA_FIFO_COUNTL= 0x73 __MPU6050_RA_FIFO_R_W= 0x74 __MPU6050_RA_WHO_AM_I= 0x75 __CALIBRATION_ITERATIONS = 500 def __init__(self, address=0x68): self.i2c = I2C(address) self.address = address self.ax_offset = 0 self.ay_offset = 0 self.az_offset = 0 self.gx_offset = 0 self.gy_offset = 0 self.gz_offset = 0 logger.info('Reseting MPU-6050') #--------------------------------------------------------------------------- # Reset all registers #--------------------------------------------------------------------------- logger.debug('Reset all registers') self.i2c.write8(self.__MPU6050_RA_CONFIG, 0x80); time.sleep(0.005) #--------------------------------------------------------------------------- # Sets sample rate to 1000/1+9 = 100Hz #--------------------------------------------------------------------------- logger.debug('Sample rate 100Hz') self.i2c.write8(self.__MPU6050_RA_SMPLRT_DIV, 0x09); time.sleep(0.005) #--------------------------------------------------------------------------- # Sets clock source to gyro reference w/ PLL #--------------------------------------------------------------------------- logger.debug('Clock gyro PLL') self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0b00000010); time.sleep(0.005) #--------------------------------------------------------------------------- # Controls frequency of wakeups in accel low power mode plus the sensor standby modes #--------------------------------------------------------------------------- logger.debug('Disable low-power') self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_2, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Disable gyro self tests, scale of 500 degrees/s #--------------------------------------------------------------------------- logger.debug('Gyro 500 degrees/s') self.i2c.write8(self.__MPU6050_RA_GYRO_CONFIG, 0b00001000); time.sleep(0.005) #--------------------------------------------------------------------------- # Disable accel self tests, scale of +-2g #--------------------------------------------------------------------------- logger.debug('Accel +/- 2g') self.i2c.write8(self.__MPU6050_RA_ACCEL_CONFIG, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Setup INT pin to latch and AUX I2C pass through #--------------------------------------------------------------------------- logger.debug('Enable interrupt') self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG, 0x30); time.sleep(0.005) #--------------------------------------------------------------------------- # Enable data ready interrupt #--------------------------------------------------------------------------- logger.debug('Interrupt data ready') self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x01); time.sleep(0.005) #--------------------------------------------------------------------------- # Disable FSync, 5Hz DLPF => 1kHz sample frequency above #--------------------------------------------------------------------------- logger.debug('5Hz DLPF') self.i2c.write8(self.__MPU6050_RA_CONFIG, 0x06); time.sleep(0.005) logger.debug('Gumph hereafter...') #--------------------------------------------------------------------------- # Freefall threshold of |0mg| #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_FF_THR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Freefall duration limit of 0 #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_FF_DUR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Motion threshold of 0mg #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_MOT_THR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Motion duration of 0s #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_MOT_DUR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Zero motion threshold #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_ZRMOT_THR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Zero motion duration threshold #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_ZRMOT_DUR, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Disable sensor output to FIFO buffer #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_FIFO_EN, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # AUX I2C setup # Sets AUX I2C to single master control, plus other config #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_I2C_MST_CTRL, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Setup AUX I2C slaves #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_ADDR, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_REG, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_CTRL, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_ADDR, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_REG, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_CTRL, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_ADDR, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_REG, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_CTRL, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_ADDR, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_REG, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_CTRL, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_ADDR, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_REG, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_DO, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_CTRL, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV4_DI, 0x00); #--------------------------------------------------------------------------- # Slave out, dont care #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_I2C_SLV0_DO, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV1_DO, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV2_DO, 0x00); self.i2c.write8(self.__MPU6050_RA_I2C_SLV3_DO, 0x00); #--------------------------------------------------------------------------- # More slave config #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Reset sensor signal paths #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_SIGNAL_PATH_RESET, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Motion detection control #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_MOT_DETECT_CTRL, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_USER_CTRL, 0x00); time.sleep(0.005) #--------------------------------------------------------------------------- # Data transfer to and from the FIFO buffer #--------------------------------------------------------------------------- self.i2c.write8(self.__MPU6050_RA_FIFO_R_W, 0x00); time.sleep(0.005) def readSensorsRaw(self): #--------------------------------------------------------------------------- # Hard loop on the GPIO MPU6050 data ready interrupt #--------------------------------------------------------------------------- while not GPIO.input(GPIO_SENSOR_DATA_RDY): continue #--------------------------------------------------------------------------- # Read the sensor data, keeping the interrupt latched #--------------------------------------------------------------------------- ax = self.i2c.readS16(self.__MPU6050_RA_ACCEL_XOUT_H) ay = self.i2c.readS16(self.__MPU6050_RA_ACCEL_YOUT_H) az = self.i2c.readS16(self.__MPU6050_RA_ACCEL_ZOUT_H) gx = self.i2c.readS16(self.__MPU6050_RA_GYRO_XOUT_H) gy = self.i2c.readS16(self.__MPU6050_RA_GYRO_YOUT_H) gz = self.i2c.readS16(self.__MPU6050_RA_GYRO_ZOUT_H) #--------------------------------------------------------------------------- # Clear the interrupt by reading the interrupt status register #--------------------------------------------------------------------------- self.i2c.readU8(self.__MPU6050_RA_INT_STATUS) return ax, ay, az, gx, gy, gz def readSensors(self): #--------------------------------------------------------------------------- # +/- 2g 2 * 16 bit range for the accelerometer # +/- 500 degrees * 16 bit range for the gyroscope #--------------------------------------------------------------------------- ax, ay, az, gx, gy, gz = self.readSensorsRaw() fax = float(ax * self.__CALIBRATION_ITERATIONS - self.ax_offset) * 4.0 / float(65536 * self.__CALIBRATION_ITERATIONS) fay = float(ay * self.__CALIBRATION_ITERATIONS - self.ay_offset) * 4.0 / float(65536 * self.__CALIBRATION_ITERATIONS) faz = float(az * self.__CALIBRATION_ITERATIONS - self.az_offset) * 4.0 / float(65536 * self.__CALIBRATION_ITERATIONS) fgx = float(gx * self.__CALIBRATION_ITERATIONS - self.gx_offset) * 1000.0 / float(65536 * self.__CALIBRATION_ITERATIONS) fgy = float(gy * self.__CALIBRATION_ITERATIONS - self.gy_offset) * 1000.0 / float(65536 * self.__CALIBRATION_ITERATIONS) fgz = float(gz * self.__CALIBRATION_ITERATIONS - self.gz_offset) * 1000.0 / float(65536 * self.__CALIBRATION_ITERATIONS) return fax, fay, faz, fgx, fgy, fgz def offsetSensors(self): #--------------------------------------------------------------------------- # We now assume the drone is placed on an accurate flat surface #--------------------------------------------------------------------------- ax_offset = 0 ay_offset = 0 az_offset = 0 gx_offset = 0 gy_offset = 0 gz_offset = 0 for loop_count in range(0, self.__CALIBRATION_ITERATIONS): ax, ay, az, gx, gy, gz = self.readSensorsRaw() ax_offset += ax ay_offset += ay az_offset += az gx_offset += gx gy_offset += gy gz_offset += gz self.ax_offset = ax_offset self.ay_offset = ay_offset self.az_offset = az_offset self.gx_offset = gx_offset self.gy_offset = gy_offset self.gz_offset = gz_offset def getEulerAngles(self, fax, fay, faz): #--------------------------------------------------------------------------- # What's the angle in the x and y plane from horizonal? #--------------------------------------------------------------------------- theta = math.atan2(fax, math.pow(math.pow(fay, 2) + math.pow(faz, 2), 0.5)) psi = math.atan2(fay, math.pow(math.pow(fax, 2) + math.pow(faz, 2), 0.5)) phi = math.atan2(math.pow(math.pow(fax, 2) + math.pow(fay, 2), 0.5), faz) theta *= (180 / math.pi) psi *= (180 / math.pi) phi *= (180 / math.pi) return theta, psi, phi def readTemp(self): temp = self.i2c.readS16(self.__MPU6050_RA_TEMP_OUT_H) temp = (float(temp) / 340) + 36.53 logger.debug('temp = %s oC', temp) return temp