def getGyro(self): mpu = self.mpu gyX = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_XOUT_H), mpu.readReg(MPU6000.R_GYRO_XOUT_L)) gyY = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_YOUT_H), mpu.readReg(MPU6000.R_GYRO_YOUT_L)) gyZ = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_ZOUT_H), mpu.readReg(MPU6000.R_GYRO_ZOUT_L)) raw_gyro = (gyX, gyY, gyZ) gyro = tuple([d / 131.0 for d in raw_gyro]) return gyro
def getAccel(self): mpu = self.mpu aX = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_XOUT_H), mpu.readReg(MPU6000.R_ACCEL_XOUT_L)) aY = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_YOUT_H), mpu.readReg(MPU6000.R_ACCEL_YOUT_L)) aZ = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_ZOUT_H), mpu.readReg(MPU6000.R_ACCEL_ZOUT_L)) raw_accel = (aX, aY, aZ) accel = tuple([d / 16384.0 for d in raw_accel]) return accel
def pollFifo(self): while self.polling: count = self.getFifoCount() while count > 0: if(count % 12 != 0): self.resetFifo() break aX = MPU6000.smashhl(self.getFifo(), self.getFifo()) aY = MPU6000.smashhl(self.getFifo(), self.getFifo()) aZ = MPU6000.smashhl(self.getFifo(), self.getFifo()) gX = MPU6000.smashhl(self.getFifo(), self.getFifo()) gY = MPU6000.smashhl(self.getFifo(), self.getFifo()) gZ = MPU6000.smashhl(self.getFifo(), self.getFifo()) raw_accel = aX,aY,aZ raw_gyro = gX,gY,gZ gyro = tuple([d / (131.0 * 58.82) for d in raw_gyro]) accel = tuple([d / 16384.0 for d in raw_accel]) #self.GYRO_BIAS = tuple(map(operator.div,tuple(map(operator.add, self.GYRO_BIAS, gyro)), (2,2,2))) gyro = tuple(map(operator.add, self.CUR_ROT, gyro)) self.CUR_ROT = tuple(map(operator.add, gyro, self.GYRO_BIAS)) count = self.getFifoCount() time.sleep(.01)
def getFifoCount(self): mpu = self.mpu fifo = MPU6000.smashhl(mpu.readRegU(MPU6000.R_FIFO_COUNT_H), mpu.readRegU(MPU6000.R_FIFO_COUNT_L)) return fifo
def getTemp(self): mpu = self.mpu temph = mpu.readReg(MPU6000.R_TEMP_OUT_H) templ = mpu.readReg(MPU6000.R_TEMP_OUT_L) temp = MPU6000.smashhl(temph, templ) return (int(temp) / 340.0) + 36.53