mpu6050 class updated

examples/03-mpu6050/main.py

@@ -1,32 +1,37 @@
 # https://microcontrollerslab.com/micropython-mpu-6050-esp32-esp8266/
+# https://how2electronics.com/measure-pitch-roll-yaw-with-mpu6050-hmc5883l-esp32/
+# https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
+# https://how2electronics.com/interfacing-mpu6050-accelerometer-gyroscope-with-arduino/
 
-from machine import I2C
-from machine import Pin
+from machine import Pin, I2C
 import time
 import mpu6050
 
+led = Pin(2, Pin.OUT)
+
 # Connect the MPU6050 sensor
-i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=100_000)
-mpu = mpu6050.MPU6050(i2c)
+i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400_000)
+print(f"I2C configuration: {str(i2c)}")
 
-# Ignore the first measurement
-mpu.get_raw_values()
-time.sleep(1)
+mpu = mpu6050.MPU6050(i2c)
+led.on()
+mpu.calibrate()
+led.off()
 
-print(f"I2C configuration: {str(i2c)}")
-print()
-print("Start measuring. Press `Ctrl+C` to stop")
+print("\nPress `Ctrl+C` to stop\n")
+# print("roll;pitch;yaw")
 
 try:
     while True:
-        mpu.get_values()
-        print(mpu.get_values())
-        print()
-
-        time.sleep(10)
+        # pitch, roll, yaw = mpu.calculate_orientation()
+        # Different module orientation
+        roll, pitch, yaw = mpu.calculate_orientation()
+        print(f"Roll: {roll:.1f}°\tPitch: {pitch:.1f}°\tYaw(Z): {yaw:.1f}°")
+        time.sleep_ms(200)
 
 except KeyboardInterrupt:
     # This part runs when Ctrl+C is pressed
-    print("Program stopped. Exiting...")
+    print("\nProgram stopped. Exiting...")
 
     # Optional cleanup code
+    led.off()

examples/03-mpu6050/mpu6050.py

@@ -1,49 +1,138 @@
-# import machine
+import time
+import math
+
+# Device addresses
+MPU6050_ADDR = 0x68
+
+# MPU6050 addresses
+ACCEL_XOUT_H = 0x3B
+ACCEL_YOUT_H = 0x3D
+ACCEL_ZOUT_H = 0x3F
+TEMP_OUT_H = 0x41
+GYRO_XOUT_H = 0x43
+GYRO_YOUT_H = 0x45
+GYRO_ZOUT_H = 0x47
+POWER_MGMT_1 = 0x6B
+
+# Sensitivity scales
+ACCEL_SCALE = 16384.0  # Accelerometer scale for ±2g (16-bit)
+GYRO_SCALE = 131.0  # Gyroscope scale for ±250°/s (16-bit)
 
-# BME280 default address
-MPU6050_I2CADDR = 0x68
 
 class MPU6050:
-    def __init__(self, i2c, addr=MPU6050_I2CADDR):
-        self.addr = addr
-        self._i2c = i2c
-        # self.iic.start()
-        self._i2c.writeto(self.addr, bytearray([107, 0]))
-        # self.iic.stop()
-
-    def get_raw_values(self):
-        # self.iic.start()
-        a = self._i2c.readfrom_mem(self.addr, 0x3B, 14)
-        # self.iic.stop()
-        return a
-
-    def get_ints(self):
-        b = self.get_raw_values()
-        c = []
-        for i in b:
-            c.append(i)
-        return c
-
-    def bytes_toint(self, firstbyte, secondbyte):
-        if not firstbyte & 0x80:
-            return firstbyte << 8 | secondbyte
-        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
+    def __init__(self, i2c, address=MPU6050_ADDR):
+        self.address = address
+        self.i2c = i2c
+        self.accel_offset_x = 0
+        self.accel_offset_y = 0
+        self.accel_offset_z = 0
+        self.gyro_offset_x = 0
+        self.gyro_offset_y = 0
+        self.gyro_offset_z = 0
+        self.yaw = 0
+        self.last_time = time.ticks_ms()
+        self._init_mpu6050()
+
+    def _init_mpu6050(self):
+        # Wake up, enable temperature sensor, internal 8MHz oscillator
+        self.i2c.writeto(self.address, bytearray([POWER_MGMT_1, 0]))
+
+        # TODO: Setup resolution and filters
+
+    def _get_raw(self, addr):
+        # Accel_X_H:L
+        # Accel_Y_H:L
+        # Accel_Z_H:L
+        # Temp_H:L
+        # Gyro_X_H:L
+        # Gyro_Y_H:L
+        # Gyro_Z_H:L
+        high, low = self.i2c.readfrom_mem(self.address, addr, 2)
+        if not high & 0x80:
+            return high << 8 | low
+        return - (((high ^ 255) << 8) | (low ^ 255) + 1)
 
     def get_values(self):
