MPU-6050 は加速度（３軸）・角速度（３軸）や内部温度も計測できるセンサーです。
コーディングにはMicroPython を使ってみます。ファームウェアをPico にセットしておきます。
元ネタ Interfacing MPU6050 with Raspberry Pi Pico & MicroPython
開発環境はThonny IDE （インストール済み）、母艦はラズパイ4 Model B を使います。
Wiring（配線）
6軸データを取得
Thonny を起動して、以下の２つのコードをPico側に書き込みます。
ライブラリーコード
【mpu6050.py】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 |
from machine import Pin, I2C import utime PWR_MGMT_1 = 0x6B SMPLRT_DIV = 0x19 CONFIG = 0x1A GYRO_CONFIG = 0x1B ACCEL_CONFIG = 0x1C TEMP_OUT_H = 0x41 ACCEL_XOUT_H = 0x3B GYRO_XOUT_H = 0x43 def init_mpu6050(i2c, address=0x68): i2c.writeto_mem(address, PWR_MGMT_1, b'\x00') utime.sleep_ms(100) i2c.writeto_mem(address, SMPLRT_DIV, b'\x07') i2c.writeto_mem(address, CONFIG, b'\x00') i2c.writeto_mem(address, GYRO_CONFIG, b'\x00') i2c.writeto_mem(address, ACCEL_CONFIG, b'\x00') def read_raw_data(i2c, addr, address=0x68): high = i2c.readfrom_mem(address, addr, 1)[0] low = i2c.readfrom_mem(address, addr + 1, 1)[0] value = high << 8 | low if value > 32768: value = value - 65536 return value def get_mpu6050_data(i2c): temp = read_raw_data(i2c, TEMP_OUT_H) / 340.0 + 36.53 accel_x = read_raw_data(i2c, ACCEL_XOUT_H) / 16384.0 accel_y = read_raw_data(i2c, ACCEL_XOUT_H + 2) / 16384.0 accel_z = read_raw_data(i2c, ACCEL_XOUT_H + 4) / 16384.0 gyro_x = read_raw_data(i2c, GYRO_XOUT_H) / 131.0 gyro_y = read_raw_data(i2c, GYRO_XOUT_H + 2) / 131.0 gyro_z = read_raw_data(i2c, GYRO_XOUT_H + 4) / 131.0 return { 'temp': temp, 'accel': { 'x': accel_x, 'y': accel_y, 'z': accel_z, }, 'gyro': { 'x': gyro_x, 'y': gyro_y, 'z': gyro_z, } } |
実行コード
【main.py】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
from machine import Pin, I2C import utime from mpu6050 import init_mpu6050, get_mpu6050_data i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000) init_mpu6050(i2c) while True: data = get_mpu6050_data(i2c) print("Temperature: {:.2f} °C".format(data['temp'])) print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z'])) print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'],data['gyro']['y'], data['gyro']['z'])) utime.sleep(0.5) |
傾斜角 Pitch, Roll, Yaw を計測
以下のコードは、加速度計のデータを使用して傾斜角を計算します。 ただし、ピッチ、ロール、ヨーの計算には通常、カルマン フィルターや相補フィルターの使用など、より複雑なアルゴリズムとセンサー フュージョンが必要であることに注意することが重要です。 ジャイロスコープ データは絶対角度ではなく角速度を提供するため、これらの角度を計算するにはジャイロスコープ データだけでは不十分であり、角度を取得するためにジャイロスコープ データを統合すると、時間の経過とともに大きなドリフトが発生する可能性があります。
ピッチ、ロール、ヨーの値を計算するには、加速度計とジャイロスコープのデータを組み合わせる補完フィルターを使用して、計算された角度の精度を向上させることができます。 以下は、補完的なフィルター実装を含む更新された「main.py」コードです。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 |
from machine import Pin, I2C import utime import math from mpu6050 import init_mpu6050, get_mpu6050_data def calculate_tilt_angles(accel_data): x, y, z = accel_data['x'], accel_data['y'], accel_data['z'] tilt_x = math.atan2(y, math.sqrt(x * x + z * z)) * 180 / math.pi tilt_y = math.atan2(-x, math.sqrt(y * y + z * z)) * 180 / math.pi tilt_z = math.atan2(z, math.sqrt(x * x + y * y)) * 180 / math.pi return tilt_x, tilt_y, tilt_z def complementary_filter(pitch, roll, gyro_data, dt, alpha=0.98): pitch += gyro_data['x'] * dt roll -= gyro_data['y'] * dt pitch = alpha * pitch + (1 - alpha) * math.atan2(gyro_data['y'], math.sqrt(gyro_data['x'] * gyro_data['x'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi roll = alpha * roll + (1 - alpha) * math.atan2(-gyro_data['x'], math.sqrt(gyro_data['y'] * gyro_data['y'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi return pitch, roll i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000) init_mpu6050(i2c) pitch = 0 roll = 0 prev_time = utime.ticks_ms() while True: data = get_mpu6050_data(i2c) curr_time = utime.ticks_ms() dt = (curr_time - prev_time) / 1000 tilt_x, tilt_y, tilt_z = calculate_tilt_angles(data['accel']) pitch, roll = complementary_filter(pitch, roll, data['gyro'], dt) prev_time = curr_time print("Temperature: {:.2f} °C".format(data['temp'])) print("Tilt angles: X: {:.2f}, Y: {:.2f}, Z: {:.2f} degrees".format(tilt_x, tilt_y, tilt_z)) print("Pitch: {:.2f}, Roll: {:.2f} degrees".format(pitch, roll)) print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z'])) print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'], data['gyro']['y'], data['gyro']['z'])) utime.sleep(1) |
Thonny Shell コンソールでは、加速度計、ジャイロスコープ、温度値、および X、Y、Z 軸の傾斜角を確認できます。 これとは別に、ピッチとロールの値も表示されます。 しかし、ヨー値はまだ存在しないままです。
上記のコードは相補フィルターを使用してピッチ角とロール角を計算することに注意してください。 ただし、ヨー角の計算には方位情報が含まれるため、磁力計が必要です。これは、加速度計とジャイロスコープだけでは正確に導き出すことができません。
MPU-6050 には磁力計が内蔵されていないため、MPU-6050 だけを使用してヨーを正確に計算することはできません。 ヨーを計算したい場合は、HMC5883L などの追加センサーを使用するか、MPU-9250 や MPU-9255 などの磁力計が内蔵された IMU を使用する必要があります。
Appendix
Arduino IDE 版はこちら
Leave a Reply