LSM303DLHを用いたRaspberry Piでのデジタルコンパス・加速度入力 プログラム」でLSM303DLHの加速度センサと地磁気センサからモーションデータを取得しましたが、このモーションデータからピッチ、ロール、ヘディングを計算します。

デジタルコンパス・加速度センサ「LSM303DLH」との接続

Raspberry PiとLSM303DLHとの接続は前回と同じですが、今回はRapberryPi Zero Wを次のように接続しました。

加速度センサと地磁気センサからピッチ、ロール、ヘディングの計算式

加速度センサと地磁気センサからピッチ、ロール、ヘディングの計算は、「アルプスIoT Smart Moduleのモーションセンサからピッチ、ロール、ヘディングの取得」で計算しましたが、計算時に例外が発生するため、計算式を少し変えます。

加速度センサからの出力 $(A_x, A_y, A_z)$ をグローバル座標系へと回転させるロール、ピッチ、ヘディングを $\gamma 、 \rho 、 \Psi$ とします。

ロール $\gamma$ は次の計算式で求めます。

\begin{eqnarray}
\gamma & = & \tan^{-1} \left( \frac{A_y}{A_z} \right)
\end{eqnarray}

ピッチ $\rho$ は次の計算式で求めます。

\begin{eqnarray}
\rho & = & \tan^{-1} \left( \frac{-A_x}{\sqrt{A_y^2 + A_z^2}} \right)
\end{eqnarray}

磁気センサからの出力を $(M_x, M_y, M_z)$ をグローバル座標系へと回転させ、加速度センサから算出したロールとピッチを $\gamma 、 \rho$ として、ヘディング $\Psi$ は次の計算式で求めます。

\begin{eqnarray}
M_{x1} & = & M_x\cos\rho+ M_z\sin\rho \\
M_{y1} & = & M_x\sin\gamma\sin\rho + M_y\cos\gamma – M_z\sin\gamma\cos\rho \\ \\
Heading \, \Psi & = & \tan^{-1}(\frac{M_{y1}}{M_{x1}}) & (M_{x1}> 0 で M_{y1}\geq 0 )\\
& = & 180° + \tan^{-1}(\frac{M_{y1}}{M_{x1}}) & (M_{x1}< 0 )\\ & = & 360° + \tan^{-1}(\frac{M_{y1}}{M_{x1}}) & (M_{x1}> 0 で M_{y1}\leq 0 )\\
& = & 90° & (M_{x1}= 0 で M_{y1}< 0 )\\ & = & 270° & (M_{x1}= 0 で M_{y1}> 0 )\\
\end{eqnarray}

前回は、ロール $\gamma$ 、ピッチを $\rho$ を $\sin^{-1}$ 関数を用いて求めましたが、関数の引数が範囲外になって例外が発生しました。このため計算式を変更しています。

ピッチ、ロール、ヘディングの計算ソフトの作成

ピッチ、ロール、ヘディングを計算するソフト「motionpad.py」を次に示します。

#!/usr/bin/env python3
# coding: UTF-8

import math

import time
import board
import busio

import adafruit_lsm303

def computeAngle(GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z):
    print ('computeAngle')
    try:
        Pitch = math.atan(-Acceleration_X/math.sqrt(Acceleration_Y**2+Acceleration_Z**2))
        print ('Pitch:{0:.7f}'.format(Pitch ))
    
        Roll = math.atan(Acceleration_Y/Acceleration_Z)
        print ('Roll:{0:.7f}'.format(Roll ))
 
        Mx = GeoMagnetic_X*math.cos(Pitch)+GeoMagnetic_Z*math.sin(Pitch)
        #Mx = GeoMagnetic_X*math.cos(Pitch)+GeoMagnetic_Y*math.sin(Pitch)*math.sin(Roll) +GeoMagnetic_Z*math.sin(Pitch)*math.cos(Roll)
        print ('cos:{0:.7f} sin:{1:.7f}'.format(math.cos(Pitch),math.sin(Pitch) ))
     
        My=GeoMagnetic_X*math.sin(Roll)*math.sin(Pitch)+GeoMagnetic_Y*math.cos(Roll)-GeoMagnetic_Z*math.sin(Roll)*math.cos(Pitch)
        #My=GeoMagnetic_Y*math.cos(Roll)-GeoMagnetic_Z*math.sin(Roll)
        print ('Mx:{0:.7f} My:{1:.7f}'.format(Mx,My ))
        Heading  = math.atan(My/Mx)  
        
        if Mx>0 and My>=0:
            pass
        elif Mx<0:
            Heading = math.pi + Heading
        elif Mx>0 and My<=0:
            Heading = 2*math.pi + Heading
        elif Mx==0 and My<0:
            Heading = math.pi/2
        elif Mx==0 and My>0:
            Heading = math.pi/2*3
        
    except ValueError:
        print ('computeAngle-except')
        return False  ,0,0,0
     
    return True  ,math.degrees(Roll),math.degrees(Pitch),math.degrees(Heading)
    
def write_report(report):
    with open('/dev/hidg0', 'rb+') as fd:
        fd.write(report.encode())

