-
Notifications
You must be signed in to change notification settings - Fork 0
/
MPU9250.py
282 lines (243 loc) · 7.99 KB
/
MPU9250.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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
# coding: utf-8
## @package MPU9250
# This is a FaBo9Axis_MPU9250 library for the FaBo 9AXIS I2C Brick.
#
# http://fabo.io/202.html
#
# Released under APACHE LICENSE, VERSION 2.0
#
# http://www.apache.org/licenses/
#
# FaBo <[email protected]>
import smbus
import time
## MPU9250 Default I2C slave address
SLAVE_ADDRESS = 0x68
## AK8963 I2C slave address
AK8963_SLAVE_ADDRESS = 0x0C
## Device id
DEVICE_ID = 0x71
''' MPU-9250 Register Addresses '''
## sample rate driver
SMPLRT_DIV = 0x19
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
ACCEL_CONFIG = 0x1C
ACCEL_CONFIG_2 = 0x1D
LP_ACCEL_ODR = 0x1E
WOM_THR = 0x1F
FIFO_EN = 0x23
I2C_MST_CTRL = 0x24
I2C_MST_STATUS = 0x36
INT_PIN_CFG = 0x37
INT_ENABLE = 0x38
INT_STATUS = 0x3A
ACCEL_OUT = 0x3B
TEMP_OUT = 0x41
GYRO_OUT = 0x43
I2C_MST_DELAY_CTRL = 0x67
SIGNAL_PATH_RESET = 0x68
MOT_DETECT_CTRL = 0x69
USER_CTRL = 0x6A
PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C
FIFO_R_W = 0x74
WHO_AM_I = 0x75
## Gyro Full Scale Select 250dps
GFS_250 = 0x00
## Gyro Full Scale Select 500dps
GFS_500 = 0x01
## Gyro Full Scale Select 1000dps
GFS_1000 = 0x02
## Gyro Full Scale Select 2000dps
GFS_2000 = 0x03
## Accel Full Scale Select 2G
AFS_2G = 0x00
## Accel Full Scale Select 4G
AFS_4G = 0x01
## Accel Full Scale Select 8G
AFS_8G = 0x02
## Accel Full Scale Select 16G
AFS_16G = 0x03
# AK8963 Register Addresses
AK8963_ST1 = 0x02
AK8963_MAGNET_OUT = 0x03
AK8963_CNTL1 = 0x0A
AK8963_CNTL2 = 0x0B
AK8963_ASAX = 0x10
# CNTL1 Mode select
## Power down mode
AK8963_MODE_DOWN = 0x00
## One shot data output
AK8963_MODE_ONE = 0x01
## Continous data output 8Hz
AK8963_MODE_C8HZ = 0x02
## Continous data output 100Hz
AK8963_MODE_C100HZ = 0x06
# Magneto Scale Select
## 14bit output
AK8963_BIT_14 = 0x00
## 16bit output
AK8963_BIT_16 = 0x01
## smbus
bus = smbus.SMBus(1)
## MPU9250 I2C Controll class
class MPU9250:
## Constructor
# @param [in] address MPU-9250 I2C slave address default:0x68
def __init__(self, address=SLAVE_ADDRESS):
self.address = address
self.configMPU9250(GFS_250, AFS_2G)
self.configAK8963(AK8963_MODE_C8HZ, AK8963_BIT_16)
## Search Device
# @param [in] self The object pointer.
# @retval true device connected
# @retval false device error
def searchDevice(self):
who_am_i = bus.read_byte_data(self.address, WHO_AM_I)
if(who_am_i == DEVICE_ID):
return true
else:
return false
## Configure MPU-9250
# @param [in] self The object pointer.
# @param [in] gfs Gyro Full Scale Select(default:GFS_250[+250dps])
# @param [in] afs Accel Full Scale Select(default:AFS_2G[2g])
def configMPU9250(self, gfs, afs):
if gfs == GFS_250:
self.gres = 250.0/32768.0
elif gfs == GFS_500:
self.gres = 500.0/32768.0
elif gfs == GFS_1000:
self.gres = 1000.0/32768.0
else: # gfs == GFS_2000
self.gres = 2000.0/32768.0
if afs == AFS_2G:
self.ares = 2.0/32768.0
elif afs == AFS_4G:
self.ares = 4.0/32768.0
elif afs == AFS_8G:
self.ares = 8.0/32768.0
else: # afs == AFS_16G:
self.ares = 16.0/32768.0
# sleep off
bus.write_byte_data(self.address, PWR_MGMT_1, 0x00)
time.sleep(0.1)
# auto select clock source
bus.write_byte_data(self.address, PWR_MGMT_1, 0x01)
time.sleep(0.1)
# DLPF_CFG
bus.write_byte_data(self.address, CONFIG, 0x03)
# sample rate divider
bus.write_byte_data(self.address, SMPLRT_DIV, 0x04)
# gyro full scale select
bus.write_byte_data(self.address, GYRO_CONFIG, gfs << 3)
# accel full scale select
bus.write_byte_data(self.address, ACCEL_CONFIG, afs << 3)
# A_DLPFCFG
bus.write_byte_data(self.address, ACCEL_CONFIG_2, 0x03)
# BYPASS_EN
bus.write_byte_data(self.address, INT_PIN_CFG, 0x02)
time.sleep(0.1)
## Configure AK8963
# @param [in] self The object pointer.
# @param [in] mode Magneto Mode Select(default:AK8963_MODE_C8HZ[Continous 8Hz])
# @param [in] mfs Magneto Scale Select(default:AK8963_BIT_16[16bit])
def configAK8963(self, mode, mfs):
if mfs == AK8963_BIT_14:
self.mres = 4912.0/8190.0
else: # mfs == AK8963_BIT_16:
self.mres = 4912.0/32760.0
bus.write_byte_data(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x00)
time.sleep(0.01)
# set read FuseROM mode
bus.write_byte_data(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x0F)
time.sleep(0.01)
# read coef data
data = bus.read_i2c_block_data(AK8963_SLAVE_ADDRESS, AK8963_ASAX, 3)
self.magXcoef = (data[0] - 128) / 256.0 + 1.0
self.magYcoef = (data[1] - 128) / 256.0 + 1.0
self.magZcoef = (data[2] - 128) / 256.0 + 1.0
# set power down mode
bus.write_byte_data(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, 0x00)
time.sleep(0.01)
# set scale&continous mode
bus.write_byte_data(AK8963_SLAVE_ADDRESS, AK8963_CNTL1, (mfs<<4|mode))
time.sleep(0.01)
## brief Check data ready
# @param [in] self The object pointer.
# @retval true data is ready
# @retval false data is not ready
def checkDataReady(self):
drdy = bus.read_byte_data(self.address, INT_STATUS)
if drdy & 0x01:
return True
else:
return False
## Read accelerometer
# @param [in] self The object pointer.
# @retval x : x-axis data
# @retval y : y-axis data
# @retval z : z-axis data
def readAccel(self):
data = bus.read_i2c_block_data(self.address, ACCEL_OUT, 6)
x = self.dataConv(data[1], data[0])
y = self.dataConv(data[3], data[2])
z = self.dataConv(data[5], data[4])
x = round(x*self.ares, 3)
y = round(y*self.ares, 3)
z = round(z*self.ares, 3)
return {"x":x, "y":y, "z":z}
## Read gyro
# @param [in] self The object pointer.
# @retval x : x-gyro data
# @retval y : y-gyro data
# @retval z : z-gyro data
def readGyro(self):
data = bus.read_i2c_block_data(self.address, GYRO_OUT, 6)
x = self.dataConv(data[1], data[0])
y = self.dataConv(data[3], data[2])
z = self.dataConv(data[5], data[4])
x = round(x*self.gres, 3)
y = round(y*self.gres, 3)
z = round(z*self.gres, 3)
return {"x":x, "y":y, "z":z}
## Read magneto
# @param [in] self The object pointer.
# @retval x : X-magneto data
# @retval y : y-magneto data
# @retval z : Z-magneto data
def readMagnet(self):
x=0
y=0
z=0
# check data ready
drdy = bus.read_byte_data(AK8963_SLAVE_ADDRESS, AK8963_ST1)
if drdy & 0x01 :
data = bus.read_i2c_block_data(AK8963_SLAVE_ADDRESS, AK8963_MAGNET_OUT, 7)
# check overflow
if (data[6] & 0x08)!=0x08:
x = self.dataConv(data[0], data[1])
y = self.dataConv(data[2], data[3])
z = self.dataConv(data[4], data[5])
x = round(x * self.mres * self.magXcoef, 3)
y = round(y * self.mres * self.magYcoef, 3)
z = round(z * self.mres * self.magZcoef, 3)
return {"x":x, "y":y, "z":z}
## Read temperature
# @param [out] temperature temperature(degrees C)
def readTemperature(self):
data = bus.read_i2c_block_data(self.address, TEMP_OUT, 2)
temp = self.dataConv(data[1], data[0])
temp = round((temp / 333.87 + 21.0), 3)
return temp
## Data Convert
# @param [in] self The object pointer.
# @param [in] data1 LSB
# @param [in] data2 MSB
# @retval Value MSB+LSB(int 16bit)
def dataConv(self, data1, data2):
value = data1 | (data2 << 8)
if(value & (1 << 16 - 1)):
value -= (1<<16)
return value