def _readData16bit(self, register_name):
high = wiringpi.wiringPiI2CReadReg16(self.device,
self.registers[register_name])
low = wiringpi.wiringPiI2CReadReg16(self.device,
self.registers[register_name] + 1)
value = ((high << 8) | low)
return value if value > 32786 else value - 65546
def acc_z_read(acc_id):
LSBz = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2C)
MSBz = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2D)
if MSBz & 0x8000:
MSBz = -(0x010000 - MSBz)
acc_z = ((MSBz << 8) | LSBz) >> 4
return acc_z
def acc_y_read(acc_id):
LSBy = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2A)
MSBy = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2B)
if MSBy & 0x8000:
MSBy = -(0x010000 - MSBy)
acc_y = ((MSBy << 8) | LSBy) >> 4
return acc_y
def acc_x_read(acc_id):
LSBx = wiringpi.wiringPiI2CReadReg16(acc_id, 0x28)
MSBx = wiringpi.wiringPiI2CReadReg16(acc_id, 0x29)
if MSBx & 0x8000:
MSBx = -(0x010000 - MSBx)
acc_x = ((MSBx << 8) | LSBx) >> 4
return acc_x
def get_ta_to(bus_fd):
# 数据采集函数
ta = wpi.wiringPiI2CReadReg16(bus_fd, settings.TA) # 读取环境温度
ta = int(ta) * 0.02 - 273.15
log(logging.DEBUG, "环境温度==>%s", str(ta))
to_1 = wpi.wiringPiI2CReadReg16(bus_fd, settings.TOBJ1) # 读取物体温度to1
to_2 = wpi.wiringPiI2CReadReg16(bus_fd, settings.TOBJ2) # 这里到底是什么?好像读出来是0
log(logging.DEBUG, "To1+To2==> %d %d", int(to_1), int(to_2))
return ta, to_1, to_2
def gyro_y_read(gyro_id):
LSBy = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x24)
MSBy = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x25)
if MSBy & 0x8000:
MSBy = -(0x010000 - MSBy)
gyro_y = ((MSBy << 8) | LSBy)
gyro_rate_y = gyro_y * gyro_sens
gyro_y_time = (gyro_rate_y - calibrate_y) * dt / 50
return gyro_y_time
def gyro_x_read(gyro_id):
LSBx = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x22)
MSBx = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x23)
if MSBx & 0x8000:
MSBx = -(0x010000 - MSBx)
gyro_x = ((MSBx << 8) | LSBx)
gyro_rate_x = gyro_x * gyro_sens
gyro_x_time = (gyro_rate_x - calibrate_x) * dt / 50
return gyro_x_time
def gyro_z_read(gyro_id):
LSBz = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x26)
MSBz = wiringpi.wiringPiI2CReadReg16(gyro_id, 0x27)
if MSBz & 0x8000:
MSBz = -(0x010000 - MSBz)
gyro_z = ((MSBz << 8) | LSBz)
gyro_rate_z = gyro_z * gyro_sens
gyro_z_time = (gyro_rate_z - calibrate_z) * dt / 50
return gyro_z_time
def getTemperature(self):
with self.busMux.lock:
self.busMux.setBusMux(self.device_num)
i2cDev = wiringpi.wiringPiI2CSetup(self.device_id)
# Read both the humidity and the temperature, one measurement
reading = wiringpi.wiringPiI2CReadReg16(i2cDev, 0xE3) >> 2
return (175.72 * reading2) / 65536.0 - 46.85
def getHumidity(self):
with self.busMux.lock:
self.busMux.setBusMux(self.device_num)
i2cDev = wiringpi.wiringPiI2CSetup(self.device_id)
# Read both the humidity and the temperature, one measurement
reading1 = wiringpi.wiringPiI2CReadReg16(i2cDev, 0xE5) >> 2
return (125.0 * reading1) / 65536.0 - 6.0
def calibrate():
for v in range(0, 51):
LSBx = wiringpi.wiringPiI2CReadReg16(id, 0x22)
MSBx = wiringpi.wiringPiI2CReadReg16(id, 0x23)
if MSBx & 0x8000:
MSBx = -(0x010000 - MSBx)
x = ((MSBx << 8) | LSBx)
LSBy = wiringpi.wiringPiI2CReadReg16(id, 0x24)
MSBy = wiringpi.wiringPiI2CReadReg16(id, 0x25)
if MSBy & 0x8000:
MSBy = -(0x010000 - MSBy)
y = ((MSBy << 8) | LSBy)
LSBz = wiringpi.wiringPiI2CReadReg16(id, 0x26)
MSBz = wiringpi.wiringPiI2CReadReg16(id, 0x27)
if MSBz & 0x8000:
MSBz = -(0x010000 - MSBz)
z = ((MSBz << 8) | LSBz)
gyro_rate_x = x * gyro_sens
cx[v] = gyro_rate_x
global cx2
cx2 += cx[v]
gyro_rate_y = y * gyro_sens
cy[v] = gyro_rate_y
global cy2
cy2 += cy[v]
gyro_rate_z = z * gyro_sens
cz[v] = gyro_rate_z
global cz2
cz2 += cz[v]
calibrate_x = cx2 / 51
calibrate_y = cy2 / 51
calibrate_z = cz2 / 51
def acc_calibrate(acc_id):
for t in range(0, 51):
LSBx = wiringpi.wiringPiI2CReadReg16(acc_id, 0x28)
MSBx = wiringpi.wiringPiI2CReadReg16(acc_id, 0x29)
acc_x = ((MSBx << 8) | LSBx) >> 4
LSBy = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2A)
MSBy = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2B)
acc_y = ((MSBy << 8) | LSBy) >> 4
LSBz = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2C)
MSBz = wiringpi.wiringPiI2CReadReg16(acc_id, 0x2D)
acc_z = ((MSBz << 8) | LSBz) >> 4
acc_x_angle = (math.atan2(acc_y, acc_z) +
3.141592653589793238546) * 57.29578
acx[t] = acc_x_angle
global acx2
acx2 += acx[t]
acc_y_angle = (math.atan2(acc_x, acc_z) +
3.141592653589793238546) * 57.29578
acy[t] = acc_y_angle
global acy2
acy2 += acy[t]
acc_z_angle = (math.atan2(acc_x, acc_y) +
3.141592653589793238546) * 57.29578
acz[t] = acc_z_angle
global acz2
acz2 += acz[t]
acc_calibrate_x = acx2 / 51
acc_calibrate_y = acy2 / 51
acc_calibrate_z = acz2 / 51
print "errno: ", errno
quit()
x = int(sys.argv[1])
y = int(sys.argv[2])
theta = int(sys.argv[3])
print "Trying to set X to: ", x
result = wiringpi.wiringPiI2CWriteReg16(fd,0x10,x)
if result == -1:
print "Error writing. errno: ", result
else:
print "X set to: ", x
time.sleep(0.5)
result = wiringpi.wiringPiI2CReadReg16(fd, 0x10)
if result == -1:
print "Error writing. errno: ", result
else:
print "read x to be: ", result
print "Trying to set Y to: ", y
time.sleep(0.5)
result = wiringpi.wiringPiI2CWriteReg16(fd,0x12,y)
if result == -1:
print "Error writing. errno: ", result
else:
print "Y set to: ", y
time.sleep(0.5)
result = wiringpi.wiringPiI2CReadReg16(fd, 0x12)