「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