is the reference.
I'm coding a driver for MPU6050 using interrupt pin. With the interrupt and setting at 15.625Hz I get all updated data without losing any. I've implemented a complementary filter.
Code: Select all
# Author: Olivier Lenoir - <olivier.len02@gmail.com>
# Created: 2021-12-29 14:06:10
# License: MIT, Copyright (c) 2021 Olivier Lenoir
# Language: MicroPython v1.17
# Project: MPU-6050
# Description: GY-521 MPU-6050 3-axis gyroscope and acceleration sensor
# picocom /dev/ttyUSB0 -b 115200 | tee MPU6050raw.dat
from machine import Pin, I2C
from ustruct import unpack
from utime import sleep_ms, ticks_us, ticks_diff
from math import sin, asin, radians, degrees
SDA = const(21)
SCL = const(22)
INT_PIN = const(15)
MPU6050_ADDR = const(0x68)
MPU6050_SMPRT_DIV = const(0x19)
MPU6050_CONFIG = const(0x1a)
MPU6050_INT_ENABLE = const(0x38)
MPU6050_ACCEL_OUT = const(0x3b)
MPU6050_TEMP_OUT = const(0x41)
MPU6050_GYRO_OUT = const(0x43)
MPU6050_PWR_MGMT_1 = const(0x6b)
class MPU6050(object):
def __init__(self, ic2, int_pin, addr=MPU6050_ADDR):
self.i2c = i2c
self.int_pin = Pin(int_pin, Pin.IN, Pin.PULL_UP)
self.addr = addr
self.angle_pitch = 0
self.angle_roll = 0
self.angle_pitch_output = 0
self.angle_roll_output = 0
self.set_gyro_angles = False
self._init()
self.calibrate()
self.irq_start(self.gyro)
def _init(self):
# Init MPU6050
# Power sensor
self.i2c.writeto_mem(MPU6050_ADDR, MPU6050_PWR_MGMT_1, bytes([0]))
# Set Digital Low Pass Filter (DLPF) 260Hz, Output rate 1kHz
self.i2c.writeto_mem(MPU6050_ADDR, MPU6050_CONFIG, bytes([6]))
# Set Sample Rate Divider at 64
# Output rate: 1kHz / 64 = 15.625Hz
# Output period: 1 / 15.625Hz = 64ms
self.i2c.writeto_mem(MPU6050_ADDR, MPU6050_SMPRT_DIV, bytes([63]))
self.period = 64 / 1000 # one measure every period in second
self.gyro_lsb = 131 # Gyro LSB Sensitivity
self.gyro_conv = self.period / self.gyro_lsb
# Interrupt enable on data ready
self.i2c.writeto_mem(MPU6050_ADDR, MPU6050_INT_ENABLE, bytes([1]))
sleep_ms(200)
def raw(self, irq=None):
# Get accel, temp and gyro raw data
return unpack('>hhhhhhh', self.i2c.readfrom_mem(self.addr, MPU6050_ACCEL_OUT, 14))
def raw_stdscr(self, irq=None):
# Get accel and gyro raw data normalize with standard score
raw = list(self.raw())
raw.pop(3) # Pop temperature
# Raw accel and standard score gyro
return raw[0:3] + [(r - m) / s for r, m, s in zip(raw[3:6], self.mean[3:6], self.stddev[3:6])]
def calibrate(self, sample=200):
# Calculate mean and standard deviation for accel and gyro
self._sample = 0
self.sample = sample
self.sum = self.sum2 = [0, 0, 0, 0, 0, 0]
self.irq_start(self.mean_stddev)
print('Calibrating', end='')
while self._sample < self.sample:
print('.', end='')
sleep_ms(800)
print()
print('Calibration completed')
def mean_stddev(self, irq=None):
# Calculate mean and standard deviation for accel and gyro
raw_data = list(self.raw())
raw_data.pop(3) # Pop temperature
self.sum = [rd + s for rd, s in zip(raw_data, self.sum)]
self.sum2 = [rd ** 2 + s2 for rd, s2 in zip(raw_data, self.sum2)]
self._sample += 1
if self._sample >= self.sample:
self.irq_stop()
p = self._sample
self.mean = [s / p for s in self.sum]
self.stddev = [(s2 / p - m ** 2) ** 0.5 for m, s2 in zip(self.mean, self.sum2)]
def gyro(self, irq=None):
# update gyro pitch and roll
raw_stdscr = self.raw_stdscr()
accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z = raw_stdscr
accel_vector = sum(a ** 2 for a in raw_stdscr[0:3]) ** 0.5
# update gyro
self.angle_pitch += gyro_x * self.gyro_conv
self.angle_roll += gyro_y * self.gyro_conv
yawed = sin(radians(gyro_z * self.gyro_conv))
self.angle_pitch += self.angle_roll * yawed
self.angle_roll -= self.angle_pitch * yawed
# Accelerometer angle
angle_pitch_accel = degrees(asin(accel_y / accel_vector))
angle_roll_accel = - degrees(asin(accel_x / accel_vector))
# calibration
# angle_pitch_accel -= 0
# angle_roll_accel -= 0
if self.set_gyro_angles:
# Drift correction
self.angle_pitch = self.angle_pitch * 0.98 + angle_pitch_accel * 0.02
self.angle_roll = self.angle_roll * 0.98 + angle_roll_accel * 0.02
else:
# Gyro origin based on gravity
self.angle_pitch = angle_pitch_accel
self.angle_roll = angle_roll_accel
self.set_gyro_angles = True
# Complementary filter
self.angle_pitch_output = self.angle_pitch_output * 0.9 + self.angle_pitch * 0.1
self.angle_roll_output = self.angle_roll_output * 0.9 + self.angle_roll * 0.1
def irq_start(self, func):
# Start IRQ
self.int_pin.irq(trigger=Pin.IRQ_FALLING, handler=func)
def irq_stop(self):
# Stop IRQ
self.int_pin.irq(trigger=Pin.IRQ_FALLING)
def print_raw(self, irq=None):
print(self.raw())
if __name__ == '__main__':
i2c = I2C(0, scl=Pin(SCL), sda=Pin(SDA), freq=400000)
detector = MPU6050(i2c, INT_PIN)
print('Mean:', detector.mean)
print('StdDev:', detector.stddev)
while True:
print(detector.angle_pitch_output, detector.angle_roll_output)
sleep_ms(500)