Pi Pico で６軸センサーのMPU-6050を使ってみる

以下のコードは、加速度計のデータを使用して傾斜角を計算します。ただし、ピッチ、ロール、ヨーの計算には通常、カルマンフィルターや相補フィルターの使用など、より複雑なアルゴリズムとセンサーフュージョンが必要であることに注意することが重要です。ジャイロスコープデータは絶対角度ではなく角速度を提供するため、これらの角度を計算するにはジャイロスコープデータだけでは不十分であり、角度を取得するためにジャイロスコープデータを統合すると、時間の経過とともに大きなドリフトが発生する可能性があります。

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 ) 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: °C" . format ( data [ 'temp' ] ) ) print ( "Tilt angles: X: , Y: , Z: degrees" . format ( tilt_x , tilt_y , tilt_z ) ) print ( "Pitch: , Roll: degrees" . format ( pitch , roll ) )

print ( "Acceleration: X: , Y: , Z: g" . format ( data [ 'accel' ] [ 'x' ] , data [ 'accel' ] [ 'y' ] , data [ 'accel' ] [ 'z' ] ) )

print ( "Gyroscope: X: , Y: , Z: °/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 版はこちら

Be the first to comment Leave a Reply コメントをキャンセル新着・更新

ラズパイ５+Ollama + ローカルLLM + Difyでチャットボット＋RAGを実行 2026年3月25日
ラズパイ５にOllama + ローカルLLM + Dify を導入 2026年3月22日
ラズパイ５にGPUを外付けしてローカルでLLMを動かしてみる（LLM編） 2026年3月19日
ラズパイ５にGPUを外付けしてローカルでLLMを動かしてみる（GPU編） 2026年3月19日
RP2040-Zero をUART経由でラズパイZeroから制御してみる（PWM制御ーメモ） 2026年1月21日