Detection if device is moving (in motion) or not

Discussion about programs, libraries and tools that work with MicroPython. Mostly these are provided by a third party.
Target audience: All users and developers of MicroPython.
User avatar
OlivierLenoir
Posts: 126
Joined: Fri Dec 13, 2019 7:10 pm
Location: Picardie, FR

Re: Detection if device is moving (in motion) or not

Post by OlivierLenoir » Thu Jan 13, 2022 9:52 am

netsrac wrote:
Thu Jan 13, 2022 8:10 am
OlivierLenoir wrote:
Wed Jan 12, 2022 7:54 pm
For asyncio, Peter alias pythoncoder 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.
Stupid question: That's a hardware interrupt of the MPU6050 sensor connected to the Microcontroller?

Never found any references how to use this.
I've shared a wiring on this post.

netsrac
Posts: 17
Joined: Sun Jun 07, 2020 7:19 pm
Location: Berlin, Germany

Re: Detection if device is moving (in motion) or not

Post by netsrac » Thu Jan 13, 2022 4:55 pm

OlivierLenoir wrote:
Wed Jan 12, 2022 7:54 pm
For asyncio, Peter alias pythoncoder 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.
Just thinking about it....could you use this interrupt to wake an ESP32 out of deep sleep mode?

User avatar
OlivierLenoir
Posts: 126
Joined: Fri Dec 13, 2019 7:10 pm
Location: Picardie, FR

Re: Detection if device is moving (in motion) or not

Post by OlivierLenoir » Thu Jan 13, 2022 5:50 pm

netsrac wrote:
Thu Jan 13, 2022 4:55 pm
OlivierLenoir wrote:
Wed Jan 12, 2022 7:54 pm
For asyncio, Peter alias pythoncoder 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.
Just thinking about it....could you use this interrupt to wake an ESP32 out of deep sleep mode?
At 15Hz or even at 5Hz, I don't see the interest of deep sleep.
If you want to reduce consumption, I suggest decreasing ESP32 frequency. In this post, you can estimate the gain.
In the current driver, calculate MPU6050.gyro() with an ESP32 @160MHz take around 4ms. So do not slow down to much the ESP32, otherwise you won't be able to catch each interrupt and gyro result will be wrong.
In the current version, we're focused on gyro or at least I am, but acceleration still need to be implemented to detect speed or inaccurate travels.

User avatar
OlivierLenoir
Posts: 126
Joined: Fri Dec 13, 2019 7:10 pm
Location: Picardie, FR

Re: Detection if device is moving (in motion) or not

Post by OlivierLenoir » Fri Jan 21, 2022 3:48 pm

OlivierLenoir wrote:
Wed Jan 12, 2022 7:54 pm
For asyncio, Peter alias pythoncoder 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.
Read the code and let know if you have questions.

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)
Inspired by Analog Devices application note AN-1057 I've added function to the above class using accelerometer for inclinating sensing.

Code: Select all

class MPU6050(object):
...
    def tilt(self, irq=None):
        # Tri-axis tilt sensing using 3-axis accelerometer
        raw_accel = list(self.raw())[0:3]
        g = sum(a ** 2 for a in raw_accel) ** 0.5
        return [degrees(f(a / g)) for a, f in zip(raw_accel, (asin, asin, acos))]
...

Post Reply