This repository has been archived by the owner on Feb 12, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathimu_setup.py
71 lines (55 loc) · 1.64 KB
/
imu_setup.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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
import sys
import getopt
import RTIMU
import os.path
import time
import math
import logging
imu_init_success = True
# IMU init
k = 0
if k == 0:
SETTINGS_FILE = "RTIMULib"
if not os.path.exists(SETTINGS_FILE + ".ini"):
imu_init_success = False
print("Settings file does not exist, will be created")
s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)
if (not imu.IMUInit()):
imu_init_success = False
imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
poll_interval = imu.IMUGetPollInterval()
k = 1
# Read Imu number for X time and return the average value
def ReadImu(accel, x):
r = 0
p = 0
y = 0
i = 0
# Collect IMU Data X times and calcue an average
while i < x:
if imu.IMURead():
data = imu.getIMUData() # Read IMU data
fusionPose = data["fusionPose"]
#print("r: %f p: %f y: %f" %(math.degrees(fusionPose[0]), math.degrees(fusionPose[1]),math.degrees(fusionPose[2])))
r = r + math.degrees(fusionPose[0])
p = p + math.degrees(fusionPose[1])
y = y + math.degrees(fusionPose[2])
i = i + 1
accel.roll = r / x
accel.pitch = p / x
accel.yaw = y / x
return 1
# return a array of 3 angle : first : roll 2nd : pitch , 3rd : yaw
def ReadSingleIMU():
if imu.IMURead():
data = imu.getIMUData() # Read IMU data
fusionPose = data["fusionPose"]
time.sleep(imu.IMUGetPollInterval() * 0.001)
return fusionPose
else:
time.sleep(imu.IMUGetPollInterval() * 0.001)
return None