1.测试代码
多设备i2c设备测试，注意设备的频率需要一致
- main.py
# coding=utf-8
import utime as time
import usys
usys.path.append('/data/pyamp')
import ADS1x15
from mpu6050 import mpu6050
from driver import GPIO
mpu=mpu6050()
gpio=GPIO()
gpio.open("gpio31")
adc = ADS1x15.ADS1115()
GAIN = 2/3
print('Reading ADS1x15 and mpu6050 values, press Ctrl-C to quit...')
while True:
print("gpio31 level:",gpio.read())
if gpio.read()==1:
values = [0]*4
for i in range(4):
values[i] = adc.read_adc(i, gain=GAIN)
values[i]=values[i]*0.1875/1000
print('-'*45)
print('| {0:>7} | {1:>7} | {2:>7} | {3:>7} |'.format('IN0','IN1','IN2','IN3'))
print('| {0:>6} | {1:>6} | {2:>6} | {3:>6} |'.format(*values))
print('-'*45)
gpio.write(0)
time.sleep(5)
print("gpio31 level:",gpio.read())
if gpio.read()==0:
print('--------------------------------mpu6050test----------------------------------')
mpu_id=mpu.get_dev_id()
print('mpu6050 chip id is ',hex(ord(mpu_id.decode())))
print('Temperature:',mpu.get_temp())
accel_data = mpu.get_accel_data()
print('accel_data = {},{},{}'.format(accel_data['x'],accel_data['y'],accel_data['z']))
gyro_data = mpu.get_gyro_data()
print('gyro_data = {},{},{}'.format(gyro_data['x'],gyro_data['y'],gyro_data['z']))
angle=mpu.get_imu_angle(accel_data['x'],accel_data['y'],accel_data['z'],gyro_data['x'],gyro_data['y'],gyro_data['z'])
print("Roll={},pitch={},Yaw={}".format(angle['Pitch'],angle['Roll'],angle['Yaw']))
print('-------------------------------------------------------------------------------')
gpio.write(1)
time.sleep(5)
- mpu6050.py
import math
class mpu6050:
# 重力加速度值
GRAVITIY_MS2 = 9.80665
#比例增益支配率收敛到加速度计/磁强计
Kp=100.0
#积分增益支配率的陀螺仪偏见的衔接
Ki= 0.002
#采样周期的一半
halfT= 0.001
#俯仰角 横滚角 航向角
Pitch=0
Roll=0
Yaw=0
#四元数的元素，代表估计方向
q0 = 1
q1 = 0
q2 = 0
q3 = 0
#按比例缩小积分误差
exInt = 0
eyInt = 0
ezInt = 0
# Scale Modifiers
ACCEL_SCALE_MODIFIER_2G = 16384.0
ACCEL_SCALE_MODIFIER_4G = 8192.0
ACCEL_SCALE_MODIFIER_8G = 4096.0
ACCEL_SCALE_MODIFIER_16G = 2048.0
GYRO_SCALE_MODIFIER_250DEG = 131.0
GYRO_SCALE_MODIFIER_500DEG = 65.5
GYRO_SCALE_MODIFIER_1000DEG = 32.8
GYRO_SCALE_MODIFIER_2000DEG = 16.4
# Pre-defined ranges
ACCEL_RANGE_2G = 0x00
ACCEL_RANGE_4G = 0x08
ACCEL_RANGE_8G = 0x10
ACCEL_RANGE_16G = 0x18
GYRO_RANGE_250DEG = 0x00
GYRO_RANGE_500DEG = 0x08
GYRO_RANGE_1000DEG = 0x10
GYRO_RANGE_2000DEG = 0x18
# MPU-6050 Registers
PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C
ACCEL_XOUT0 = 0x3B
ACCEL_YOUT0 = 0x3D
ACCEL_ZOUT0 = 0x3F
TEMP_OUT0 = 0x41
GYRO_XOUT0 = 0x43
GYRO_YOUT0 = 0x45
GYRO_ZOUT0 = 0x47
ACCEL_CONFIG = 0x1C
GYRO_CONFIG = 0x1B
def __init__(self):
from driver import I2C
self.i2c=I2C()
self.i2c.open('mpu6050')
writeBuf=bytearray(2)
writeBuf[0]=self.PWR_MGMT_1
writeBuf[1]=0x00
# Wake up the MPU-6050 since it starts in sleep mode
self.i2c.write(writeBuf,2)
def get_dev_id(self):
readBuf=bytearray([0x75])
self.i2c.read(readBuf,1)
return readBuf
def read_i2c_word(self, register):
# Read the data from the registers
readBuf=bytearray(2)
readBuf[0]=register
readBuf[1]=register+1
self.i2c.read(readBuf,2)
high=readBuf[0]
low=readBuf[1]
value = (high << 8) + low
if (value >= 0x8000):
return -((65535 - value) + 1)
else:
return value
# 得到mpu6050的温度值
def get_temp(self):
readBuf=bytearray(1)
readBuf[0]=self.TEMP_OUT0
self.i2c.read(readBuf,1)
actual_temp=(ord(readBuf.decode())/340)+36.53
return actual_temp
#设置加速度量程
def set_accel_range(self, accel_range):
# First change it to 0x00 to make sure we write the correct value
writeBuf1=bytearray(2)
writeBuf1[0]=self.ACCEL_CONFIG
writeBuf1[1]=0x00
self.i2c.write(writeBuf1,2)
# Write the new range to the ACCEL_CONFIG register
writeBuf2=bytearray(2)
writeBuf2[0]=self.ACCEL_CONFIG
writeBuf2[1]=accel_range
self.i2c.write(writeBuf2,2)
#读取加速度量程
def read_accel_range(self, raw = False):
readBuf=bytearray(1)
readBuf[0]=self.ACCEL_CONFIG
self.i2c.read(readBuf,1)
if raw is True:
return readBuf
elif raw is False:
if readBuf == self.ACCEL_RANGE_2G:
return 2
elif readBuf == self.ACCEL_RANGE_4G:
return 4
elif readBuf == self.ACCEL_RANGE_8G:
return 8
elif readBuf == self.ACCEL_RANGE_16G:
return 16
else:
return -1
#得到加速度数据
def get_accel_data(self, g = False):
x = self.read_i2c_word(self.ACCEL_XOUT0)
y = self.read_i2c_word(self.ACCEL_YOUT0)
z = self.read_i2c_word(self.ACCEL_ZOUT0)
accel_scale_modifier = None
accel_range = self.read_accel_range(True)
if accel_range == self.ACCEL_RANGE_2G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
elif accel_range == self.ACCEL_RANGE_4G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_4G
elif accel_range == self.ACCEL_RANGE_8G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_8G
elif accel_range == self.ACCEL_RANGE_16G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G
else:
#如果不设置量程，会默认使用ACCEL_SCALE_MODIFIER_2G
print("Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G")
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
x = x / accel_scale_modifier
y = y / accel_scale_modifier
z = z / accel_scale_modifier
if g is True:
return {'x': x, 'y': y, 'z': z}
elif g is False:
x = x * self.GRAVITIY_MS2
y = y * self.GRAVITIY_MS2
z = z * self.GRAVITIY_MS2
return {'x': x, 'y': y, 'z': z}
#设置角速度量程
def set_gyro_range(self, gyro_range):
writeBuf1=bytearray(2)
writeBuf1[0]=self.GYRO_CONFIG
writeBuf1[1]=0x00
self.i2c.write(writeBuf1,2)
writeBuf2=bytearray(2)
writeBuf2[0]=self.GYRO_CONFIG
writeBuf2[1]=gyro_range
self.i2c.write(writeBuf2,2)
#读取角速度量程
def read_gyro_range(self, raw = False):
readBuf=bytearray(1)
readBuf[0]=self.GYRO_CONFIG
self.i2c.read(readBuf,1)
if raw is True:
return readBuf
elif raw is False:
if readBuf == self.GYRO_RANGE_250DEG:
return 250
elif readBuf == self.GYRO_RANGE_500DEG:
return 500
elif readBuf == self.GYRO_RANGE_1000DEG:
return 1000
elif readBuf == self.GYRO_RANGE_2000DEG:
return 2000
else:
return -1
#得到角速度数据
def get_gyro_data(self):
x = self.read_i2c_word(self.GYRO_XOUT0)
y = self.read_i2c_word(self.GYRO_YOUT0)
z = self.read_i2c_word(self.GYRO_ZOUT0)
gyro_scale_modifier = None
gyro_range = self.read_gyro_range(True)
if gyro_range == self.GYRO_RANGE_250DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
elif gyro_range == self.GYRO_RANGE_500DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_500DEG
elif gyro_range == self.GYRO_RANGE_1000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_1000DEG
elif gyro_range == self.GYRO_RANGE_2000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_2000DEG
else:
#如果不设置量程，会默认使用GYRO_SCALE_MODIFIER_250DEG
print("Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG")
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
x = x / gyro_scale_modifier
y = y / gyro_scale_modifier
z = z / gyro_scale_modifier
return {'x': x, 'y': y, 'z': z}
#得到 加速度和角速度数据，和温度值
def get_all_data(self):
temp = self.get_temp()
accel = self.get_accel_data()
gyro = self.get_gyro_data()
return [accel, gyro, temp]
def get_imu_angle(self,ax,ay,az,gx,gy,gz):
#测量正常化
norm1 = math.sqrt(ax*ax + ay*ay + az*az);
if norm1!=0:
#单位化
ax = ax / norm1
ay = ay / norm1
az = az / norm1
# 估计方向的重力
vx = 2*(self.q1*self.q3 - self.q0*self.q2)
vy = 2*(self.q0*self.q1 + self.q2*self.q3)
vz = self.q0*self.q0 - self.q1*self.q1 - self.q2*self.q2 + self.q3*self.q3
#错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy)
ey = (az*vx - ax*vz)
ez = (ax*vy - ay*vx)
#积分误差比例积分增益
exInt = self.exInt + ex*self.Ki
eyInt = self.eyInt + ey*self.Ki
ezInt = self.ezInt + ez*self.Ki
# 调整后的陀螺仪测量
gx = gx + self.Kp*ex + exInt
gy = gy + self.Kp*ey + eyInt
gz = gz + self.Kp*ez + ezInt
else:
print("norm1 is 0")
#整合四元数率和正常化
q0 = self.q0 + (-self.q1*gx - self.q2*gy - self.q3*gz)*self.halfT
q1 = self.q1 + (self.q0*gx + self.q2*gz - self.q3*gy)*self.halfT
q2 = self.q2 + (self.q0*gy - self.q1*gz + self.q3*gx)*self.halfT
q3 = self.q3 + (self.q0*gz + self.q1*gy - self.q2*gx)*self.halfT
# 正常化四元
norm2 = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
if norm2!=0:
q0 = q0 / norm2
q1 = q1 / norm2
q2 = q2 / norm2
q3 = q3 / norm2
Pitch = math.asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3
Roll = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3
#Yaw可以注释掉，因为六轴传感器没有磁力计，所以得到的航偏角不准
Yaw = math.atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
return {'Pitch': Pitch, 'Roll': Roll, 'Yaw': Yaw}
else:
print("norm2 is 0")
- ADS1x15.py
# coding=utf-8
import utime as time
'''
# Choose a gain of 1 for reading voltages from 0 to 4.09V.
# Or pick a different gain to change the range of voltages that are read:
# - 2/3 = +/-6.144V
# - 1 = +/-4.096V
# - 2 = +/-2.048V
# - 4 = +/-1.024V
# - 8 = +/-0.512V
# - 16 = +/-0.256V
'''
# Register and other configuration values:
ADS1x15_DEFAULT_ADDRESS = 0x48
ADS1x15_POINTER_CONVERSION = 0x00
ADS1x15_POINTER_CONFIG = 0x01
ADS1x15_POINTER_LOW_THRESHOLD = 0x02
ADS1x15_POINTER_HIGH_THRESHOLD = 0x03
ADS1x15_CONFIG_OS_SINGLE = 0x8000
ADS1x15_CONFIG_MUX_OFFSET = 12
# Maping of gain values to config register values.
ADS1x15_CONFIG_GAIN = {
2/3: 0x0000,
1: 0x0200,
2: 0x0400,
4: 0x0600,
8: 0x0800,
16: 0x0A00
}
ADS1x15_CONFIG_MODE_CONTINUOUS = 0x0000
ADS1x15_CONFIG_MODE_SINGLE = 0x0100
# Mapping of data/sample rate to config register values for ADS1015 (faster).
ADS1015_CONFIG_DR = {
128: 0x0000,
250: 0x0020,
490: 0x0040,
920: 0x0060,
1600: 0x0080,
2400: 0x00A0,
3300: 0x00C0
}
# Mapping of data/sample rate to config register values for ADS1115 (slower).
ADS1115_CONFIG_DR = {
8: 0x0000,
16: 0x0020,
32: 0x0040,
64: 0x0060,
128: 0x0080,
250: 0x00A0,
475: 0x00C0,
860: 0x00E0
}
ADS1x15_CONFIG_COMP_WINDOW = 0x0010
ADS1x15_CONFIG_COMP_ACTIVE_HIGH = 0x0008
ADS1x15_CONFIG_COMP_LATCHING = 0x0004
ADS1x15_CONFIG_COMP_QUE = {
1: 0x0000,
2: 0x0001,
4: 0x0002
}
ADS1x15_CONFIG_COMP_QUE_DISABLE = 0x0003
class ADS1x15(object):
def __init__(self,**Kwargs):
#导入I2c
from driver import I2C
#实例化
self._i2c=I2C()
#打开I2c
self._i2c.open("ADS1115")
def _data_rate_default(self):
#子类需要实现 父类的方法，否则报错
raise NotImplementedError("subclass must implemet _data_rate_default!")
def _data_rate_config(self,data_rate):
#子类需要实现 父类的方法，否则报错
raise NotImplementedError("subclass must implemet _data_rate_default!")
def _conversion_value(self, low, high):
#子类需要实现 父类的方法，否则报错
raise NotImplementedError('Subclass must implement _conversion_value function!')
#mux, gain, data_rate, and mode 要在规定的范围内
def _read(self, mux, gain, data_rate, mode):
# Go out of power-down mode for conversion.
config = ADS1x15_CONFIG_OS_SINGLE
# Specify mux value.
config |= (mux & 0x07) << ADS1x15_CONFIG_MUX_OFFSET
#设置增益
if gain not in ADS1x15_CONFIG_GAIN:
raise ValueError('Gain must be one of: 2/3, 1, 2, 4, 8, 16')
config |= ADS1x15_CONFIG_GAIN[gain]
#设置模式(continuous or single shot)
config |= mode
#得到速率，默认128bps
if data_rate is None:
data_rate = self._data_rate_default()
#设置速率
config |= self._data_rate_config(data_rate)
#disable 比较器模式
config |= ADS1x15_CONFIG_COMP_QUE_DISABLE
#I2C的写函数
writeData=bytearray(3)
writeData[0]=ADS1x15_POINTER_CONFIG
writeData[1]=(config >> 8) & 0xFF
writeData[2]=config & 0xFF
self._i2c.write(writeData,3)
#等待ADC采样（根据采样率加上一个很小的偏置，如0.1ms)
time.sleep(1.0/data_rate+0.0001)
#I2C的读函数
readData=bytearray([ADS1x15_POINTER_CONVERSION,0x00])
self._i2c.read(readData,2)
#return 读取到的数据，包含高八位和低八位
return self._conversion_value(readData[1],readData[0])
def read_adc(self, channel, gain=1, data_rate=None):
#读单个ADC通道，通道值取值范围为[0,3]
assert 0 <= channel <= 3, 'Channel must be a value within 0-3!'
# Perform a single shot read and set the mux value to the channel plus
# the highest bit (bit 3) set.
return self._read(channel + 0x04, gain, data_rate, ADS1x15_CONFIG_MODE_SINGLE)
#继承父类
class ADS1115(ADS1x15):
"""ADS1115 16-bit analog to digital converter instance."""
def __init__(self, *args, **kwargs):
super(ADS1115, self).__init__(*args, **kwargs)
def _data_rate_default(self):
#默认速率为128bps
return 128
def _data_rate_config(self, data_rate):
if data_rate not in ADS1115_CONFIG_DR:
raise ValueError('Data rate must be one of: 8, 16, 32, 64, 128, 250, 475, 860')
return ADS1115_CONFIG_DR[data_rate]
def _conversion_value(self, low, high):
#转换16位数据
value = ((high & 0xFF) << 8) | (low & 0xFF)
if value & 0x8000 != 0:
value -= 1 << 16
return value
- board.json
{
"name": "haas506",
"version": "1.0.0",
"io": {
"ADS1115": {
"type": "I2C",
"port": 1,
"addrWidth": 7,
"freq": 400000,
"mode": "master",
"devAddr": 72
},
"mpu6050": {
"type": "I2C",
"port": 1,
"addrWidth": 7,
"freq": 400000,
"mode": "master",
"devAddr": 104
},
"gpio31":{
"type":"GPIO",
"port": 31,
"dir": "output",
"pull":"pullup"
},
"serial1":{
"type":"UART",
"port":0,
"dataWidth":8,
"baudRate":115200,
"stopBits":1,
"flowControl":"disable",
"parity":"none"
},
"serial2":{
"type":"UART",
"port":1,
"dataWidth":8,
"baudRate":115200,
"stopBits":1,
"flowControl":"disable",
"parity":"none"
},
"serial3":{
"type":"UART",
"port":2,
"dataWidth":8,
"baudRate":115200,
"stopBits":1,
"flowControl":"disable",
"parity":"none"
}
},
"debugLevel": "ERROR"
}
2.测试结果
Reading ADS1x15 and mpu6050 values, press Ctrl-C to quit...
gpio31 level: 0
gpio31 level: 0
--------------------------------mpu6050test----------------------------------
mpu6050 chip id is 0x68
Temperature: 37.22705882352941
Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G
accel_data = -3.088520141601562,3.713406774902344,8.10197841796875
Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG
gyro_data = 4.435114503816794,-1.137404580152672,-2.778625954198473
Roll=3.62792124703243,pitch=5.011731732119345,Yaw=-0.1596178755665964
-------------------------------------------------------------------------------
gpio31 level: 1
---------------------------------------------
| IN0 | IN1 | IN2 | IN3 |
| 0.604688 | 0.596625 | 0.605437 | 0.6045 |
---------------------------------------------
gpio31 level: 0
--------------------------------mpu6050test----------------------------------
mpu6050 chip id is 0x68
Temperature: 37.23000000000001
Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G
accel_data = -9.009380847167968,10.95347259521484,0.2346317626953125
Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG
gyro_data = -217.6641221374046,139.8320610687023,8.664122137404581
Roll=22.67982913322267,pitch=-16.45716503485358,Yaw=-2.329200646882642
-------------------------------------------------------------------------------
gpio31 level: 1
---------------------------------------------
| IN0 | IN1 | IN2 | IN3 |
| -0.0001875 | -0.0001875 | -0.0001875 | -0.0001875 |
---------------------------------------------
gpio31 level: 0
--------------------------------mpu6050test----------------------------------
mpu6050 chip id is 0x68
Temperature: 37.23000000000001
Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G
accel_data = -6.354211206054687,6.775590698242187,0.871489404296875
Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG
gyro_data = 8.183206106870228,4.33587786259542,9.259541984732824
Roll=8.147455054510537,pitch=9.363711465630846,Yaw=1.729427878507938
-------------------------------------------------------------------------------
gpio31 level: 1
---------------------------------------------
| IN0 | IN1 | IN2 | IN3 |
| 3.28556 | -0.0001875 | -0.0001875 | -0.0001875 |
---------------------------------------------
gpio31 level: 0
......
417
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