def main():

    NULL_CHAR = chr(0)

    i2c = busio.I2C(board.SCL, board.SDA)
    sensor = adafruit_lsm303.LSM303(i2c)
    fd = open('/dev/hidg0', 'rb+')
    file=open("test.csv", "w", encoding="utf-8")
    file_raw=open("test_raw.csv", "w", encoding="utf-8")
    i = 0

    while True:
            raw_accel_x, raw_accel_y, raw_accel_z = sensor.raw_acceleration
            accel_x, accel_y, accel_z = sensor.acceleration
            raw_mag_x, raw_mag_y, raw_mag_z = sensor.raw_magnetic
            mag_x, mag_y, mag_z = sensor.magnetic

            print('Acceleration raw: ({0:6d}, {1:6d}, {2:6d}), (m/s^2): ({3:10.3f}, {4:10.3f}, {5:10.3f})'.format(raw_accel_x, raw_accel_y, raw_accel_z, accel_x, accel_y, accel_z))
            print('Magnetometer raw: ({0:6d}, {1:6d}, {2:6d}), (gauss): ({3:10.3f}, {4:10.3f}, {5:10.3f})'.format(raw_mag_x, raw_mag_y, raw_mag_z, mag_x, mag_y, mag_z))
            print('')

            #raw_accel_x =2080
            #raw_accel_y =16
            #raw_accel_z =16240
            #raw_mag_x =187
            #raw_mag_y =-19
            #raw_mag_z =-339


            GeoMagnetic_X = mag_x
            GeoMagnetic_Y = mag_y
            GeoMagnetic_Z = mag_z
            Acceleration_X = accel_x
            Acceleration_Y = accel_y
            Acceleration_Z = accel_z
            
            file.write('{0:.3f} ,{1:.3f} ,{2:.3f},{3:.3f} ,{4:.3f} ,{5:.3f}\n'.format(mag_x,mag_y,mag_z,accel_x,accel_y,accel_z ))
            file_raw.write('{0:.3f} ,{1:.3f} ,{2:.3f},{3:.3f} ,{4:.3f} ,{5:.3f}\n'.format(raw_mag_x,raw_mag_y,raw_mag_z,raw_accel_x,raw_accel_y,raw_accel_z ))
            i = i+1
            if i>1200:
               file.close()
               file_raw.close()
               break
            
            ok,Roll,Pitch,Heading = computeAngle(GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z)
            if ok :
                print ('Roll:{0:.3f} Pitch:{1:.3f} Heading:{2:.3f}'.format(Roll,Pitch,Heading ))
                write_report(chr(1)+chr(0x02)+chr(0)+chr(0x03)+chr(0)+chr(0x04)+chr(0))

            time.sleep(1)

if __name__ == "__main__":
    main()

ピッチ、ロール、ヘディングの計算ソフトの実行

次のコマンドで作成した計算ソフト「」motionpad.pyを実行します。次のように、計算したピッチ、ロール、ヘディングが表示されます。

$ python3 motionpad.py
Acceleration raw: ( -6304,  -1488,   -640), (m/s^2): (    -3.701,     -0.874,     -0.376)
Magnetometer raw: (    55,      2,      8), (gauss): (     5.000,      0.182,      0.816)

computeAngle
Pitch:1.3192895
Roll:1.1646075
cos:0.2488636 sin:0.9685385
Mx:2.0349618 My:4.3338733
Roll:66.727 Pitch:75.590 Heading:64.848
Acceleration raw: (   320,    -16,  16272), (m/s^2): (     0.188,     -0.009,      9.553)
Magnetometer raw: (    49,     -1,     10), (gauss): (     4.455,     -0.091,      1.020)

computeAngle
Pitch:-0.0196631
Roll:-0.0009833
cos:0.9998067 sin:-0.0196619
Mx:4.4336212 My:-0.0898198
Roll:-0.056 Pitch:-1.127 Heading:358.839
Acceleration raw: (   304,     16,  16112), (m/s^2): (     0.178,      0.009,      9.459)
Magnetometer raw: (    57,      2,     11), (gauss): (     5.182,      0.182,      1.122)

computeAngle
Pitch:-0.0188657
Roll:0.0009930
cos:0.9998220 sin:-0.0188646
Mx:5.1597216 My:0.1806066
Roll:0.057 Pitch:-1.081 Heading:2.005
Acceleration raw: (   320,    -80,  16288), (m/s^2): (     0.188,     -0.047,      9.562)
Magnetometer raw: (    55,     -2,      6), (gauss): (     5.000,     -0.182,      0.612)

computeAngle
Pitch:-0.0196436
Roll:-0.0049116
cos:0.9998071 sin:-0.0196423
Mx:4.9870094 My:-0.1783271
Roll:-0.281 Pitch:-1.125 Heading:357.952
Acceleration raw: (   304,     32,  16304), (m/s^2): (     0.178,      0.019,      9.572)
Magnetometer raw: (    50,     -6,      6), (gauss): (     4.545,     -0.545,      0.612)

computeAngle
Pitch:-0.0186435
Roll:0.0019627
cos:0.9998262 sin:-0.0186425
Mx:4.5332509 My:-0.5468213
Roll:0.112 Pitch:-1.068 Heading:353.122