[PYTHON] Use "MPU-9250 9-axis sensor module (manufacturer part number: MPU-9250)" manufactured by Strawberry Linux with Rasberry pi 3.
It is a struggle record for using "MPU-9250 9-axis sensor module (manufacturer part number: MPU-9250)" manufactured by Strawberry Linux on Raspberry pi 3. I'm trying with Python2.
Since GW is over and I don't seem to have time anymore, I can do it ... I bought the sensor as an adult, but it was almost untouched. I feel like an elementary school student on the last day of summer vacation.
(It's a long time because I wrote the process of trial and error as it is. I want to summarize it someday.)
2017-04-09 Uploaded to github. https://github.com/boyaki-machine/MPU-9250
Physical wiring
Solder the pin header to the sensor that arrived. Then wire.
Sensor 1pin (VDD)-> Raspi pin 1 Sensor 2pin (VDDIO)-> Raspi pin 1 Sensor 3pin (SCL)-> Raspi 5th pin Sensor 4pin (SDA)-> Raspi pin 3 Sensor 5pin (GND or VDD)-> Raspi pin 6 (I2C address changes depending on GND or VDD) Sensor 6pin (VDDIO)-> Raspi pin 1 Sensor 10pin (GND)-> Raspi 6th pin
It's messy, but it looks like this.
Enable I2C on Raspberry pi
Set in the OS config menu.
$ sudo raspi-config
Select the menu in the order of "9 Advenced Options"-> "A7 I2C". You will be asked "Would you like the ARM I2C interface to be enabled?", So select yes. You will be asked "Would you like the I2C kernel module to be loaded by default?", So select yes.
Next, edit /boot/config.txt.
$ sudo vi /boot/config.txt
...Added the following contents
dtparam=i2c_arm=on
In addition, edit / etc / modules.
$ sudo vi /etc/modules
...Added the following contents
snd-bcm2835
i2c-dev
After completing the settings, restart Raspy. Make sure the kernel module is loaded after rebooting.
$ lsmod
...
i2c_dev 6709 0
snd_bcm2835 21342 0
...
Install tools
$ sudo apt-get install i2c-tools python-smbus
Address confirmation
Check the address of the sensor.
$ sudo i2cdetect -y 1 (When pin 5 is connected to GND)
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
Recognized at address 0x68. This MPU-9250 has a built-in magnetic sensor AK8963 manufactured by Asahi Kasei Electronics. In the initial state, AK8963 cannot be accessed, but it can be accessed by I2C by manipulating some registers. Set the register with the python script below.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
channel = 1
bus = smbus.SMBus(channel)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Magnetic sensor function with I2C(AK8963)Make access to(BYPASS_EN=1)
bus.write_i2c_block_data(address, 0x37, [0x02])
time.sleep(0.1)
Check the address again after executing the script.
$ sudo i2cdetect -y 1 (When pin 5 is connected to VDD)
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- -- -- 0c -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
AK8963 The address 0x0C for access is displayed.
Get acceleration
Get raw data
Here I was allowed to reference.
According to the attachment from Strawberry Linux
Initially it is in sleep mode and no sensing is done. First, write 0x00 to the internal address 0x6B of MPU-9250. Furthermore, 0x02 is written to the internal address 0x37. This will start the operation and enable I2C communication with the magnetic sensor. In this state, the acceleration X, Y, Z gyro X, Y, Z data is stored in 14 bytes from the internal register 0x3B. Each data is 16 bits, and the upper 8 bits are lined up first. Acceleration is easy because you can move the sensor to see the gravitational acceleration. The gyro needs to be rotated, which can be a hassle.
Why is it 14 bytes when it is 16 bits and 6 axes? I was wondering, but looking at the data sheet, it seems that the temperature can also be measured, which is 2 bytes.
For the time being, the acceleration of 6 bytes is acquired and displayed.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Get raw data
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = data[0] << 8 | data[1] #Most significant bit first
rawY = data[2] << 8 | data[3] #Most significant bit first
rawZ = data[4] << 8 | data[5] #Most significant bit first
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
With this, I was able to confirm that the data was output continuously.
Calculate g
Calculate acceleration from raw data. The default setting seems to output in the range of ± 2g. Based on this, I calculated as follows. However, the displayed value is strange.
rawX = (2.0 / float(0x8000)) * (data[0] << 8 | data[1])
rawY = (2.0 / float(0x8000)) * (data[2] << 8 | data[3])
rawZ = (2.0 / float(0x8000)) * (data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
According to various investigations, the value output by the sensor is an Unsigned type, so it seems that it is necessary to convert it to a Signed type in order to express a negative value.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#Convert unsigned to signed(16-bit only)
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Get raw data
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (2.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
rawY = (2.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
rawZ = (2.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
Now, the Z-axis direction outputs almost 1g.
Change the measurement range
When you swing the sensor around, it reaches 2g unexpectedly. So, let's change the measurement range. According to the data sheet, it seems that you can specify a range of 2,4,8,16.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#Convert unsigned to signed(16-bit only)
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Set the accelerometer range to ± 8g
bus.write_i2c_block_data(address, 0x1C, [0x08])
#Get raw data
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
rawY = (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
rawZ = (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
Hmmm, now you can get the value even if you swing around a lot.
Calibrate the acceleration
According to the attachment from Strawberry Linux
Since there are variations in the sensor, the observed value does not become 0 even if the acceleration = 0 g, gyro = 0 ° sec, and magnetism = 0 μT, and the values are slightly different. The allowable offset range is shown in the data sheet, so the range is normal. It will be necessary for the software to make adjustments by subtracting the offset.
There is. Certainly, even if the sensor is fixed, the acceleration on the X and Y axes will not be zero. According to the data sheet, it seems that you can set the offset value on the register, but first try calibration on the python side.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#Convert unsigned to signed(16-bit only)
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Set the accelerometer range to ± 8g
bus.write_i2c_block_data(address, 0x1C, [0x08])
#Calculate the calibration value
print "Accel calibration start"
_sum = [0,0,0]
#Take a sample of real data
for _i in range(1000):
data = bus.read_i2c_block_data(address, 0x3B ,6)
_sum[0] += (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
_sum[1] += (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
_sum[2] += (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
#Offset the average value
offsetAccelX = -1.0 * _sum[0] / 1000
offsetAccelY = -1.0 * _sum[1] / 1000
#Z axis subtracts gravity really 1.0 is suspicious
offsetAccelZ = -1.0 * ((_sum[2] / 1000 ) - 1.0)
print "Accel calibration complete"
#Get raw data
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1]) + offsetAccelX
rawY = (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3]) + offsetAccelY
rawZ = (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5]) + offsetAccelZ
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
Hmmm, it's not exactly 0, but it's better than it was at the beginning. I don't know if you can use the mean value for the offset.
Other
Even after calibration, if the sensor is fixed and continuous sensing is performed, the value will continue to fluctuate slightly. Are you detecting an unknown wave !? It's scary. .. .. Perhaps the sensor is too sensitive to pick up electrical noise. There was "Accelerometer low pass filter setting" in the register list of the data sheet, so if you play with this, you may get a more stable value. I didn't touch it this time because the time was up.
Obtaining temperature
It seems that the temperature can be taken, so I tried it.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Get raw data
while True:
data = bus.read_i2c_block_data(address, 0x65 ,2)
raw = data[0] << 8 | data[1]
#The formula for calculating the temperature is as follows from the data sheet
# ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC
temp = (raw / 333.87) + 21
print str(temp)
With this, the value that seems to be room temperature was obtained. The formula to calculate the temperature from the raw data is written in the data sheet, but the essential values of RoomTemp_Offset and Temp_Sensitivity are not listed, so [here](https://ryusakura.wordpress.com/2015/04/ 23 / raspberrypie% E3% 81% A8mpu-9250% E3% 81% AE% E6% B8% A9% E5% BA% A6% E3% 82% BB% E3% 83% B3% E3% 82% B5 /) I referred to the article. It seems that the followability is not so good, and the value does not change easily even if you hold it with your hand.
Get angular velocity
Data acquisition
The method is the same as for acceleration. It also calculates dps values from raw data, corrects the measurement range, and calculates calibration values.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(1)
#Reset the register
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_Clear 1
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
#Correct the measurement range to 1000dps
bus.write_i2c_block_data(address, 0x1B, [0x10])
#Coefficient to calculate dps
gyroCoefficient = 1000 / float(0x8000)
#Calculate the calibration value
print "Gyro calibration start"
_sum = [0,0,0]
#Take a sample of real data
for _i in range(1000):
data = bus.read_i2c_block_data(address, 0x43 ,6)
_sum[0] += gyroCoefficient * u2s(data[0] << 8 | data[1])
_sum[1] += gyroCoefficient * u2s(data[2] << 8 | data[3])
_sum[2] += gyroCoefficient * u2s(data[4] << 8 | data[5])
#Offset the average value
offsetGyroX = -1.0 * _sum[0] / 1000
offsetGyroY = -1.0 * _sum[1] / 1000
offsetGyroZ = -1.0 * _sum[2] / 1000
print "Gyro calibration complete"
#Get the data
while True:
data = bus.read_i2c_block_data(address, 0x43 ,6)
rawX = gyroCoefficient * u2s(data[0] << 8 | data[1]) + offsetGyroX
rawY = gyroCoefficient * u2s(data[2] << 8 | data[3]) + offsetGyroY
rawZ = gyroCoefficient * u2s(data[4] << 8 | data[5]) + offsetGyroZ
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ + " "
time.sleep(1)
Other
As with the accelerometer, the dps value fluctuates even if the sensor is fixed. It's just an error when viewed from the measurement range, but it doesn't settle down. .. .. I'm hoping that this can also be improved by setting DLPF_CFG in the 0x1A register, but I couldn't try it this time.
Acquisition of magnetic sensor
The magnetic sensor is AK8963 manufactured by Asahi Kasei Electronics. I really appreciate the Japanese data sheet. .. ..
It's a shame, but I thought that the sensor was the only Japanese manufacturer, but even if I checked the sample module connected to the raspi, it was only made overseas. I want Japanese manufacturers to do their best.
So, with the AK8963, it's easy to just take the data once, but when trying to perform continuous sensing, various procedures were troublesome. I mean, it's a maniac design, or you can make strangely detailed settings. .. ..
There are four measurement modes: single shot, continuous 1 (8Hz), continuous 2 (100Hz), and external trigger. In addition, the number of bits to output the measured value can be selected in 16bit / 14bit. In single-shot mode, once sensing, it will transition to Power down, so you need to set the single-shot mode again. Especially in the continuous sensing mode, the later (new) data will be discarded until the previous data is retrieved. The retrieved data should be checked each time for overflow. It seems that the correct value will not come out if it overflows.
Also, I was addicted to the fact that the lower bits can be acquired first when reading data from the register. Please note that the high-order bit comes first for acceleration and angular velocity.
With that feeling, I thought I would extract only the magnetic sensor part and write it, but it is quite difficult, so please read the classified operation.
It is difficult to judge whether the magnetic sensor is correct even if the value is obtained. According to wiki It seems that "the strength of the geomagnetism at 50 degrees longitude: 58 μT", so I don't think it is a very different value.
Also, I wanted to calculate the bearing from each axis μT, but I couldn't handle it either. Is it the next issue?
Make a class together
Classify the results so far together.
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
#"MPU" from Strawberry Linux-Class to get data by I2C from "9250"(python 2)
# https://strawberry-linux.com/catalog/items?code=12250
#
# 2016-05-03 Boyaki Machine
class SL_MPU9250:
#Constant declaration
REG_PWR_MGMT_1 = 0x6B
REG_INT_PIN_CFG = 0x37
REG_GYRO_CONFIG = 0x1B
REG_ACCEL_CONFIG1 = 0x1C
REG_ACCEL_CONFIG2 = 0x1D
MAG_MODE_POWERDOWN = 0 #Magnetic sensor power down
MAG_MODE_SERIAL_1 = 1 #Magnetic sensor 8Hz continuous measurement mode
MAG_MODE_SERIAL_2 = 2 #Magnetic sensor 100Hz continuous measurement mode
MAG_MODE_SINGLE = 3 #Magnetic sensor single-shot measurement mode
MAG_MODE_EX_TRIGER = 4 #Magnetic sensor external trigger measurement mode
MAG_MODE_SELF_TEST = 5 #Magnetic sensor self-test mode
MAG_ACCESS = False #Access to the magnetic sensor
MAG_MODE = 0 #Magnetic sensor mode
MAG_BIT = 14 #Number of bits output by the magnetic sensor
offsetRoomTemp = 0
tempSensitivity = 333.87
gyroRange = 250 # 'dps' 00:250, 01:500, 10:1000, 11:2000
accelRange = 2 # 'g' 00:±2, 01:±4, 10:±8, 11:±16
magRange = 4912 # 'μT'
offsetAccelX = 0.0
offsetAccelY = 0.0
offsetAccelZ = 0.0
offsetGyroX = 0.0
offsetGyroY = 0.0
offsetGyroZ = 0.0
#constructor
def __init__(self, address, channel):
self.address = address
self.channel = channel
self.bus = smbus.SMBus(self.channel)
self.addrAK8963 = 0x0C
# Sensor initialization
self.resetRegister()
self.powerWakeUp()
self.gyroCoefficient = self.gyroRange / float(0x8000) #Coefficient to convert the sensed Decimal value to dps
self.accelCoefficient = self.accelRange / float(0x8000) #Coefficient to convert the sensed Decimal value to g
self.magCoefficient16 = self.magRange / 32760.0 #Coefficient to convert the sensed Decimal value to μT(At 16bit)
self.magCoefficient14 = self.magRange / 8190.0 #Coefficient to convert the sensed Decimal value to μT(At 14bit)
#Resets the registers to their default settings.
def resetRegister(self):
if self.MAG_ACCESS == True:
self.bus.write_i2c_block_data(self.addrAK8963, 0x0B, [0x01])
self.bus.write_i2c_block_data(self.address, 0x6B, [0x80])
self.MAG_ACCESS = False
time.sleep(0.1)
#Makes the register measurable.
def powerWakeUp(self):
# PWR_MGMT_Clear 1
self.bus.write_i2c_block_data(self.address, self.REG_PWR_MGMT_1, [0x00])
time.sleep(0.1)
#Magnetic sensor function with I2C(AK8963)Make access to(BYPASS_EN=1)
self.bus.write_i2c_block_data(self.address, self.REG_INT_PIN_CFG, [0x02])
self.MAG_ACCESS = True
time.sleep(0.1)
#Set the register of the magnetic sensor
def setMagRegister(self, _mode, _bit):
if self.MAG_ACCESS == False:
#Raise an exception because access to the magnetic sensor is not enabled
raise Exception('001 Access to a sensor is invalid.')
_writeData = 0x00
#Measurement mode setting
if _mode=='8Hz': #Continuous measurement mode 1
_writeData = 0x02
self.MAG_MODE = self.MAG_MODE_SERIAL_1
elif _mode=='100Hz': #Continuous measurement mode 2
_writeData = 0x06
self.MAG_MODE = self.MAG_MODE_SERIAL_2
elif _mode=='POWER_DOWN': #Power down mode
_writeData = 0x00
self.MAG_MODE = self.MAG_MODE_POWERDOWN
elif _mode=='EX_TRIGER': #External trigger measurement mode
_writeData = 0x04
self.MAG_MODE = self.MAG_MODE_EX_TRIGER
elif _mode=='SELF_TEST': #Self-test mode
_writeData = 0x08
self.MAG_MODE = self.MAG_MODE_SELF_TEST
else: # _mode='SINGLE' #Single measurement mode
_writeData = 0x01
self.MAG_MODE = self.MAG_MODE_SINGLE
#Number of bits to output
if _bit=='14bit': #14bit output
_writeData = _writeData | 0x00
self.MAG_BIT = 14
else: # _bit='16bit' #16bit output
_writeData = _writeData | 0x10
self.MAG_BIT = 16
self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])
#Sets the acceleration measurement range. In the wide range, the measurement particle size becomes coarse.
# val = 16, 8, 4, 2(default)
def setAccelRange(self, val, _calibration=False):
# ±2g (00), ±4g (01), ±8g (10), ±16g (11)
if val==16 :
self.accelRange = 16
_data = 0x18
elif val==8 :
self.accelRange = 8
_data = 0x10
elif val==4 :
self.accelRange = 4
_data = 0x08
else:
self.accelRange = 2
_data = 0x00
self.bus.write_i2c_block_data(self.address, self.REG_ACCEL_CONFIG1, [_data])
self.accelCoefficient = self.accelRange / float(0x8000)
time.sleep(0.1)
#Reset offset value(To prevent past offset values from being inherited)
self.offsetAccelX = 0
self.offsetAccelY = 0
self.offsetAccelZ = 0
#I think it's better to calibrate, but it takes time.
if _calibration == True:
self.calibAccel(1000)
return
#Set the measurement range of the gyro. In the wide range, the measurement particle size becomes coarse.
# val= 2000, 1000, 500, 250(default)
def setGyroRange(self, val, _calibration=False):
if val==2000:
self.gyroRange = 2000
_data = 0x18
elif val==1000:
self.gyroRange = 1000
_data = 0x10
elif val==500:
self.gyroRange = 500
_data = 0x08
else:
self.gyroRange = 250
_data = 0x00
self.bus.write_i2c_block_data(self.address, self.REG_GYRO_CONFIG, [_data])
self.gyroCoefficient = self.gyroRange / float(0x8000)
time.sleep(0.1)
#Reset offset value(To prevent past offset values from being inherited)
self.offsetGyroX = 0
self.offsetGyroY = 0
self.offsetGyroZ = 0
#It's better to calibrate, but it takes time.
if _calibration == True:
self.calibGyro(1000)
return
#Set the Low Pass Filter of the accelerometer.
# def setAccelLowPassFilter(self,val):
#If you try to use the data from the sensor as it is, it will be treated as unsigned, so it will be converted to signed.(16-bit only)
def u2s(self,unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
#Get the acceleration value
def getAccel(self):
data = self.bus.read_i2c_block_data(self.address, 0x3B ,6)
rawX = self.accelCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetAccelX
rawY = self.accelCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetAccelY
rawZ = self.accelCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetAccelZ
return rawX, rawY, rawZ
#Get the gyro value.
def getGyro(self):
data = self.bus.read_i2c_block_data(self.address, 0x43 ,6)
rawX = self.gyroCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetGyroX
rawY = self.gyroCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetGyroY
rawZ = self.gyroCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetGyroZ
return rawX, rawY, rawZ
def getMag(self):
if self.MAG_ACCESS == False:
#The magnetic sensor is not valid.
raise Exception('002 Access to a sensor is invalid.')
#Pre-processing
if self.MAG_MODE==self.MAG_MODE_SINGLE:
#The single measurement mode becomes Power Down as soon as the measurement ends, so change the mode again.
if self.MAG_BIT==14: #14bit output
_writeData = 0x01
else: #16bit output
_writeData = 0x11
self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])
time.sleep(0.01)
elif self.MAG_MODE==self.MAG_MODE_SERIAL_1 or self.MAG_MODE==self.MAG_MODE_SERIAL_2:
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
if (status[0] & 0x02) == 0x02:
#Sensing again due to data overrun
self.bus.read_i2c_block_data(self.addrAK8963, 0x09 ,1)
elif self.MAG_MODE==self.MAG_MODE_EX_TRIGER:
#Unimplemented
return
elif self.MAG_MODE==self.MAG_MODE_POWERDOWN:
raise Exception('003 Mag sensor power down')
#Check the ST1 register to see if data can be read.
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
while (status[0] & 0x01) != 0x01:
#Wait until data ready
time.sleep(0.01)
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
#Data read
data = self.bus.read_i2c_block_data(self.addrAK8963, 0x03 ,7)
rawX = self.u2s(data[1] << 8 | data[0]) #The lower bit comes first
rawY = self.u2s(data[3] << 8 | data[2]) #The lower bit comes first
rawZ = self.u2s(data[5] << 8 | data[4]) #The lower bit comes first
st2 = data[6]
#Overflow check
if (st2 & 0x08) == 0x08:
#The correct value is not obtained due to overflow
raise Exception('004 Mag sensor over flow')
#Conversion to μT
if self.MAG_BIT==16: #At 16-bit output
rawX = rawX * self.magCoefficient16
rawY = rawY * self.magCoefficient16
rawZ = rawZ * self.magCoefficient16
else: #At the time of 14bit output
rawX = rawX * self.magCoefficient14
rawY = rawY * self.magCoefficient14
rawZ = rawZ * self.magCoefficient14
return rawX, rawY, rawZ
def getTemp(self):
data = self.bus.read_i2c_block_data(self.address, 0x65 ,2)
raw = data[0] << 8 | data[1]
return ((raw - self.offsetRoomTemp) / self.tempSensitivity) + 21
def selfTestMag(self):
print "start mag sensor self test"
self.setMagRegister('SELF_TEST','16bit')
self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x40])
time.sleep(1.0)
data = self.getMag()
print data
self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x00])
self.setMagRegister('POWER_DOWN','16bit')
time.sleep(1.0)
print "end mag sensor self test"
return
#Calibrate the accelerometer
#I think it is necessary to consider latitude, altitude, terrain, etc., but it is simple.
#Premise that gravity is applied correctly in the z-axis direction and acceleration other than gravity is not generated
def calibAccel(self, _count=1000):
print "Accel calibration start"
_sum = [0,0,0]
#Take a sample of real data
for _i in range(_count):
_data = self.getAccel()
_sum[0] += _data[0]
_sum[1] += _data[1]
_sum[2] += _data[2]
#Offset the average value
self.offsetAccelX = -1.0 * _sum[0] / _count
self.offsetAccelY = -1.0 * _sum[1] / _count
self.offsetAccelZ = -1.0 * ((_sum[2] / _count ) - 1.0) #Subtract gravity
#I want to register the offset value in the register, but I do not know the operation, so implementation is suspended
print "Accel calibration complete"
return self.offsetAccelX, self.offsetAccelY, self.offsetAccelZ
#Calibrate the gyro sensor
#Premise that rotation does not occur in each axis
def calibGyro(self, _count=1000):
print "Gyro calibration start"
_sum = [0,0,0]
#Take a sample of real data
for _i in range(_count):
_data = self.getGyro()
_sum[0] += _data[0]
_sum[1] += _data[1]
_sum[2] += _data[2]
#Offset the average value
self.offsetGyroX = -1.0 * _sum[0] / _count
self.offsetGyroY = -1.0 * _sum[1] / _count
self.offsetGyroZ = -1.0 * _sum[2] / _count
#I want to register the offset value in the register, but I do not know the operation, so implementation is suspended
print "Gyro calibration complete"
return self.offsetGyroX, self.offsetGyroY, self.offsetGyroZ
if __name__ == "__main__":
sensor = SL_MPU9250(0x68,1)
sensor.resetRegister()
sensor.powerWakeUp()
sensor.setAccelRange(8,True)
sensor.setGyroRange(1000,True)
sensor.setMagRegister('100Hz','16bit')
# sensor.selfTestMag()
while True:
now = time.time()
acc = sensor.getAccel()
gyr = sensor.getGyro()
mag = sensor.getMag()
print "%+8.7f" % acc[0] + " ",
print "%+8.7f" % acc[1] + " ",
print "%+8.7f" % acc[2] + " ",
print " | ",
print "%+8.7f" % gyr[0] + " ",
print "%+8.7f" % gyr[1] + " ",
print "%+8.7f" % gyr[2] + " ",
print " | ",
print "%+8.7f" % mag[0] + " ",
print "%+8.7f" % mag[1] + " ",
print "%+8.7f" % mag[2]
sleepTime = 0.1 - (time.time() - now)
if sleepTime < 0.0:
continue
time.sleep(sleepTime)
Issues
If you use the above class, you can get the data continuously for the time being. However, in order to realize more accurate sensing, I think it is necessary to utilize the offset attached to the chip and the Low pass filter function. I would like to try it if I have time in the future.
When only acceleration and angular velocity were continuously sensed with a sample source using the above class, 100Hz could be achieved without any problem, but there was a slight delay at 200Hz. I think that it will be possible to perform sensing with a higher frequency by devising the implementation here as well.
You may not care much about using it with raspi, but if you perform sensing with high frequency and high accuracy, power consumption will increase, so it is necessary to select an appropriate mode according to the actual use. I'd like to know which mode consumes how much power .. I wonder if this will take time so ...
If its helpful then im happy.
Recommended Posts