-        raw_ints = self.get_raw_values()
-        vals = {}
-        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
-        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
-        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
-        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
-        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
-        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
-        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
-        return vals  # returned in range of Int16
-        # -32768 to 32767
-
-    def val_test(self):  # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
-        import time
-        while True:
-            print(self.get_values())
-            time.sleep(5)
+        ax = self._get_raw(ACCEL_XOUT_H)
+        ay = self._get_raw(ACCEL_YOUT_H)
+        az = self._get_raw(ACCEL_ZOUT_H)
+        temp = self._get_raw(TEMP_OUT_H)
+        gx = self._get_raw(GYRO_XOUT_H)
+        gy = self._get_raw(GYRO_YOUT_H)
+        gz = self._get_raw(GYRO_ZOUT_H)
+
+        # Convert to 'g' (gravity)
+        ax = ax / ACCEL_SCALE - self.accel_offset_x
+        ay = ay / ACCEL_SCALE - self.accel_offset_y
+        az = az / ACCEL_SCALE - self.accel_offset_z
+
+        # Temperature_in_celsius = raw_temperature / 340.0 + 36.53
+        temp = temp / 340.0 + 36.53
+
+        # Convert to degrees per second
+        gx = gx / GYRO_SCALE - self.gyro_offset_x
+        gy = gy / GYRO_SCALE - self.gyro_offset_x
+        gz = gz / GYRO_SCALE - self.gyro_offset_x
+
+        return ax, ay, az, temp, gz, gy, gz
+
+    def calibrate(self):
+        # Calibrate the sensor by averaging several readings for both accelerometer and gyroscope
+        print("Calibrating MPU6050, hold still... ", end="")
+        time.sleep(1)
+
+        n_samples = 200
+        for _ in range(n_samples):
+            ax, ay, az, temp, gx, gy, gz = self.get_values()
+            self.accel_offset_x += ax
+            self.accel_offset_y += ay
+            self.accel_offset_z += az
+            self.gyro_offset_x += gx
+            self.gyro_offset_y += gy
+            self.gyro_offset_z += gz
+            time.sleep_ms(10)
+
+        self.accel_offset_x /= n_samples
+        self.accel_offset_y /= n_samples
+        self.accel_offset_z /= n_samples
+        self.gyro_offset_x /= n_samples
+        self.gyro_offset_y /= n_samples
+        self.gyro_offset_z /= n_samples
+
+        # Adjust for gravity on the Z-axis
+        self.accel_offset_z -= 1.0;
+
+        print("Done")
+
+    def calculate_orientation(self):
+        ax, ay, az, temp, gx, gy, gz = self.get_values()
+
+        # Calculate pitch and roll from accelerometer data
+        # https://engineering.stackexchange.com/questions/3348/calculating-pitch-yaw-and-roll-from-mag-acc-and-gyro-data
+        pitch = math.atan2(-ax, math.sqrt(ay**2 + az**2)) * 180.0 / math.pi
+        roll = math.atan2(ay, az) * 180 / math.pi
+
+        # Time difference in seconds
+        current_time = time.ticks_ms()
+        dt = (current_time - self.last_time) / 1000.0
+        self.last_time = current_time
+
+        # Calculate yaw from gyroscope data (simple integration)
+        self.yaw += gz * dt
+
+        # Pitch: The angle between local level and the longitudinal
+        # axis of the aircraft. It is defined as positive for the nose
+        # of the aircraft pointing above local level.
+        #
+        # Roll: The angle of rotation about the longitudinal axis of
+        # the aircraft. Roll is defined as 0° when the aircraft is
+        # upright and the lateral axis is in the level plane. It is
+        # defined as positive for the right wing of the aircraft below
+        # the left wing.
+        #
+        # Heading: The relative angle between the projection of the
+        # longitudinal axis of the aircraft onto the local level frame
+        # and some definition of North, for example either True North
+        # or Magnetic North. Heading is positive for angles clockwise
+        # from (east of) North.
+        return -pitch, -roll, -self.yaw