class Device(object):
def __init__(self, address, bus):
self._bus = SMBus(bus)
self._address = address
def writeRaw8(self, value):
value = value & 0xff
self._bus.write_byte(self._address, value)
def readRaw8(self):
result = self._bus.read_byte(self._address) & 0xff
return result
def write8(self, register, value):
value = value & 0xff
self._bus.write_byte_data(self._address, register, value)
def readU8(self, register):
result = self._bus.read_byte_data(self._address, register) & 0xFF
return result
def readS8(self, register):
result = self.readU8(register)
if result > 127:
result -= 256
return result
def write16(self, register, value):
value = value & 0xffff
self._bus.write_word_data(self._address, register, value)
def readU16(self, register, little_endian = True):
result = self._bus.read_word_data(self._address,register) & 0xFFFF
if not little_endian:
result = ((result << 8) & 0xFF00) + (result >> 8)
return result
def readS16(self, register, little_endian = True):
result = self.readU16(register, little_endian)
if result > 32767:
result -= 65536
return result
def writeList(self, register, data):
self._bus.write_i2c_block_data(self._address, register, data)
def readList(self, register, length):
results = self._bus.read_i2c_block_data(self._address, register, length)
return results
class Sensor:
ADDR_X = 0x3B
ADDR_Y = 0x3D
ADDR_Z = 0x3F
def __init__(self, bus, address):
self.address = address
self.bus = SMBus(bus)
self.x = 0
self.y = 0
self.z = 0
self.bus.write_byte_data(self.address, DIV, 7)
self.bus.write_byte_data(self.address, PWR_M, 1)
self.bus.write_byte_data(self.address, CONFIG, 0)
self.bus.write_byte_data(self.address, GYRO_CONFIG, 24)
self.bus.write_byte_data(self.address, INT_EN, 1)
def update(self):
self.x = self.readMPU(Sensor.ADDR_X)
self.y = self.readMPU(Sensor.ADDR_Y)
self.z = self.readMPU(Sensor.ADDR_Z)
def readMPU(self, address):
high = self.bus.read_byte_data(self.address, address)
low = self.bus.read_byte_data(self.address, address + 1)
value = ((high << 8) | low)
if value > 32768:
value = value - 65536
return value / 16384.0
class MAX1704x(object):
VERSION_LOW_REG = 0x09
VOLTAGE_HIGH_REG = 0x02
VOLTAGE_LOW_REG = 0x03
SOC_HIGH_REG = 0x04
SOC_LOW_REG = 0x05
CONFIG_HIGH_REG = 0x0C
CONFIG_LOW_REG = 0x0D
CONFIG_ALARM_BIT = 5
CONFIG_SLEEP_BIT = 7
voltage_coeffs = {3: 1.25, 4: 2.5}
def __init__(self, bus=1, addr=0x36):
self.bus_num = bus
self.bus = SMBus(self.bus_num)
self.addr = addr
self.version = self.bus.read_byte_data(self.addr, self.VERSION_LOW_REG)
def get_voltage(self):
high = self.bus.read_byte_data(self.addr, self.VOLTAGE_HIGH_REG)
low = self.bus.read_byte_data(self.addr, self.VOLTAGE_LOW_REG)
result = ((high << 4) + (low >> 4)) * self.voltage_coeffs[self.version]
return result
def get_soc(self):
percents = self.bus.read_byte_data(self.addr, self.SOC_HIGH_REG)
result = "{}%".format(percents)
return result
def alarm_occured(self):
status = self.bus.read_byte_data(self.addr, self.CONFIG_LOW_REG)
return status & 1 << self.CONFIG_ALARM_BIT
def read2byte(i2cBus: SMBus, i2cAddr: int, lowByteAddr: int,
highByteAddr: int) -> int:
data_l = i2cBus.read_byte_data(i2cAddr, lowByteAddr)
data_h = i2cBus.read_byte_data(i2cAddr, highByteAddr)
data = data_l | (data_h << 8)
data = struct.unpack('h', struct.pack('H', data))[0]
return data
def LSM303D_z():
from smbus import SMBus
busNum = 1
b = SMBus(busNum)
LSM = 0x1d
LSM_WHOAMI = 0b1001001 #Device self-id
#Control register addresses -- from LSM303D datasheet
CTRL_0 = 0x1F #General settings
CTRL_1 = 0x20 #Turns on accelerometer and configures data rate
CTRL_2 = 0x21 #Self test accelerometer, anti-aliasing accel filter
CTRL_3 = 0x22 #Interrupts
CTRL_4 = 0x23 #Interrupts
CTRL_5 = 0x24 #Turns on temperature sensor
CTRL_6 = 0x25 #Magnetic resolution selection, data rate config
CTRL_7 = 0x26 #Turns on magnetometer and adjusts mode
#Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
MAG_X_LSB = 0x08 # x
MAG_X_MSB = 0x09
MAG_Y_LSB = 0x0A # y
MAG_Y_MSB = 0x0B
MAG_Z_LSB = 0x0C # z
MAG_Z_MSB = 0x0D
#Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
ACC_X_LSB = 0x28 # x
ACC_X_MSB = 0x29
ACC_Y_LSB = 0x2A # y
ACC_Y_MSB = 0x2B
ACC_Z_LSB = 0x2C # z
ACC_Z_MSB = 0x2D
#Registers holding 12-bit right justified, twos-complemented temperature data -- from LSM303D datasheet
TEMP_MSB = 0x05
TEMP_LSB = 0x06
if b.read_byte_data(LSM, 0x0f) == LSM_WHOAMI:
LSM303D_init = 1
#print('LSM303D detected successfully.')
else:
print('No LSM303D detected on bus ' + str(busNum) + '.')
b.write_byte_data(LSM, CTRL_1,
0b1010111) # enable accelerometer, 50 hz sampling
b.write_byte_data(LSM, CTRL_2, 0x00) #set +/- 2g full scale
b.write_byte_data(
LSM, CTRL_5,
0b01100100) #high resolution mode, thermometer off, 6.25hz ODR
b.write_byte_data(LSM, CTRL_6, 0b00100000) # set +/- 4 gauss full scale
b.write_byte_data(LSM, CTRL_7,
0x00) #get magnetometer out of low power mode
LSM303D_init = 1
# end of initialization
#accx = twos_comp_combine(b.read_byte_data(LSM, ACC_X_MSB), b.read_byte_data(LSM, ACC_X_LSB))
#accy = twos_comp_combine(b.read_byte_data(LSM, ACC_Y_MSB), b.read_byte_data(LSM, ACC_Y_LSB))
accz = twos_comp_combine(b.read_byte_data(LSM, ACC_Z_MSB),
b.read_byte_data(LSM, ACC_Z_LSB))
# print(accz)
return accz
class AK8975:
def __init__(self, i2c_bus, i2c_addr):
self._bus = SMBus(i2c_bus)
self._dev_addr = i2c_addr
self.xCoeff = self._bus.read_byte_data(self._dev_addr, REG_ASAX)
self.yCoeff = self._bus.read_byte_data(self._dev_addr, REG_ASAY)
self.zCoeff = self._bus.read_byte_data(self._dev_addr, REG_ASAZ)
self._bus.write_byte_data(self._dev_addr, REG_CNTL, CNTL_PWRDWN)
self.offsets = [0.] * 3
self.scale = [1.] * 3
self.last_sin = 0.
self.last_cos = 0.0
self.lock_counter = 100
def setOffset(self, offsets):
self.offsets = offsets
def setScale(self, scale):
self.scale = scale
def close(self):
self._bus.close()
def read_gauss(self):
self._bus.write_byte_data(self._dev_addr, REG_CNTL, CNTL_MEASURE)
while (self._bus.read_byte_data(self._dev_addr, REG_ST1)
& ST1_DRDY) == 0:
time.sleep(0.001)
data = self._bus.read_i2c_block_data(self._dev_addr, REG_HXL, 6)
data = bytes(data)
fields = struct.unpack('hhh', data)
x = self.adjustValue(fields[0], self.xCoeff)
y = self.adjustValue(fields[1], self.yCoeff)
z = self.adjustValue(fields[2], self.zCoeff)
return (x, y, z)
def read_heading(self):
x, y, z = self.read_gauss()
x = (x - self.offsets[0]) * self.scale[0]
y = (y - self.offsets[1]) * self.scale[1]
z = (z - self.offsets[2]) * self.scale[2]
#heading = 90+ (math.atan2(y, x) * 180./math.pi)
#heading = heading + 90.0 #robot compass not north aligned
heading = math.atan2(y, x)
self.last_sin = ALPHA * self.last_sin + (1 - ALPHA) * math.sin(heading)
self.last_cos = ALPHA * self.last_cos + (1 - ALPHA) * math.cos(heading)
heading = math.atan2(self.last_sin, self.last_cos) * 180. / math.pi
heading = heading + 180
if self.lock_counter == 0:
return heading
else:
self.lock_counter = self.lock_counter - 1
return None
def adjustValue(self, value, adj):
# apply the proper compensation to value. This equation is taken
# from the AK8975 datasheet, section 8.3.11
return (value * ((((adj - 128.0) * 0.5) / 128.0) + 1.0))
class HumidityI2cSensorAdapterTask(BaseSensorSimTask):
"""
Shell representation of class for student implementation.
"""
def __init__(self):
super(HumidityI2cSensorAdapterTask, self).__init__(
sensorType=SensorData.HUMIDITY_SENSOR_TYPE,
minVal=SensorDataGenerator.LOW_NORMAL_ENV_HUMIDITY,
maxVal=SensorDataGenerator.HI_NORMAL_ENV_HUMIDITY)
self._sensorName = "HumidityI2cSensor"
# init the I2C bus
self.i2cBus = SMBus(1)
# humidity sensor I2c address
self.humidAddr = 0x5F # HTS221
pass
def generateTelemetry(self) -> SensorData:
data = SensorData(sensorType=self._sensorType)
data.setValue(self._readValueFromI2cBus())
data.setName()
self._latestSensorData = data
return self._latestSensorData
pass
def _readValueFromI2cBus(self) -> float:
# read humidity sensor humidity
# H0_T0_OUT
H0_T0_OUT_L = 0x36
H0_T0_OUT_H = 0x37
H0_T0_OUT = read2byte(self.i2cBus, self.humidAddr, H0_T0_OUT_L,
H0_T0_OUT_H)
# H1_T0_OUT
H1_T0_OUT_L = 0x3A
H1_T0_OUT_H = 0x3B
H1_T0_OUT = read2byte(self.i2cBus, self.humidAddr, H1_T0_OUT_L,
H1_T0_OUT_H)
# H0_rH_x2
H0_rH_x2_ADDR = 0x30
H0_rH_x2 = self.i2cBus.read_byte_data(self.humidAddr, H0_rH_x2_ADDR)
H0_rH = H0_rH_x2 / 2
# H1_rH_x2
H1_rH_x2_ADDR = 0x31
H1_rH_x2 = self.i2cBus.read_byte_data(self.humidAddr, H1_rH_x2_ADDR)
H1_rH = H1_rH_x2 / 2
# H_OUT
HUMIDITY_OUT_L = 0x28
HUMIDITY_OUT_H = 0x29
HUMIDITY_OUT = read2byte(self.i2cBus, self.humidAddr, HUMIDITY_OUT_L,
HUMIDITY_OUT_H)
# Linear interpolation
humidity = linearInterpolation(H0_T0_OUT, HUMIDITY_OUT, H1_T0_OUT,
H0_rH, H1_rH)
return humidity
pass
class ImuReader(object):
def __init__(self, bus_num, lsm_addr, lgd_addr):
self.bus_num = bus_num
self.bus = SMBus(bus_num)
self.lsm_addr = lsm_addr
self.lgd_addr = lgd_addr
def check_status(self):
if self.bus.read_byte_data(self.lsm_addr, LSM_WHOAMI_ADDRESS) == LSM_WHOAMI_CONTENTS:
print('LSM303D detected on I2C bus %d.' % self.bus_num)
else:
raise Exception('No LSM303D detected on I2C bus %d.' % self.bus_num)
if self.bus.read_byte_data(self.lgd_addr, LGD_WHOAMI_ADDRESS) == LGD_WHOAMI_CONTENTS:
print('L3GD20H detected successfully on I2C bus %d.' % self.bus_num)
else:
raise Exception('No L3GD20H detected on bus on I2C bus %d.' % self.bus_num)
def configure_for_reading(self):
b = self.bus
b.write_byte_data(self.lsm_addr, LSM_CTRL_1, 0b1010111) # enable accelerometer, 50 hz sampling
b.write_byte_data(self.lsm_addr, LSM_CTRL_2, 0x00) #set +/- 2g full scale
b.write_byte_data(self.lsm_addr, LSM_CTRL_5, 0b01100100) #high resolution mode, thermometer off, 6.25hz ODR
b.write_byte_data(self.lsm_addr, LSM_CTRL_6, 0b00100000) # set +/- 4 gauss full scale
b.write_byte_data(self.lsm_addr, LSM_CTRL_7, 0x00) #get magnetometer out of low power mode
b.write_byte_data(self.lgd_addr, LGD_CTRL_1, 0x0F) #turn on gyro and set to normal mode
def read_lsm_field(self, field_name):
msb_reg, lsb_reg = LSM_FIELDS[field_name]
return twos_comp_combine(self.bus.read_byte_data(self.lsm_addr, msb_reg),
self.bus.read_byte_data(self.lsm_addr, lsb_reg))
def read_mag_field(self):
return (self.read_lsm_field('MAG_X'),
self.read_lsm_field('MAG_Y'),
self.read_lsm_field('MAG_Z'))
def read_acceleration(self):
return (self.read_lsm_field('ACC_X'),
self.read_lsm_field('ACC_Y'),
self.read_lsm_field('ACC_Z'))
def read_lgd_field(self, field_name):
msb_reg, lsb_reg = LGD_FIELDS[field_name]
return twos_comp_combine(self.bus.read_byte_data(self.lgd_addr, msb_reg),
self.bus.read_byte_data(self.lgd_addr, lsb_reg))
def read_gyro(self):
return (self.read_lgd_field('GYRO_X'),
self.read_lgd_field('GYRO_Y'),
self.read_lgd_field('GYRO_Z'))
class TMP100:
""" Class to read the onboard temperatur Sensor"""
_SLAVE_ADDR = 0x49
_CONFIG_REG = 0x01
_TEMP_REG = 0x00
# config register
_CONFIG_REG_OS = 0x01
_CONFIG_REG_RES_9B = 0x00
_CONFIG_REG_RES_12B = 0x03
_CONFIG_REG_TRIG_OS = 0x80
def __init__(self, device_number=1):
""" """
try:
self.bus = SMBus(device_number)
except Exception:
raise i2cError()
configList = [self._CONFIG_REG_OS, self._CONFIG_REG_RES_12B]
self.configTMP100(configList)
def configTMP100(self, list):
""" Write list elements to tmp100#s configuration register"""
reg = (list[1] << 5) + list[0]
# write to config register
self.bus.write_byte_data(self._SLAVE_ADDR, self._CONFIG_REG, reg)
if DEBUG:
# read config register back
tmpReg = self.bus.read_byte_data(self._SLAVE_ADDR,
self._CONFIG_REG)
print(reg, tmpReg)
def getTemperature(self):
""" Get temperature readings """
# read first config register
config = self.bus.read_byte_data(self._SLAVE_ADDR, self._CONFIG_REG)
#trigger single shot
newConfig = config + self._CONFIG_REG_TRIG_OS
# write config register new value back
self.bus.write_byte_data(self._SLAVE_ADDR, self._CONFIG_REG, newConfig)
time.sleep(0.001) # wait a bit
#read temperature register
raw = self.bus.read_i2c_block_data(self._SLAVE_ADDR,
self._TEMP_REG)[:2]
val = ((raw[0] << 8) + raw[1]) >> 4
#TODO: get resolution factor properly :)
return val * 0.0625
class I2cButton:
def __init__(self, address, nums, interval=0.05):
self.data = 0
self.address = address
self.nums = nums
self.signel = {}
self.pressed = {}
self.released = {}
self.busMask = 0
self.bus = SMBus(1)
self.isRunning = False
for i in range(self.nums):
self.signel[i] = Debouncer(self.readSignal(i), interval)
self.busMask = self.busMask | (1 << i)
thread = threading.Thread(target=self.run, args=())
thread.daemon = True
thread.start()
def readSignal(self, pos):
def readData(*arg):
return self.data >> pos & 1
return readData
def update(self):
self.data = self.bus.read_byte_data(self.address, self.busMask)
# print("{0:08b}".format(self.data), "{0:08b}".format(self.busMask))
def whenPressed(self, pos, action):
self.pressed[pos] = AsyncCall(action)
def whenReleased(self, pos, action):
self.released[pos] = AsyncCall(action)
def emitPressed(self, pos):
# print(pos, "pressed")
if (self.isRunning and pos in self.pressed):
args = (False)
self.pressed[pos].run(args)
def emitReleased(self, pos):
# print(pos, "released")
if (self.isRunning and pos in self.released):
args = (False)
self.released[pos].run(args)
def start(self):
self.isRunning = True
def run(self):
while True:
self.update()
for i in range(self.nums):
self.signel[i].update()
if self.signel[i].fell:
self.emitPressed(i)
if self.signel[i].rose:
self.emitReleased(i)
def getPressure():
# I2C Constants
ADDR = 0x60
CTRL_REG1 = 0x26
PT_DATA_CFG = 0x13
bus = SMBus(1)
who_am_i = bus.read_byte_data(ADDR, 0x0C)
print hex(who_am_i)
if who_am_i != 0xc4:
print "Device not active."
exit(1)
# Set oversample rate to 128
setting = bus.read_byte_data(ADDR, CTRL_REG1)
newSetting = setting | 0x38
bus.write_byte_data(ADDR, CTRL_REG1, newSetting)
# Enable event flags
bus.write_byte_data(ADDR, PT_DATA_CFG, 0x07)
# Toggel One Shot
setting = bus.read_byte_data(ADDR, CTRL_REG1)
if (setting & 0x02) == 0:
bus.write_byte_data(ADDR, CTRL_REG1, (setting | 0x02))
status = -1;
while (status & 0x08) == 0:
status = bus.read_byte_data(ADDR,0x00)
time.sleep(.5);
print "Reading sensor data..."
p_data = bus.read_i2c_block_data(ADDR,0x01,3)
t_data = bus.read_i2c_block_data(ADDR,0x04,2)
status = bus.read_byte_data(ADDR,0x00)
print "status: "+bin(status)
p_msb = p_data[0]
p_csb = p_data[1]
p_lsb = p_data[2]
t_msb = t_data[0]
t_lsb = t_data[1]
pressure = (p_msb << 10) | (p_csb << 2) | (p_lsb >> 6)
p_decimal = ((p_lsb & 0x30) >> 4)/4.0
return pressure+p_decimal
def __init__(self, addr):
self.address = addr
bus = SMBus(1)
self.VOUT_MODE = bus.read_byte_data(self.address, 0x20)
bus.close()
voutN = self.VOUT_MODE & 0b00011111
self.VOUT_N = self.twos_comp(voutN, 5)
print("DRQ1250 succesfully connected to PMBus... \n")
class SHT20():
SOFT_RESET_DELAY = 0.02
TEMPERATURE_DELAY = 0.1
HUMIDITY_DELAY = 0.04
RESOLUTION_12BITS = 0b00000000
RESOLUTION_11BITS = 0b10000001
RESOLUTION_10BITS = 0b10000000
RESOLUTION_8BITS = 0b00000001
def __init__(self, bus=3, resolution=RESOLUTION_12BITS):
self.bus = SMBus(bus)
self._resolution = resolution
self._onchip_heater = _DISABLE_ONCHIP_HEATER
self._otp_reload = _DISABLE_OTP_RELOAD
self.bus.write_byte(SHT20_I2C, SHT20_RESET)
time.sleep(self.SOFT_RESET_DELAY)
config = self.bus.read_byte_data(SHT20_I2C, SHT20_READ_USER_REG)
config = ((config & _RESERVED_BITMASK) | self._resolution | self._onchip_heater | self._otp_reload)
#self.bus.write_byte(SHT20_I2C, SHT20_WRITE_USER_REG)
self.bus.write_byte_data(SHT20_I2C, SHT20_WRITE_USER_REG, config)
def humidity(self):
self.bus.write_byte(SHT20_I2C, SHT20_HUMID_HM)
time.sleep(self.HUMIDITY_DELAY)
msb, lsb, crc = self.bus.read_i2c_block_data(SHT20_I2C, SHT20_HUMID_HM, 3)
return -6.0 + 125.0 * (msb * 256.0 + lsb) / 65536.0
def temperature(self):
self.bus.write_byte(SHT20_I2C, SHT20_TEMP_HM)
time.sleep(self.TEMPERATURE_DELAY)
msb, lsb, crc = self.bus.read_i2c_block_data(SHT20_I2C, SHT20_TEMP_HM, 3)
return -46.85 + 175.72 * (msb * 256.0 + lsb) / 65536
def temperature_f(self):
return (self.temperature * 1.8 + 32)
def connected(self):
retval = self.bus.read_byte_data(SHT20_I2C, SHT20_READ_USER_REG)
if retval != 0xFF:
return True
else:
return False
class IMU:
def __init__(self):
# 0 for R-Pi Rev. 1, 1 for Rev. 2
self.bus = SMBus(1)
#initialise the accelerometer
self.writeACC(CTRL_REG1_XM, 0b01100111) #z,y,x axis enabled, continuos update, 100Hz data rate
self.writeACC(CTRL_REG2_XM, 0b00100000) #+/- 16G full scale
def writeACC(self, register,value):
self.bus.write_byte_data(ACC_ADDRESS , register, value)
return -1
def readACCx(self):
acc_l = self.bus.read_byte_data(ACC_ADDRESS, OUT_X_L_A)
acc_h = self.bus.read_byte_data(ACC_ADDRESS, OUT_X_H_A)
acc_combined = (acc_l | acc_h <<8)
return acc_combined if acc_combined < 32768 else acc_combined - 65536
def readACCy(self):
acc_l = self.bus.read_byte_data(ACC_ADDRESS, OUT_Y_L_A)
acc_h = self.bus.read_byte_data(ACC_ADDRESS, OUT_Y_H_A)
acc_combined = (acc_l | acc_h <<8)
return acc_combined if acc_combined < 32768 else acc_combined - 65536
def readACCz(self):
acc_l = self.bus.read_byte_data(ACC_ADDRESS, OUT_Z_L_A)
acc_h = self.bus.read_byte_data(ACC_ADDRESS, OUT_Z_H_A)
acc_combined = (acc_l | acc_h <<8)
return acc_combined if acc_combined < 32768 else acc_combined - 65536
def read_simple_accelerometer(self):
ACCx = self.readACCx()
ACCy = self.readACCy()
ACCz = self.readACCz()
return (ACCx, ACCy, ACCz)
def get_acceleration_norm(self):
x = self.readACCx() * 0.732 / 1000
y = self.readACCy() * 0.732 / 1000
z = self.readACCz() * 0.732 / 1000
return math.sqrt(x*x+y*y+z*z);
class TMP100:
""" Class to read the onboard temperatur Sensor"""
_SLAVE_ADDR = 0x49
_CONFIG_REG = 0x01
_TEMP_REG = 0x00
# config register
_CONFIG_REG_OS = 0x01
_CONFIG_REG_RES_9B = 0x00
_CONFIG_REG_RES_12B = 0x03
_CONFIG_REG_TRIG_OS = 0x80
def __init__(self,device_number = 1):
""" """
try:
self.bus = SMBus(device_number)
except Exception:
raise i2cError()
configList = [self._CONFIG_REG_OS, self._CONFIG_REG_RES_12B]
self.configTMP100(configList)
def configTMP100(self, list):
""" Write list elements to tmp100#s configuration register"""
reg = (list[1] << 5) + list[0]
# write to config register
self.bus.write_byte_data(self._SLAVE_ADDR,self._CONFIG_REG,reg)
if DEBUG:
# read config register back
tmpReg = self.bus.read_byte_data(self._SLAVE_ADDR,self._CONFIG_REG)
print(reg,tmpReg)
def getTemperature(self):
""" Get temperature readings """
# read first config register
config = self.bus.read_byte_data(self._SLAVE_ADDR,self._CONFIG_REG)
#trigger single shot
newConfig = config + self._CONFIG_REG_TRIG_OS
# write config register new value back
self.bus.write_byte_data(self._SLAVE_ADDR,self._CONFIG_REG,newConfig)
time.sleep(0.001) # wait a bit
#read temperature register
raw = self.bus.read_i2c_block_data(self._SLAVE_ADDR,self._TEMP_REG)[:2]
val = ((raw[0] << 8) + raw[1]) >> 4
#TODO: get resolution factor properly :)
return val*0.0625
class Mpu6050:
def __init__(self):
self.bus = SMBus(1)
self.bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)
def __read_word(self, adr):
high = self.bus.read_byte_data(DEV_ADDR, adr)
low = self.bus.read_byte_data(DEV_ADDR, adr + 1)
val = (high << 8) + low
return val
def __read_word_sensor(self, adr):
val = self.__read_word(adr)
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
# 角速度(rad/msec)取得
def __load_gyro_sensor(self):
gyro_x = self.__read_word_sensor(GYRO_XOUT)
gyro_y = self.__read_word_sensor(GYRO_YOUT)
gyro_z = self.__read_word_sensor(GYRO_ZOUT)
return [gyro_x, gyro_y, gyro_z]
def gyro_lsb(self):
gyro_x, gyro_y, gyro_z = self.__load_gyro_sensor()
gyro_x /= 131
gyro_y /= 131
gyro_z /= 131
return [gyro_x, gyro_y, gyro_z]
# 加速度(m/msec)取得
def __load_accelerometer(self):
accel_x = self.__read_word_sensor(ACCEL_XOUT)
accel_y = self.__read_word_sensor(ACCEL_YOUT)
accel_z = self.__read_word_sensor(ACCEL_ZOUT)
return [accel_x, accel_y, accel_z]
def accel_lsb(self):
x_accel, y_accel, z_accel = self.__load_accelerometer()
x_accel /= 16384
y_accel /= 16384
z_accel /= 16384
return [x_accel, y_accel, z_accel]
def _get_regs(self, register, length):
bus = SMBus(I2CBUS)
if length == 1:
resp = bus.read_byte_data(self.i2c_addr, register)
else:
resp = bus.read_i2c_block_data(self.i2c_addr, register, length)
bus.close()
return resp
class MTSMBus(I2CBus):
""" Multi-thread compatible SMBus bus.
This is just a wrapper of SMBus, serializing I/O on the bus for use
in multi-threaded context and adding _i2c_ variants of block transfers.
"""
def __init__(self, bus_id=1, **kwargs):
"""
:param int bus_id: the SMBus id (see Raspberry Pi documentation)
:param kwargs: parameters transmitted to :py:class:`smbus.SMBus` initializer
"""
I2CBus.__init__(self, **kwargs)
self._bus = SMBus(bus_id)
# I/O serialization lock
self._lock = threading.Lock()
def read_byte(self, addr):
with self._lock:
return self._bus.read_byte(addr)
def write_byte(self, addr, data):
with self._lock:
self._bus.write_byte(addr, data)
def read_byte_data(self, addr, reg):
with self._lock:
return self._bus.read_byte_data(addr, reg)
def write_byte_data(self, addr, reg, data):
with self._lock:
self._bus.write_byte_data(addr, reg, data)
def read_word_data(self, addr, reg):
with self._lock:
return self._bus.read_word_data(addr, reg)
def write_word_data(self, addr, reg, data):
with self._lock:
self._bus.write_word_data(addr, reg, data)
def read_block_data(self, addr, reg):
with self._lock:
return self._bus.read_block_data(addr, reg)
def write_block_data(self, addr, reg, data):
with self._lock:
self._bus.write_block_data(addr, reg, data)
def read_i2c_block_data(self, addr, reg, count):
with self._lock:
return self._bus.read_i2c_block_data(addr, reg, count)
def write_i2c_block_data(self, addr, reg, data):
with self._lock:
self._bus.write_i2c_block_data(addr, reg, data)
def DetectCap(i2c_addr, i2c_bus, product_id):
bus = SMBus(i2c_bus)
try:
if bus.read_byte_data(i2c_addr, R_PRODUCT_ID) == product_id:
return True
else:
return False
except IOError:
return False
def DetectCap(i2c_addr, i2c_bus, product_id):
bus = SMBus(i2c_bus)
try:
if bus.read_byte_data(i2c_addr, R_PRODUCT_ID) == product_id:
return True
else:
return False
except IOError:
return False
def GetAlt():
# get the altitude from the MPL3115a2 device
print "GetAlt()"
# device address and register addresses
altAddress = 0x60
ctrlReg1 = 0x26
ptDataCfg = 0x13
# values
oversample128 = 0x38
oneShot = 0x02
altMode = 0x80
bus = SMBus(1)
for i in range(0, 5):
whoAmI = bus.read_byte_data(altAddress, 0x0C)
if whoAmI == 0xC4:
break
elif i == 4:
sys.exit()
else:
time.sleep(0.5)
bus.write_byte_data(altAddress, ptDataCfg, 0x07)
oldSetting = bus.read_byte_data(altAddress, ctrlReg1)
newSetting = oldSetting | oversample128 | oneShot | altMode
bus.write_byte_data(altAddress, ctrlReg1, newSetting)
status = bus.read_byte_data(altAddress, 0x00)
while (status & 0x08) == 0:
status = bus.read_byte_data(altAddress, 0x00)
time.sleep(0.5)
msb, csb, lsb = bus.read_i2c_block_data(altAddress, 0x01, 3)
alt = ((msb << 24) | (csb << 16) | (lsb << 8)) / 65536.0
if alt > (1 << 15):
alt -= 1 << 16
return alt
class PressureI2cSensorAdapterTask(BaseSensorSimTask):
"""
Shell representation of class for student implementation.
"""
def __init__(self):
super(PressureI2cSensorAdapterTask, self).__init__(
sensorType=SensorData.PRESSURE_SENSOR_TYPE,
minVal=SensorDataGenerator.LOW_NORMAL_ENV_PRESSURE,
maxVal=SensorDataGenerator.HI_NORMAL_ENV_PRESSURE)
self._sensorName = "PressureI2cSensor"
# init the I2C bus
self.i2cBus = SMBus(1)
# pressure sensor I2c address
self.pressAddr = 0x5C # LPS25H
pass
def generateTelemetry(self) -> SensorData:
data = SensorData(sensorType=self._sensorType)
data.setValue(self._readValueFromI2cBus())
self._latestSensorData = data
return self._latestSensorData
pass
def _readValueFromI2cBus(self) -> float:
PRESS_OUT_XL = 0x28
PRESS_OUT_L = 0x29
PRESS_OUT_H = 0x2A
press_xl = self.i2cBus.read_byte_data(self.pressAddr, PRESS_OUT_XL)
press_l = self.i2cBus.read_byte_data(self.pressAddr, PRESS_OUT_L)
press_h = self.i2cBus.read_byte_data(self.pressAddr, PRESS_OUT_H)
"""
The full reference pressure value is composed by PRESS_OUT_H/_L/_XL and is represented as 2’s complement.
Pressure Values exceeding the operating pressure Range are clipped.
"""
press = (press_h << 16) | (press_l << 8) | press_xl
press /= 4096
return press
pass
class PressureSensor(object):
def __init__(self):
log.debug("Initialising pressure sensor")
self.bus = SMBus(1)
self.address = 0x60
def read_pressure(self):
# Return dummy data for now
# return 100. + random.random()
response = self.bus.read_byte_data(self.address, 0x02)
print 'data:', response
return response
class TemperatureSensor(object):
def __init__(self):
log.debug("Initialising temperature/humidity sensor")
self.bus = SMBus(1)
self.address = 0x40
self.MEASURE_RELATIVE_TEMPERATURE = 0xE3
self.MEASURE_RELATIVE_HUMIDITY = 0xE5
self.READ_FIRMWARE_VERSION = '\x84\xb8'
def read_firmware_version(self):
self.bus.write_byte(self.address, 0x84)
self.bus.write_byte(self.address, 0xB8)
response = self.bus.read_byte(self.address)
print 'firmware version:', response
# response = self.bus.read_byte_data(self.address,
# 0xB8)
# print 'firmware version:', response
return response
def read_temperature(self):
# Return dummy data for now
# return 20. + random.random()
response = self.bus.read_byte_data(self.address,
self.MEASURE_RELATIVE_TEMPERATURE)
print 'temperature:', response
return response
def read_humidity(self):
# Return dummy data for now
# return random.randint(40, 90)
response = self.bus.read_byte_data(self.address,
self.MEASURE_RELATIVE_HUMIDITY)
print 'humidity:', response
return response
def readMPL3155():
# I2C Constants
ADDR = 0x60
CTRL_REG1 = 0x26
PT_DATA_CFG = 0x13
bus = SMBus(2)
who_am_i = bus.read_byte_data(ADDR, 0x0C)
print hex(who_am_i)
if who_am_i != 0xc4:
tfile = open('temp.txt', 'w')
tfile.write("Barometer funktioniert nicht")
tfile.close()
exit(1)
# Set oversample rate to 128
setting = bus.read_byte_data(ADDR, CTRL_REG1)
#newSetting = setting | 0x38
newSetting = 0x38
bus.write_byte_data(ADDR, CTRL_REG1, newSetting)
# Enable event flags
bus.write_byte_data(ADDR, PT_DATA_CFG, 0x07)
# Toggel One Shot
setting = bus.read_byte_data(ADDR, CTRL_REG1)
if (setting & 0x02) == 0:
bus.write_byte_data(ADDR, CTRL_REG1, (setting | 0x02))
# Read sensor data
print "Waiting for data..."
status = bus.read_byte_data(ADDR,0x00)
#while (status & 0x08) == 0:
while (status & 0x06) == 0:
#print bin(status)
status = bus.read_byte_data(ADDR,0x00)
time.sleep(1)
print "Reading sensor data..."
status = bus.read_byte_data(ADDR,0x00)
p_data = bus.read_i2c_block_data(ADDR,0x01,3)
t_data = bus.read_i2c_block_data(ADDR,0x04,2)
p_msb = p_data[0]
p_csb = p_data[1]
p_lsb = p_data[2]
t_msb = t_data[0]
t_lsb = t_data[1]
pressure = (p_msb << 10) | (p_csb << 2) | (p_lsb >> 6)
p_decimal = ((p_lsb & 0x30) >> 4)/4.0
celsius = t_msb + (t_lsb >> 4)/16.0
tfile = open('temp.txt', 'w')
tfile.write("Luftdruck "+str(pressure/100)+" Hektopascal. ")
tfile.write("Temperatur "+str("{0:.1f}".format(celsius).replace('.',','))+" Grad Celsius")
tfile.close()
class PressureSensor(object):
def __init__(self):
log.debug("Initialising pressure sensor")
self.bus = SMBus(1)
self.address = 0x60
def read_pressure(self):
# Return dummy data for now
# return 100. + random.random()
response = self.bus.read_byte_data(self.address,
0x02)
print 'data:', response
return response
class LocalhostSMBusSlave(SMBusSlave):
def __init__(self, bus, address):
SMBusSlave.__init__(self, bus, address)
self.bus = SMBus(bus)
self.address = address
#return: one byte hex string without '0x': '12'
def get_byte(self, offset):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
ret = self.bus.read_byte_data(self.address, offset)
time.sleep(self.interval)
data = hex(ret)
return data[2:].zfill(2)
#input: offset:str, data:int
def set_byte(self, offset, data):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
self.bus.write_byte_data(self.address, offset, data)
time.sleep(self.interval)
#return: two byte hex string without '0x': '1234'
def get_word(self, offset):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
ret = self.bus.read_word_data(self.address, offset)
data = hex(ret)
time.sleep(self.interval)
return data[2:].zfill(4)
def set_word(self, offset, data):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
if sys.byteorder == "big":
data = ((data & 0xff00) >> 8) + ((data & 0x00ff) << 8)
self.bus.write_word_data(self.address, offset, data)
time.sleep(self.interval)
#return: hex string list per byte,without '0x' :['12', '0a']
def get_block(self, offset, length="32"):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
ret = self.bus.read_block_data(self.address, offset)
time.sleep(self.interval)
hex_ret = [hex(i)[2:].zfill(2) for i in ret]
return hex_ret
def set_block(self, offset, data):
# print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name)
data_arr = [(data & 0x00ff), ((data & 0xff00) >> 8)]
#self.bus.write_i2c_block_data(self.address, offset, data_arr)
self.bus.write_block_data(self.address, offset, data_arr)
time.sleep(self.interval)
class Magnetometer(RPCComponentThreaded):
"""
HMC5883L I2C magnetometer
"""
def main_prepare(self):
self.bus = SMBus(self.Configuration['bus'])
self.address = self.Configuration['address']
self.bus.write_byte_data(self.address, 2, 0)
self.values = [0, 0, 0]
def mainthread(self):
for i, offset in enumerate(range(3, 8, 2)):
msb = self.bus.read_byte_data(self.address, offset)
lsb = self.bus.read_byte_data(self.address, offset + 1)
value = (msb << 8) + lsb
if value >= 32768:
value -= 65536
self.values[i] = value
for subscriber, func in self.subscribers.items():
self.send(Message(sender=self.name, recipient=subscriber, func=func,
arg={'x': self.values[0], 'y': self.values[2], 'z': self.values[1]}),
"outbox")
sleep(0.1)
def callI2C(cmd, val):
b = SMBus(1) # 1 indicates /dev/i2c-1
addr = 0x04
try:
b.write_byte_data(addr, cmd, val)
time.sleep(.01)
try:
if b.read_byte_data(addr, cmd) == val:
return True
else:
return False
except IOError:
print 'IOError: Could not read driver state'
except IOError:
print 'IOError: Could not deliver command to driver.'
def main():
'''
Main program function
'''
# Define registers values from datasheet
IODIRA = 0x00 # IO direction A - 1= input 0 = output
IODIRB = 0x01 # IO direction B - 1= input 0 = output
IPOLA = 0x02 # Input polarity A
IPOLB = 0x03 # Input polarity B
GPINTENA = 0x04 # Interrupt-onchange A
GPINTENB = 0x05 # Interrupt-onchange B
DEFVALA = 0x06 # Default value for port A
DEFVALB = 0x07 # Default value for port B
INTCONA = 0x08 # Interrupt control register for port A
INTCONB = 0x09 # Interrupt control register for port B
IOCON = 0x0A # Configuration register
GPPUA = 0x0C # Pull-up resistors for port A
GPPUB = 0x0D # Pull-up resistors for port B
INTFA = 0x0E # Interrupt condition for port A
INTFB = 0x0F # Interrupt condition for port B
INTCAPA = 0x10 # Interrupt capture for port A
INTCAPB = 0x11 # Interrupt capture for port B
GPIOA = 0x12 # Data port A
GPIOB = 0x13 # Data port B
OLATA = 0x14 # Output latches A
OLATB = 0x15 # Output latches B
i2cbus = SMBus(1) # Create a new I2C bus
i2caddress = 0x20 # Address of MCP23017 device
i2cbus.write_byte_data(i2caddress, IOCON,
0x02) # Update configuration register
i2cbus.write_word_data(
i2caddress, IODIRA,
0xFF00) # Set Port A as outputs and Port B as inputs
while (True):
portb = i2cbus.read_byte_data(i2caddress,
GPIOB) # Read the value of Port B
print(portb) # print the value of Port B
i2cbus.write_byte_data(i2caddress, GPIOA, 0x01) # Set pin 1 to on
time.sleep(0.5) # Wait 500ms
i2cbus.write_byte_data(i2caddress, GPIOA, 0x00) # Set pin 1 to off
time.sleep(0.5) # Wait 500ms
class I2C:
mutex = Lock()
@classmethod
def start(self):
try:
self.bus = SMBus(1)
sleep(0.5)
except Exception as e:
Logger.warn("Could not found i2c bus : " + str(e))
@classmethod
def write_reg(self, ip, register, data):
self.mutex.acquire()
try:
self.bus.write_byte_data(ip, register, data)
except Exception as e:
Logger.error("I2C write reg error : " + str(e))
sleep(0.01) # to make sure all the infos are sent
self.mutex.release()
@classmethod
def write_data(self, ip, data):
self.mutex.acquire()
try:
self.bus.write_i2c_block_data(ip, 0, data)
except Exception as e:
Logger.error("I2C write data error : " + str(e))
self.mutex.release()
return 1
sleep(0.01)
self.mutex.release()
return 0
@classmethod
def read_reg(self, ip, register):
self.mutex.acquire()
try:
data = self.bus.read_byte_data(ip, register)
except Exception as e:
Logger.error("I2C read reg error : " + str(e))
self.mutex.release()
return None
sleep(0.01)
self.mutex.release()
return data
def rotate(value1):
angle = 0
value1 = value1 * 7 / 10
b = SMBus(1)
L3G = 0x6b
who = 0b11010111
Z_MSB = 0x2d
Z_LSB = 0x2c
CTRL_GYRO_1 = 0x20
CTRL_GYRO_2 = 0x21
CTRL_GYRO_6 = 0x39
b.write_byte_data(L3G, CTRL_GYRO_1, 0b00001111)
b.write_byte_data(L3G, CTRL_GYRO_2, 0x00)
b.write_byte_data(L3G, CTRL_GYRO_6, 0b000000)
sens = .00875
value2 = True
while value2:
start = time.time()
z = twos_comp_combine(b.read_byte_data(L3G, Z_MSB),
b.read_byte_data(L3G, Z_LSB))
zdps = z * sens
heading = zdps * time_div(start)
angle += heading
print(angle)
if value1 > 0:
if (abs(angle) >= 360):
angle = 0
if angle < value1:
pwm.lspin()
else:
pwm.stop()
value2 = False
elif value1 < 0:
if (abs(angle) >= 360):
angle = 0
if angle > value1:
pwm.rspin()
else:
pwm.stop()
value2 = False
def get(self):
lStatus = "ok"
lCommand = ""
lValue = ""
lArgs = self.__mParser.parse_args()
lBusId = int(lArgs['bus_id'], 0)
lBus = SMBus(lBusId)
lAddress = int(lArgs['address'], 0)
try:
if lArgs['cmd'] is None:
lValue = lBus.read_byte(lAddress)
else:
lCommand = int(lArgs['cmd'], 0)
lValue = lBus.read_byte_data(lAddress, lCommand)
except IOError, pExc:
lStatus = "Error reading from bus: " + str(pExc)
class SHT20():
SOFT_RESET_DELAY = 0.02
TEMPERATURE_DELAY = 0.1
HUMIDITY_DELAY = 0.04
RESOLUTION_12BITS = 0b00000000
RESOLUTION_11BITS = 0b10000001
RESOLUTION_10BITS = 0b10000000
RESOLUTION_8BITS = 0b00000001
def __init__(self, bus=1, resolution=RESOLUTION_12BITS):
self.bus = SMBus(bus)
self._resolution = resolution
self._onchip_heater = _DISABLE_ONCHIP_HEATER
self._otp_reload = _DISABLE_OTP_RELOAD
self.reset()
def reset(self):
try:
self.bus.write_byte(SHT20_I2C, SHT20_RESET)
time.sleep(self.SOFT_RESET_DELAY)
config = self.bus.read_byte_data(SHT20_I2C, SHT20_READ_USER_REG)
config = ((config & _RESERVED_BITMASK) | self._resolution
| self._onchip_heater | self._otp_reload)
#self.bus.write_byte(SHT20_I2C, SHT20_WRITE_USER_REG)
self.bus.write_byte_data(SHT20_I2C, SHT20_WRITE_USER_REG, config)
except Exception as e:
logging.error("Could not initialize SHT20: " + str(e))
async def humidity(self):
self.reset()
self.bus.write_byte(SHT20_I2C, SHT20_HUMID_HM)
time.sleep(self.HUMIDITY_DELAY)
msb, lsb, crc = self.bus.read_i2c_block_data(SHT20_I2C, SHT20_HUMID_HM,
3)
return -6.0 + 125.0 * (msb * 256.0 + lsb) / 65536.0
async def temperature(self):
self.reset()
self.bus.write_byte(SHT20_I2C, SHT20_TEMP_HM)
time.sleep(self.TEMPERATURE_DELAY)
msb, lsb, crc = self.bus.read_i2c_block_data(SHT20_I2C, SHT20_TEMP_HM,
3)
return -46.85 + 175.72 * (msb * 256.0 + lsb) / 65536
class EPuck:
"""Class for interfacing with a generic e-puck robot."""
def __init__(self,
i2c_bus: Optional[int] = None,
i2c_address: Optional[int] = None):
if i2c_bus is not None:
self._bus = SMBus(i2c_bus)
else:
try:
self._bus = SMBus(_EPUCK_I2C_CHANNEL)
except FileNotFoundError:
self._bus = SMBus(_EPUCK_LEGACY_I2C_CHANNEL)
if i2c_address is not None:
self._i2c_address = i2c_address
else:
self._i2c_address = _EPUCK_I2C_ADDRESS
GPIO.setmode(GPIO.BOARD)
GPIO.setup(_EPUCK_RESET_PIN, GPIO.OUT, initial=GPIO.HIGH)
def __del__(self):
GPIO.cleanup(_EPUCK_RESET_PIN)
def _write_data_8(self, address, data):
self._bus.write_byte_data(self._i2c_address, address, data)
def _write_data_16(self, address, data):
self._bus.write_word_data(self._i2c_address, address, data)
def _read_data_8(self, address):
return self._bus.read_byte_data(self._i2c_address, address)
def _read_data_16(self, address):
return self._bus.read_word_data(self._i2c_address, address)
@staticmethod
def reset_robot():
GPIO.output(_EPUCK_RESET_PIN, GPIO.LOW)
sleep(0.1)
GPIO.output(_EPUCK_RESET_PIN, GPIO.HIGH)
class I2CDevice:
def __init__(self, addr=None, addr_default=None, bus=BUS_NUMBER):
if not addr:
# try autodetect address, else use default if provided
try:
self.addr = int('0x{}'.format(
findall("[0-9a-z]{2}(?!:)", check_output(['/usr/sbin/i2cdetect', '-y', str(BUS_NUMBER)]).decode())[0]), base=16) \
if exists('/usr/sbin/i2cdetect') else addr_default
except:
self.addr = addr_default
else:
self.addr = addr
self.bus = SMBus(bus)
# write a single command
def write_cmd(self, cmd):
self.bus.write_byte(self.addr, cmd)
sleep(0.0001)
# write a command and argument
def write_cmd_arg(self, cmd, data):
self.bus.write_byte_data(self.addr, cmd, data)
sleep(0.0001)
# write a block of data
def write_block_data(self, cmd, data):
self.bus.write_block_data(self.addr, cmd, data)
sleep(0.0001)
# read a single byte
def read(self):
return self.bus.read_byte(self.addr)
# read
def read_data(self, cmd):
return self.bus.read_byte_data(self.addr, cmd)
# read a block of data
def read_block_data(self, cmd):
return self.bus.read_block_data(self.addr, cmd)
def init():
global luxAdd, bus, gain, dataType, intgTime,gainT
global ledOn, exiTime, fileName, startDelay, logo
global sampleNumber, dataTypeT, intgTimeT, ch
bus = SMBus(1) # use bus 1
luxAdd = 0x29 # address of TSL2591
io.setmode(io.BCM); io.setwarnings(False)
io.setup(4,io.OUT) ; io.output(4,0)
# confirm we have a sensor attached
print("Checking for TSL2591 sensor")
sec = bus.read_byte_data(luxAdd, 0xA0 | 0x12) # ID
if sec != 0x50:
print("TSL2591 not found")
else :
print("TSL2591 found")
gain = 2 ; intgTime = 1 ;setGain() ;ledOn = True
gainT = ["low gain","medium gain","high gain","maximum gain"]
intgTimeT = ["100 mS","200 mS","300 mS","400 mS","500 mS","600 mS"]
dataTypeT = ["IR","Visible","Full spectrum"]
exiTime = 2 ; dataType = 2 ; startDelay = 1
ch = [0,0,0]; sampleNumber = 0
fileName = "/home/pi/my_data.csv"
logo = pygame.image.load("images/Logo.png").convert_alpha()
class MB85RC04:
def __init__(self, I2C_bus_number = 1, address = 0x50):
self.bus = SMBus(I2C_bus_number)
self.address = address
def readByte(self, registerAddress):
if(registerAddress > 255):
self.address = self.address | 1
registerAddress = registerAddress - 256
else:
self.address = self.address & 0xFE
return self.bus.read_byte_data(self.address, registerAddress)
def writeByte(self, registerAddress, data):
if(registerAddress > 255):
self.address = self.address | 1
registerAddress = registerAddress - 256
else:
self.address = self.address & 0xFE
self.bus.write_byte_data(self.address, registerAddress, data)
def readBytes(self, registerAddress):
if(registerAddress > 255):
self.address = self.address | 1
registerAddress = registerAddress - 256
else:
self.address = self.address & 0xFE
return self.bus.read_i2c_block_data(self.address, registerAddress)
def writeBytes(self, registerAddress, data):
if(registerAddress > 255):
self.address = self.address | 1
registerAddress = registerAddress - 256
else:
self.address = self.address & 0xFE
self.bus.write_i2c_block_data(self.address, registerAddress, data)
class i2c_interface:
def __init__(self, address=0x3c):
"""
:param address: i2c address of ssd1306
"""
self.bus = SMBus(self.bus_id())
self.address = address
def __del__(self):
self.close_i2c()
def close_i2c(self):
self.bus.close()
def bus_id(self):
"""
:return: Returns SMBUS id of Raspberry Pi
"""
revision = [
lines[12:-1] for lines in open('/proc/cpuinfo', 'r').readlines()
if "Revision" in lines[:8]
]
revision = (revision + ['0000'])[0]
return 1 if int(revision, 16) >= 4 else 0
def i2c_read(self, register=0):
data = self.bus.read_byte_data(self.address, register)
return data
def i2c_write(self, register=DISPLAY_START_LINE, data=0):
# Write a byte to address, register
self.bus.write_byte_data(self.address, register, data)
def i2c_write_block(self, register=DISPLAY_START_LINE, data=None):
if data is None:
data = [40]
self.bus.write_i2c_block_data(self.address, register, data)
def on_message(client, userdata, msg):
msg.topic = msg.topic.replace('/OUT/I2C/', '')
print(msg.topic+" "+str(msg.payload))
payload = json.loads(msg.payload)
# print payload
if msg.topic == 'setbit':
pass
try:
if payload['bus'] not in [0,1,2]:
raise ValueError
if payload['bit'] not in range(8):
raise ValueError
if payload['register'] not in range(256):
raise ValueError
if payload['device'] not in range(256):
raise ValueError
if payload['value'] not in [0,1]:
raise ValueError
bus = SMBus(payload['bus'])
current_reg_value = bus.read_byte_data(payload['device'],
payload['register'])
if payload['value'] == 1:
new_reg_value = current_reg_value | (1 << payload['bit'])
if payload['value'] == 0:
new_reg_value = current_reg_value & ~(1 << payload['bit'])
bus.write_byte_data(payload['device'],
payload['register'],
new_reg_value)
except IOError:
print "Device IO error"
except KeyError:
print "Some of required parameters are not present"
except ValueError:
print "Some of required parameters are not valid"
def detect_device():
for busNum in range(20):
if not os.path.exists('/dev/i2c-' + str(busNum)):
if busNum == 20 - 1:
print("device not found.")
return -1
continue
bus = SMBus(busNum)
print("Bus #{} open. Searching...".format(busNum))
response = bus.read_byte(addr)
if response >= 0:
#print("On bus {} a device {} found with response of {}".format(busNum, hex(dev_addr), hex(response)))
if bus.read_byte_data(addr, 0x00) == 2:
M = bus.read_byte(addr)
m = bus.read_byte(addr)
print(
"Bus:{} Addr:{} Reg:0x00 query returns {}.{}.\nPossible AsRock AURA device with FW nu50_{}.{} detected."
.format(busNum, hex(addr), M, m, M, m))
if M != 1 and m != 10:
print(
"This FW is NOT supported, yet. Try with your own risk!"
)
return bus
bus.close()
class Cap1xxx():
supported = [PID_CAP1208, PID_CAP1188, PID_CAP1166]
number_of_inputs = 8
number_of_leds = 8
def __init__(self, i2c_addr=DEFAULT_ADDR, i2c_bus=1, alert_pin=-1, reset_pin=-1, on_touch=None, skip_init=False):
if on_touch == None:
on_touch = [None] * self.number_of_inputs
self.async_poll = None
self.i2c_addr = i2c_addr
self.i2c = SMBus(i2c_bus)
self.alert_pin = alert_pin
self.reset_pin = reset_pin
self._delta = 50
GPIO.setmode(GPIO.BCM)
if not self.alert_pin == -1:
GPIO.setup(self.alert_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
if not self.reset_pin == -1:
GPIO.setup(self.reset_pin, GPIO.OUT)
GPIO.setup(self.reset_pin, GPIO.LOW)
GPIO.output(self.reset_pin, GPIO.HIGH)
time.sleep(0.01)
GPIO.output(self.reset_pin, GPIO.LOW)
self.handlers = {
'press' : [None] * self.number_of_inputs,
'release' : [None] * self.number_of_inputs,
'held' : [None] * self.number_of_inputs
}
self.touch_handlers = on_touch
self.last_input_status = [False] * self.number_of_inputs
self.input_status = ['none'] * self.number_of_inputs
self.input_delta = [0] * self.number_of_inputs
self.input_pressed = [False] * self.number_of_inputs
self.repeat_enabled = 0b00000000
self.release_enabled = 0b11111111
self.product_id = self._get_product_id()
if not self.product_id in self.supported:
raise Exception("Product ID {} not supported!".format(self.product_id))
if skip_init:
return
# Enable all inputs with interrupt by default
self.enable_inputs(0b11111111)
self.enable_interrupts(0b11111111)
# Disable repeat for all channels, but give
# it sane defaults anyway
self.enable_repeat(0b00000000)
self.enable_multitouch(True)
self.set_hold_delay(210)
self.set_repeat_rate(210)
# Tested sane defaults for various configurations
self._write_byte(R_SAMPLING_CONFIG, 0b00001000) # 1sample per measure, 1.28ms time, 35ms cycle
self._write_byte(R_SENSITIVITY, 0b01100000) # 2x sensitivity
self._write_byte(R_GENERAL_CONFIG, 0b00111000)
self._write_byte(R_CONFIGURATION2, 0b01100000)
self.set_touch_delta(10)
atexit.register(self.stop_watching)
def get_input_status(self):
"""Get the status of all inputs.
Returns an array of 8 boolean values indicating
whether an input has been triggered since the
interrupt flag was last cleared."""
touched = self._read_byte(R_INPUT_STATUS)
threshold = self._read_block(R_INPUT_1_THRESH, self.number_of_inputs)
delta = self._read_block(R_INPUT_1_DELTA, self.number_of_inputs)
#status = ['none'] * 8
for x in range(self.number_of_inputs):
if (1 << x) & touched:
status = 'none'
_delta = self._get_twos_comp(delta[x])
#threshold = self._read_byte(R_INPUT_1_THRESH + x)
# We only ever want to detect PRESS events
# If repeat is disabled, and release detect is enabled
if _delta >= threshold[x]: # self._delta:
self.input_delta[x] = _delta
# Touch down event
if self.input_status[x] in ['press','held']:
if self.repeat_enabled & (1 << x):
status = 'held'
if self.input_status[x] in ['none','release']:
if self.input_pressed[x]:
status = 'none'
else:
status = 'press'
else:
# Touch release event
if self.release_enabled & (1 << x) and not self.input_status[x] == 'release':
status = 'release'
else:
status = 'none'
self.input_status[x] = status
self.input_pressed[x] = status in ['press','held','none']
else:
self.input_status[x] = 'none'
self.input_pressed[x] = False
return self.input_status
def _get_twos_comp(self,val):
if ( val & (1<< (8 - 1))) != 0:
val = val - (1 << 8)
return val
def clear_interrupt(self):
"""Clear the interrupt flag, bit 0, of the
main control register"""
main = self._read_byte(R_MAIN_CONTROL)
main &= ~0b00000001
self._write_byte(R_MAIN_CONTROL, main)
def _interrupt_status(self):
if self.alert_pin == -1:
return self._read_byte(R_MAIN_CONTROL) & 1
else:
return not GPIO.input(self.alert_pin)
def wait_for_interrupt(self, timeout=100):
"""Wait for, interrupt, bit 0 of the main
control register to be set, indicating an
input has been triggered."""
start = self._millis()
while True:
status = self._interrupt_status() # self._read_byte(R_MAIN_CONTROL)
if status:
return True
if self._millis() > start + timeout:
return False
time.sleep(0.005)
def on(self, channel=0, event='press', handler=None):
self.handlers[event][channel] = handler
self.start_watching()
return True
def start_watching(self):
if not self.alert_pin == -1:
try:
GPIO.add_event_detect(self.alert_pin, GPIO.FALLING, callback=self._handle_alert, bouncetime=1)
self.clear_interrupt()
except:
pass
return True
if self.async_poll == None:
self.async_poll = AsyncWorker(self._poll)
self.async_poll.start()
return True
return False
def stop_watching(self):
if not self.alert_pin == -1:
GPIO.remove_event_detect(self.alert_pin)
if not self.async_poll == None:
self.async_poll.stop()
self.async_poll = None
return True
return False
def set_touch_delta(self, delta):
self._delta = delta
def auto_recalibrate(self, value):
self._change_bit(R_GENERAL_CONFIG, 3, value)
def filter_analog_noise(self, value):
self._change_bit(R_GENERAL_CONFIG, 4, not value)
def filter_digital_noise(self, value):
self._change_bit(R_GENERAL_CONFIG, 5, not value)
def set_hold_delay(self, ms):
"""Set time before a press and hold is detected,
Clamps to multiples of 35 from 35 to 560"""
repeat_rate = self._calc_touch_rate(ms)
input_config = self._read_byte(R_INPUT_CONFIG2)
input_config = (input_config & ~0b1111) | repeat_rate
self._write_byte(R_INPUT_CONFIG2, input_config)
def set_repeat_rate(self, ms):
"""Set repeat rate in milliseconds,
Clamps to multiples of 35 from 35 to 560"""
repeat_rate = self._calc_touch_rate(ms)
input_config = self._read_byte(R_INPUT_CONFIG)
input_config = (input_config & ~0b1111) | repeat_rate
self._write_byte(R_INPUT_CONFIG, input_config)
def _calc_touch_rate(self, ms):
ms = min(max(ms,0),560)
scale = int((round(ms / 35.0) * 35) - 35) / 35
return int(scale)
def _handle_alert(self, pin=-1):
inputs = self.get_input_status()
self.clear_interrupt()
for x in range(self.number_of_inputs):
self._trigger_handler(x, inputs[x])
def _poll(self):
"""Single polling pass, should be called in
a loop, preferably threaded."""
if self.wait_for_interrupt():
self._handle_alert()
def _trigger_handler(self, channel, event):
if event == 'none':
return
if callable(self.handlers[event][channel]):
try:
self.handlers[event][channel](CapTouchEvent(channel, event, self.input_delta[channel]))
except TypeError:
self.handlers[event][channel](channel, event)
def _get_product_id(self):
return self._read_byte(R_PRODUCT_ID)
def enable_multitouch(self, en=True):
"""Toggles multi-touch by toggling the multi-touch
block bit in the config register"""
ret_mt = self._read_byte(R_MTOUCH_CONFIG)
if en:
self._write_byte(R_MTOUCH_CONFIG, ret_mt & ~0x80)
else:
self._write_byte(R_MTOUCH_CONFIG, ret_mt | 0x80 )
def enable_repeat(self, inputs):
self.repeat_enabled = inputs
self._write_byte(R_REPEAT_EN, inputs)
def enable_interrupts(self, inputs):
self._write_byte(R_INTERRUPT_EN, inputs)
def enable_inputs(self, inputs):
self._write_byte(R_INPUT_ENABLE, inputs)
def _write_byte(self, register, value):
self.i2c.write_byte_data(self.i2c_addr, register, value)
def _read_byte(self, register):
return self.i2c.read_byte_data(self.i2c_addr, register)
def _read_block(self, register, length):
return self.i2c.read_i2c_block_data(self.i2c_addr, register, length)
def _millis(self):
return int(round(time.time() * 1000))
def _set_bit(self, register, bit):
self._write_byte( register, self._read_byte(register) | (1 << bit) )
def _clear_bit(self, register, bit):
self._write_byte( register, self._read_byte(register) & ~(1 << bit ) )
def _change_bit(self, register, bit, state):
if state:
self._set_bit(register, bit)
else:
self._clear_bit(register, bit)
def _change_bits(self, register, offset, size, bits):
original_value = self._read_byte(register)
for x in range(size):
original_value &= ~(1 << (offset+x))
original_value |= (bits << offset)
self._write_byte(register, original_value)
def __del__(self):
self.stop_watching()
def get_heading(self):
from smbus import SMBus
busNum = 1
b = SMBus(busNum)
LSM = 0x1d
LSM_WHOAMI_ADDRESS = 0x0F
LSM_WHOAMI_CONTENTS = 0b1001001
LSM_CTRL_0 = 0x1F
LSM_CTRL_1 = 0x20
LSM_CTRL_2 = 0x21
LSM_CTRL_3 = 0x22
LSM_CTRL_4 = 0x23
LSM_CTRL_5 = 0x24
LSM_CTRL_6 = 0x25
LSM_CTRL_7 = 0x26
LSM_MAG_X_LSB = 0x08
LSM_MAG_X_MSB = 0x09
LSM_MAG_Y_LSB = 0x0A
LSM_MAG_Y_MSB = 0x0B
LSM_MAG_Z_LSB = 0x0C
LSM_MAG_Z_MSB = 0x0D
if (b.read_byte_data(LSM, LSM_WHOAMI_ADDRESS) == LSM_WHOAMI_CONTENTS):
print ("LSM303D detected successfully on I2C bus ")
else:
print ("No LSM303D detected on I2C Bus ")
b.write_byte_data(LSM, LSM_CTRL_1, 0b1010111)
b.write_byte_data(LSM, LSM_CTRL_2, 0x00)
b.write_byte_data(LSM, LSM_CTRL_5, 0b01100100)
b.write_byte_data(LSM, LSM_CTRL_6, 0b00100000)
b.write_byte_data(LSM, LSM_CTRL_7, 0x00)
count = 0;
total = 0;
magXmax = 549
magYmax = 1514
magZmax = 0
magXmin = -2455
magYmin = -1583
magZmin = -720
time.sleep(0.5)
magx = self.twos_comp_combine(b.read_byte_data(LSM, LSM_MAG_X_MSB), b.read_byte_data(LSM, LSM_MAG_X_LSB))
magy = self.twos_comp_combine(b.read_byte_data(LSM, LSM_MAG_Y_MSB), b.read_byte_data(LSM, LSM_MAG_Y_LSB))
magz = self.twos_comp_combine(b.read_byte_data(LSM, LSM_MAG_Z_MSB), b.read_byte_data(LSM, LSM_MAG_Z_LSB))
magdata = (magx, -magy, magz)
#calculating max and min
scaledMagX = (magx-magXmin)/(magXmax-magXmin)*2-1
scaledMagY = (magy-magYmin)/(magYmax-magYmin)*2-1
scaledMagZ = (magz-magZmin)/(magZmax-magZmin)*2-1
heading = 180* math.atan2(scaledMagX, scaledMagY)/3.14159265359
#heading = 180* math.atan2(magdata[1], magdata[0])/3.14159265359
if (heading < 0):
heading = heading + 360
#print "Heading ", heading
return heading
import serial
import struct
from smbus import SMBus
from time import sleep
from icsp import *
sel = sys.argv[1] if len(sys.argv) > 1 else "N"
i2c0 = SMBus(0)
i2c2 = SMBus(2)
if sel == "A": # toggle A_!RST
print("selecting RFW [bus A] ...")
i2c2.write_byte(0x70, 0x5) # steer mux
ioa = i2c0.read_byte_data(0x23, 0x14)
i2c0.write_byte_data(0x23, 0x14, ioa&~0x10)
i2c0.write_byte_data(0x23, 0x14, ioa|0x10)
elif sel == "B": # toggle B_!RST
print("selecting RFE [bus B] ...")
i2c2.write_byte(0x70, 0x4) # steer mux
iob = i2c0.read_byte_data(0x22, 0x14)
i2c0.write_byte_data(0x22, 0x14, iob&~0x10)
i2c0.write_byte_data(0x22, 0x14, iob|0x10)
elif sel == "N":
print("disabling MUX ...")
i2c2.write_byte(0x70, 0x0) # disable mux
class SRF02:
def __init__(self):
self._i2c = SMBus(1)
self._i2c_address = SRF02_I2C_ADDRESS
self._waiting_for_echo = False
yesterday = datetime.now() - timedelta(1)
self._time_last_burst = yesterday
# The last distance measurement
self.distance = None # meters
# On power up, the detection threshold is set to 28cm (11")
self.mindistance = 0.28 # meters
# This is mostly for debugging and testing
self.num_bursts_sent = 0
# check that the sensor is present - read the version
self.version = self._read_version()
log.info("SRF-02 Ultrasonic Sensor initialized. Version: %d" % self.version)
# we want to check how often we get exceptions
self.error_counter = 0
# Should be called in some sensor loop, maybe at 100Hz. Is fast.
# Will trigger an ultrasonic burst or check if we received an echo.
# I we have a measurement, it is returned in meters.
def update(self):
distance = None
now = datetime.now()
time_since_last_burst = (now - self._time_last_burst).total_seconds()
# log.debug("time since last burst: {}".format(time_since_last_burst))
if self._waiting_for_echo:
# make sure we wait at least some amount of time before we read
if time_since_last_burst > SRF02_MIN_TIME_BETWEEN_BURST_READ:
# check if we have an echo
distance = self._read_echo()
# Fallback if we don't get an echo, just stop waiting
# from the data sheet:
# The SRF02 will always be ready 70mS after initiating the ranging.
if distance is None and time_since_last_burst > SRF02_MAX_WAIT_TIME:
log.warn("Fallback! Waited longer than 70ms!")
self._waiting_for_echo = False
if (not self._waiting_for_echo) and time_since_last_burst > SRF02_MIN_TIME_BETWEEN_BURSTS:
self._send_burst()
expected_error = self._calc_expected_error(distance)
return distance, expected_error
def _send_burst(self):
self._i2c.write_byte_data(self._i2c_address, 0, 0x51)
self._waiting_for_echo = True
self._time_last_burst = datetime.now()
self.num_bursts_sent += 1
log.debug("Burst sent.")
def _read_echo(self):
# it must be possible to read all of these data in 1 i2c transaction
# buf[0] software version. If this is 255, then the ping has not yet returned
# buf[1] unused
# buf[2] high byte range
# buf[3] low byte range
# buf[4] high byte minimum auto tuned range
# buf[5] low byte minimum auto tuned range
# We use the version information to detect if the result is there yet.
# 255 is a dummy version for the case that no echo has been received yet. For me, the real version is "6".
if self._read_version() == 255:
log.debug("Version is 255")
return None
self.distance = self._i2c.read_word_data(self._i2c_address, 2) / 255 / 100
self.mindistance = self._i2c.read_word_data(self._i2c_address, 4) / 255 / 100
# A value of 0 indicates that no objects were detected. We prefer None to represent this.
if self.distance == 0:
self.distance = None
self._waiting_for_echo = False
log.debug("echo received! distance is: {}".format(self.distance))
return self.distance
# The version can be read from register 0.
# Reading it has no real value for us, but we can use it to determine if a measurement is finished or not.
def _read_version(self):
try:
return self._i2c.read_byte_data(self._i2c_address, 0)
# 255 means that the unit is still measuring the distance
except IOError:
log.error("Recovering from IOError")
self.error_counter += 1
return 255
# find out what kind of error we expect (used in sensor fusion)
def _calc_expected_error(self, distance):
# no reading at all
if distance is None:
return SENSOR_ERROR_MAX
# object too close
if distance <= self.mindistance:
return SRF02_SENSOR_ERROR_LOW_RANGE
# good distance, nice measurement
elif distance <= SRF02_MAX_RANGE:
return SRF02_SENSOR_ERROR_GOOD_RANGE
# object too far
else:
return SRF02_SENSOR_ERROR_HIGH_RANGE
class I2C(object):
""" Class to set up and access I2C devices.
"""
##
## Class methods
##
## Private methods
def __init__(self, busId = 1):
""" Initialize the I2C bus. """
self._i2c = SMBus(busId)
def __del__(self):
""" Clean up routines. """
try:
# Remove SMBus connection
del(self._i2c)
except:
pass
def _combineLoHi(self, loByte, hiByte):
""" Combine low and high bytes to an unsigned 16 bit value. """
return (loByte | hiByte << 8)
def _combineSignedLoHi(self, loByte, hiByte):
""" Combine low and high bytes to a signed 16 bit value. """
combined = self._combineLoHi (loByte, hiByte)
return combined if combined < 32768 else (combined - 65536)
def _combineXLoLoHi(self, xloByte, loByte, hiByte):
""" Combine extra low, low, and high bytes to an unsigned 24 bit
value.
"""
return (xloByte | loByte << 8 | hiByte << 16)
def _combineSignedXLoLoHi(self, xloByte, loByte, hiByte):
""" Combine extra low, low, and high bytes to a signed 24 bit
value.
"""
combined = self._combineXLoLoHi(xloByte, loByte, hiByte)
return combined if combined < 8388608 else (combined - 16777216)
def _getSensorRawLoHi1(self, address, outRegs):
""" Return a scalar representing the combined raw signed 16 bit
value of the output registers of a one-dimensional sensor,
e.g. temperature.
'address' is the I2C slave address.
'outRegs' is a list of the output registers to read.
"""
# Read register outputs and combine low and high byte values
xl = self._readRegister(address, outRegs[0])
xh = self._readRegister(address, outRegs[1])
xVal = self._combineSignedLoHi(xl, xh)
# Return the scalar
return xVal
def _getSensorRawXLoLoHi1(self, address, outRegs):
""" Return a scalar representing the combined raw signed 24 bit
value of the output registers of a one-dimensional sensor,
e.g. temperature.
'address' is the I2C slave address.
'outRegs' is a list of the output registers to read.
"""
# Read register outputs and combine low and high byte values
xxl = self._readRegister(address, outRegs[0])
xl = self._readRegister(address, outRegs[1])
xh = self._readRegister(address, outRegs[2])
xVal = self._combineSignedXLoLoHi(xxl, xl, xh)
# Return the scalar
return xVal
def _getSensorRawLoHi3(self, address, outRegs):
""" Return a vector (i.e. list) representing the combined
raw signed 16 bit values of the output registers of a
3-dimensional (IMU) sensor.
'address' is the I2C slave address.
'outRegs' is a list of the output registers to read.
"""
# Read register outputs and combine low and high byte values
xl = self._readRegister(address, outRegs[0])
xh = self._readRegister(address, outRegs[1])
yl = self._readRegister(address, outRegs[2])
yh = self._readRegister(address, outRegs[3])
zl = self._readRegister(address, outRegs[4])
zh = self._readRegister(address, outRegs[5])
xVal = self._combineSignedLoHi(xl, xh)
yVal = self._combineSignedLoHi(yl, yh)
zVal = self._combineSignedLoHi(zl, zh)
# Return the vector
return [xVal, yVal, zVal]
def _readRegister(self, address, register):
""" Read a single I2C register. """
return self._i2c.read_byte_data(address, register)
def _readRegisters(self, address, register, count):
""" Read (up to 32) 'count' consecutive I2C registers. """
return self._i2c.read_i2c_block_data(address, register, count)
def _read(self, address):
""" Read a single byte from the I2C device without specifying a
register.
"""
return self._i2c.read_byte(address)
def _writeRegister(self, address, register, value):
""" Write a single byte to a I2C register. Return the value the
register had before the write.
"""
valueOld = self._readRegister(address, register)
self._i2c.write_byte_data(address, register, value)
return valueOld
def _write(self, address, value):
""" Write a single byte to the I2C device without specifying a
register.
"""
return self._i2c.write_byte(address, value)
def _testRegister(self, address, register):
""" Check, if a I2C register is readable/accessible. """
try:
return self._readRegister(address, register)
except:
return -1
class L3GD20(object):
def __init__(self, busId, slaveAddr, ifLog, ifWriteBlock):
self.__i2c = SMBus(busId)
self.__slave = slaveAddr
self.__ifWriteBlock = ifWriteBlock
self.__ifLog = ifLog
self.__x0 = 0
def __del__(self):
del(self.__i2c)
def __log(self, register, mask, current, new):
register = '0b' + bin(register)[2:].zfill(8)
mask = '0b' + bin(mask)[2:].zfill(8)
current = '0b' + bin(current)[2:].zfill(8)
new = '0b' + bin(new)[2:].zfill(8)
print('Change in register:' + register + ' mask:' + mask + ' from:' + current + ' to:' + new)
def __writeToRegister(self, register, mask, value):
current = self.__i2c.read_byte_data(self.__slave, register) # Get current value
new = bitOps.SetValueUnderMask(value, current, mask)
if self.__ifLog:
self.__log(register, mask, current, new)
if not self.__ifWriteBlock:
self.__i2c.write_byte_data(self.__slave, register, new)
def __readFromRegister(self, register, mask):
current = self.__i2c.read_byte_data(self.__slave, register) # Get current value
return bitOps.GetValueUnderMask(current, mask)
def __readFromRegisterWithDictionaryMatch(self, register, mask, dictionary):
current = self.__readFromRegister(register, mask)
for key in dictionary.keys():
if dictionary[key] == current:
return key
def __writeToRegisterWithDictionaryCheck(self, register, mask, value, dictionary, dictionaryName):
if value not in dictionary.keys():
raise Exception('Value:' + str(value) + ' is not in range of: ' + str(dictionaryName))
self.__writeToRegister(register, mask, dictionary[value])
__REG_R_WHO_AM_I = 0x0f # Device identification register
__REG_RW_CTRL_REG1 = 0x20 # Control register 1
__REG_RW_CTRL_REG2 = 0x21 # Control register 2
__REG_RW_CTRL_REG3 = 0x22 # Control register 3
__REG_RW_CTRL_REG4 = 0x23 # Control register 4
__REG_RW_CTRL_REG5 = 0x24 # Control register 5
__REG_RW_REFERENCE = 0x25 # Reference value for interrupt generation
__REG_R_OUT_TEMP = 0x26 # Output temperature
__REG_R_STATUS_REG = 0x27 # Status register
__REG_R_OUT_X_L = 0x28 # X-axis angular data rate LSB
__REG_R_OUT_X_H = 0x29 # X-axis angular data rate MSB
__REG_R_OUT_Y_L = 0x2a # Y-axis angular data rate LSB
__REG_R_OUT_Y_H = 0x2b # Y-axis angular data rate MSB
__REG_R_OUT_Z_L = 0x2c # Z-axis angular data rate LSB
__REG_R_OUT_Z_H = 0x2d # Z-axis angular data rate MSB
__REG_RW_FIFO_CTRL_REG = 0x2e # Fifo control register
__REG_R_FIFO_SRC_REG = 0x2f # Fifo src register
__REG_RW_INT1_CFG_REG = 0x30 # Interrupt 1 configuration register
__REG_R_INT1_SRC_REG = 0x31 # Interrupt source register
__REG_RW_INT1_THS_XH = 0x32 # Interrupt 1 threshold level X MSB register
__REG_RW_INT1_THS_XL = 0x33 # Interrupt 1 threshold level X LSB register
__REG_RW_INT1_THS_YH = 0x34 # Interrupt 1 threshold level Y MSB register
__REG_RW_INT1_THS_YL = 0x35 # Interrupt 1 threshold level Y LSB register
__REG_RW_INT1_THS_ZH = 0x36 # Interrupt 1 threshold level Z MSB register
__REG_RW_INT1_THS_ZL = 0x37 # Interrupt 1 threshold level Z LSB register
__REG_RW_INT1_DURATION = 0x38 # Interrupt 1 duration register
__MASK_CTRL_REG1_Xen = 0x01 # X enable
__MASK_CTRL_REG1_Yen = 0x02 # Y enable
__MASK_CTRL_REG1_Zen = 0x04 # Z enable
__MASK_CTRL_REG1_PD = 0x08 # Power-down
__MASK_CTRL_REG1_BW = 0x30 # Bandwidth
__MASK_CTRL_REG1_DR = 0xc0 # Output data rate
__MASK_CTRL_REG2_HPCF = 0x0f # High pass filter cutoff frequency
__MASK_CTRL_REG2_HPM = 0x30 # High pass filter mode selection
__MASK_CTRL_REG3_I2_EMPTY = 0x01 # FIFO empty interrupt on DRDY/INT2
__MASK_CTRL_REG3_I2_ORUN = 0x02 # FIFO overrun interrupt on DRDY/INT2
__MASK_CTRL_REG3_I2_WTM = 0x04 # FIFO watermark interrupt on DRDY/INT2
__MASK_CTRL_REG3_I2_DRDY = 0x08 # Date-ready on DRDY/INT2
__MASK_CTRL_REG3_PP_OD = 0x10 # Push-pull / Open-drain
__MASK_CTRL_REG3_H_LACTIVE = 0x20 # Interrupt active configuration on INT1
__MASK_CTRL_REG3_I1_BOOT = 0x40 # Boot status available on INT1
__MASK_CTRL_REG3_I1_Int1 = 0x80 # Interrupt enabled on INT1
__MASK_CTRL_REG4_SIM = 0x01 # SPI Serial interface selection
__MASK_CTRL_REG4_FS = 0x30 # Full scale selection
__MASK_CTRL_REG4_BLE = 0x40 # Big/little endian selection
__MASK_CTRL_REG4_BDU = 0x80 # Block data update
__MASK_CTRL_REG5_OUT_SEL = 0x03 # Out selection configuration
__MASK_CTRL_REG5_INT_SEL = 0xc0 # INT1 selection configuration
__MASK_CTRL_REG5_HPEN = 0x10 # High-pass filter enable
__MASK_CTRL_REG5_FIFO_EN = 0x40 # Fifo enable
__MASK_CTRL_REG5_BOOT = 0x80 # Reboot memory content
__MASK_STATUS_REG_ZYXOR = 0x80 # Z, Y, X axis overrun
__MASK_STATUS_REG_ZOR = 0x40 # Z axis overrun
__MASK_STATUS_REG_YOR = 0x20 # Y axis overrun
__MASK_STATUS_REG_XOR = 0x10 # X axis overrun
__MASK_STATUS_REG_ZYXDA = 0x08 # Z, Y, X data available
__MASK_STATUS_REG_ZDA = 0x04 # Z data available
__MASK_STATUS_REG_YDA = 0x02 # Y data available
__MASK_STATUS_REG_XDA = 0x01 # X data available
__MASK_FIFO_CTRL_REG_FM = 0xe0 # Fifo mode selection
__MASK_FIFO_CTRL_REG_WTM = 0x1f # Fifo treshold - watermark level
__MASK_FIFO_SRC_REG_FSS = 0x1f # Fifo stored data level
__MASK_FIFO_SRC_REG_EMPTY = 0x20 # Fifo empty bit
__MASK_FIFO_SRC_REG_OVRN = 0x40 # Overrun status
__MASK_FIFO_SRC_REG_WTM = 0x80 # Watermark status
__MASK_INT1_CFG_ANDOR = 0x80 # And/Or configuration of interrupt events
__MASK_INT1_CFG_LIR = 0x40 # Latch interrupt request
__MASK_INT1_CFG_ZHIE = 0x20 # Enable interrupt generation on Z high
__MASK_INT1_CFG_ZLIE = 0x10 # Enable interrupt generation on Z low
__MASK_INT1_CFG_YHIE = 0x08 # Enable interrupt generation on Y high
__MASK_INT1_CFG_YLIE = 0x04 # Enable interrupt generation on Y low
__MASK_INT1_CFG_XHIE = 0x02 # Enable interrupt generation on X high
__MASK_INT1_CFG_XLIE = 0x01 # Enable interrupt generation on X low
__MASK_INT1_SRC_IA = 0x40 # Int1 active
__MASK_INT1_SRC_ZH = 0x20 # Int1 source Z high
__MASK_INT1_SRC_ZL = 0x10 # Int1 source Z low
__MASK_INT1_SRC_YH = 0x08 # Int1 source Y high
__MASK_INT1_SRC_YL = 0x04 # Int1 source Y low
__MASK_INT1_SRC_XH = 0x02 # Int1 source X high
__MASK_INT1_SRC_XL = 0x01 # Int1 source X low
__MASK_INT1_THS_H = 0x7f # MSB
__MASK_INT1_THS_L = 0xff # LSB
__MASK_INT1_DURATION_WAIT = 0x80 # Wait number of samples or not
__MASK_INT1_DURATION_D = 0x7f # Duration of int1 to be recognized
PowerModeEnum = [ 'Power-down', 'Sleep', 'Normal']
__PowerModeDict = { PowerModeEnum[0] : 0, PowerModeEnum[1] : 1, PowerModeEnum[2] : 2 }
EnabledEnum = [ False, True ]
__EnabledDict = { EnabledEnum[0] : 0, EnabledEnum[1] : 1}
LevelEnum = [ 'High', 'Low' ]
__LevelDict = { LevelEnum[0] : 0, LevelEnum[1] : 1 }
OutputEnum = [ 'Push-pull', 'Open drain' ]
__OutputDict = { OutputEnum[0] : 0, OutputEnum[1] : 1 }
SimModeEnum = [ '4-wire', '3-wire' ]
__SimModeDict = { SimModeEnum[0] : 0, SimModeEnum[1] : 1 }
FullScaleEnum = [ '250dps', '500dps', '2000dps' ]
__FullScaleDict = { FullScaleEnum[0] : 0x00, FullScaleEnum[1] : 0x01, FullScaleEnum[2] : 0x02}
BigLittleEndianEnum = [ 'Big endian', 'Little endian' ]
__BigLittleEndianDict = { BigLittleEndianEnum[0] : 0x00, BigLittleEndianEnum[1] : 0x01 }
BlockDataUpdateEnum = [ 'Continous update', 'Output registers not updated until reading' ]
__BlockDataUpdateDict = { BlockDataUpdateEnum[0] : 0x00, BlockDataUpdateEnum[1] : 0x01 }
OutSelEnum = [ 'LPF1', 'HPF', 'LPF2' ]
__OutSelDict = { OutSelEnum[0] : 0x00, OutSelEnum[1] : 0x01, OutSelEnum[2] : 0x02 }
IntSelEnum = [ 'LPF1', 'HPF', 'LPF2' ]
__IntSelDict = { IntSelEnum[0] : 0x00, IntSelEnum[1] : 0x01, IntSelEnum[2] : 0x02 }
BootModeEnum = [ 'Normal', 'Reboot memory content' ]
__BootModeDict = { BootModeEnum[0] : 0x00, BootModeEnum[1] : 0x01 }
FifoModeEnum = [ 'Bypass', 'FIFO', 'Stream', 'Stream-to-Fifo', 'Bypass-to-Stream' ]
__FifoModeDict = {
FifoModeEnum[0] : 0x00,
FifoModeEnum[1] : 0x01,
FifoModeEnum[2] : 0x02,
FifoModeEnum[3] : 0x03,
FifoModeEnum[4] : 0x04
}
AndOrEnum = [ 'And', 'Or' ]
__AndOrDict = { AndOrEnum[0] : 0x00, AndOrEnum[1] : 0x01 }
DataRateValues = [95, 190, 380, 760]
BandWidthValues = [12.5, 20, 25, 30, 35, 50, 70, 100]
__DRBW = {
DataRateValues[0] : { BandWidthValues[0]:0x00, BandWidthValues[2]:0x01},
DataRateValues[1] : { BandWidthValues[0]:0x04, BandWidthValues[2]:0x05, BandWidthValues[5]:0x06, BandWidthValues[6]:0x07},
DataRateValues[2] : { BandWidthValues[1]:0x08, BandWidthValues[2]:0x09, BandWidthValues[5]:0x0a, BandWidthValues[7]:0x0b},
DataRateValues[3] : { BandWidthValues[3]:0x0c, BandWidthValues[4]:0x0d, BandWidthValues[5]:0x0e, BandWidthValues[7]:0x0f}
}
HighPassFilterCutOffFrequencyValues = [51.4, 27, 13.5, 7.2, 3.5, 1.8, 0.9, 0.45, 0.18, 0.09, 0.045, 0.018, 0.009]
__HPCF = {
HighPassFilterCutOffFrequencyValues[0] : { DataRateValues[3]:0x00 },
HighPassFilterCutOffFrequencyValues[1] : { DataRateValues[2]:0x00, DataRateValues[3]:0x01 },
HighPassFilterCutOffFrequencyValues[2] : { DataRateValues[1]:0x00, DataRateValues[2]:0x01, DataRateValues[3]:0x02 },
HighPassFilterCutOffFrequencyValues[3] : { DataRateValues[0]:0x00, DataRateValues[1]:0x01, DataRateValues[2]:0x02, DataRateValues[3]:0x03 },
HighPassFilterCutOffFrequencyValues[4] : { DataRateValues[0]:0x01, DataRateValues[1]:0x02, DataRateValues[2]:0x03, DataRateValues[3]:0x04 },
HighPassFilterCutOffFrequencyValues[5] : { DataRateValues[0]:0x02, DataRateValues[1]:0x03, DataRateValues[2]:0x04, DataRateValues[3]:0x05 },
HighPassFilterCutOffFrequencyValues[6] : { DataRateValues[0]:0x03, DataRateValues[1]:0x04, DataRateValues[2]:0x05, DataRateValues[3]:0x06 },
HighPassFilterCutOffFrequencyValues[7] : { DataRateValues[0]:0x04, DataRateValues[1]:0x05, DataRateValues[2]:0x06, DataRateValues[3]:0x07 },
HighPassFilterCutOffFrequencyValues[8] : { DataRateValues[0]:0x05, DataRateValues[1]:0x06, DataRateValues[2]:0x07, DataRateValues[3]:0x08 },
HighPassFilterCutOffFrequencyValues[9] : { DataRateValues[0]:0x06, DataRateValues[1]:0x07, DataRateValues[2]:0x08, DataRateValues[3]:0x09 },
HighPassFilterCutOffFrequencyValues[10] : { DataRateValues[0]:0x07, DataRateValues[1]:0x08, DataRateValues[2]:0x09 },
HighPassFilterCutOffFrequencyValues[11] : { DataRateValues[0]:0x08, DataRateValues[1]:0x09 },
HighPassFilterCutOffFrequencyValues[12] : { DataRateValues[0]:0x09 }
}
HighPassFilterModes = ['Normal with reset.','Reference signal for filtering.','Normal.','Autoreset on interrupt.']
__HpmDict = {
HighPassFilterModes[0]:0x0,
HighPassFilterModes[1]:0x1,
HighPassFilterModes[2]:0x2,
HighPassFilterModes[3]:0x3
}
# For calibration purposes
meanX = 0
maxX = 0
minX = 0
meanY = 0
maxY = 0
minY = 0
meanZ = 0
maxZ = 0
minZ = 0
gain = 1
def Init(self):
"""Call this method after configuratin and before doing measurements"""
print("Initiating...")
if (self.Get_FullScale_Value() == self.FullScaleEnum[0]):
self.gain = 0.00875
elif (self.Get_FullScale_Value() == self.FullScaleEnum[1]):
self.gain = 0.0175
elif (self.Get_FullScale_Value() == self.FullScaleEnum[2]):
self.gain = 0.07
print("Gain set to:{0}".format(self.gain))
def CalibrateX(self):
"""Returns (min, mean, max)"""
print("Calibrating axis X, please do not move sensor...")
buff = []
for t in range(20):
while self.Get_AxisDataAvailable_Value()[0] == 0:
time.sleep(0.0001)
buff.append(self.Get_RawOutX_Value())
self.meanX = numpy.mean(buff)
self.maxX = max(buff)
self.minX = min(buff)
print("Done: (min={0};mean={1};max={2})".format(self.minX, self.meanX, self.maxX))
def CalibrateY(self):
"""Returns (min, mean, max)"""
print("Calibrating axis Y, please do not move sensor...")
buff = []
for t in range(20):
while self.Get_AxisDataAvailable_Value()[1] == 0:
time.sleep(0.0001)
buff.append(self.Get_RawOutY_Value())
self.meanY = numpy.mean(buff)
self.maxY = max(buff)
self.minY = min(buff)
print("Done: (min={0};mean={1};max={2})".format(self.minY, self.meanY, self.maxY))
def CalibrateZ(self):
"""Returns (min, mean, max)"""
print("Calibrating axis Z, please do not move sensor...")
buff = []
for t in range(20):
while self.Get_AxisDataAvailable_Value()[2] == 0:
time.sleep(0.0001)
buff.append(self.Get_RawOutZ_Value())
self.meanZ = numpy.mean(buff)
self.maxZ = max(buff)
self.minZ = min(buff)
print("Done: (min={0};mean={1};max={2})".format(self.minZ, self.meanZ, self.maxZ))
def Calibrate(self):
self.CalibrateX()
self.CalibrateY()
self.CalibrateZ()
def ReturnConfiguration(self):
return [
[ self.Get_DeviceId_Value.__doc__, self.Get_DeviceId_Value()],
[ self.Get_DataRateAndBandwidth.__doc__, self.Get_DataRateAndBandwidth()],
[ self.Get_AxisX_Enabled.__doc__, self.Get_AxisX_Enabled()],
[ self.Get_AxisY_Enabled.__doc__, self.Get_AxisY_Enabled()],
[ self.Get_AxisZ_Enabled.__doc__, self.Get_AxisZ_Enabled()],
[ self.Get_PowerMode.__doc__, self.Get_PowerMode()],
[ self.Get_HighPassCutOffFreq.__doc__, self.Get_HighPassCutOffFreq()],
[ self.Get_INT1_Enabled.__doc__, self.Get_INT1_Enabled()],
[ self.Get_BootStatusOnINT1_Enabled.__doc__, self.Get_BootStatusOnINT1_Enabled()],
[ self.Get_ActiveConfINT1_Level.__doc__, self.Get_ActiveConfINT1_Level()],
[ self.Get_PushPullOrOpenDrain_Value.__doc__, self.Get_PushPullOrOpenDrain_Value()],
[ self.Get_DataReadyOnINT2_Enabled.__doc__, self.Get_DataReadyOnINT2_Enabled()],
[ self.Get_FifoWatermarkOnINT2_Enabled.__doc__, self.Get_FifoWatermarkOnINT2_Enabled()],
[ self.Get_FifoOverrunOnINT2_Enabled.__doc__, self.Get_FifoOverrunOnINT2_Enabled()],
[ self.Get_FifoEmptyOnINT2_Enabled.__doc__, self.Get_FifoEmptyOnINT2_Enabled()],
[ self.Get_SpiMode_Value.__doc__, self.Get_SpiMode_Value()],
[ self.Get_FullScale_Value.__doc__, self.Get_FullScale_Value()],
[ self.Get_BigLittleEndian_Value.__doc__, self.Get_BigLittleEndian_Value()],
[ self.Get_BlockDataUpdate_Value.__doc__, self.Get_BlockDataUpdate_Value()],
[ self.Get_BootMode_Value.__doc__, self.Get_BootMode_Value()],
[ self.Get_Fifo_Enabled.__doc__, self.Get_Fifo_Enabled()],
[ self.Get_HighPassFilter_Enabled.__doc__, self.Get_HighPassFilter_Enabled()],
[ self.Get_INT1Selection_Value.__doc__, self.Get_INT1Selection_Value()],
[ self.Get_OutSelection_Value.__doc__, self.Get_OutSelection_Value()],
[ self.Get_Reference_Value.__doc__, self.Get_Reference_Value()],
[ self.Get_AxisOverrun_Value.__doc__, self.Get_AxisOverrun_Value()],
[ self.Get_AxisDataAvailable_Value.__doc__, self.Get_AxisDataAvailable_Value()],
[ self.Get_FifoThreshold_Value.__doc__, self.Get_FifoThreshold_Value()],
[ self.Get_FifoMode_Value.__doc__, self.Get_FifoMode_Value()],
[ self.Get_FifoStoredDataLevel_Value.__doc__, self.Get_FifoStoredDataLevel_Value()],
[ self.Get_IsFifoEmpty_Value.__doc__, self.Get_IsFifoEmpty_Value()],
[ self.Get_IsFifoFull_Value.__doc__, self.Get_IsFifoFull_Value()],
[ self.Get_IsFifoGreaterOrEqualThanWatermark_Value.__doc__, self.Get_IsFifoGreaterOrEqualThanWatermark_Value()],
[ self.Get_Int1Combination_Value.__doc__, self.Get_Int1Combination_Value() ],
[ self.Get_Int1LatchRequest_Enabled.__doc__, self.Get_Int1LatchRequest_Enabled() ],
[ self.Get_Int1GenerationOnZHigh_Enabled.__doc__, self.Get_Int1GenerationOnZHigh_Enabled() ],
[ self.Get_Int1GenerationOnZLow_Enabled.__doc__, self.Get_Int1GenerationOnZLow_Enabled() ],
[ self.Get_Int1GenerationOnYHigh_Enabled.__doc__, self.Get_Int1GenerationOnYHigh_Enabled() ],
[ self.Get_Int1GenerationOnYLow_Enabled.__doc__, self.Get_Int1GenerationOnYLow_Enabled() ],
[ self.Get_Int1GenerationOnXHigh_Enabled.__doc__, self.Get_Int1GenerationOnXHigh_Enabled() ],
[ self.Get_Int1GenerationOnXLow_Enabled.__doc__, self.Get_Int1GenerationOnXLow_Enabled() ],
[ self.Get_Int1Active_Value.__doc__, self.Get_Int1Active_Value() ],
[ self.Get_ZHighEventOccured_Value.__doc__, self.Get_ZHighEventOccured_Value() ],
[ self.Get_ZLowEventOccured_Value.__doc__, self.Get_ZLowEventOccured_Value() ],
[ self.Get_YHighEventOccured_Value.__doc__, self.Get_YHighEventOccured_Value() ],
[ self.Get_YLowEventOccured_Value.__doc__, self.Get_YLowEventOccured_Value() ],
[ self.Get_XHighEventOccured_Value.__doc__, self.Get_XHighEventOccured_Value() ],
[ self.Get_XLowEventOccured_Value.__doc__, self.Get_XLowEventOccured_Value() ],
[ self.Get_Int1Threshold_Values.__doc__, self.Get_Int1Threshold_Values() ],
[ self.Get_Int1DurationWait_Enabled.__doc__, self.Get_Int1DurationWait_Enabled() ],
[ self.Get_Int1Duration_Value.__doc__, self.Get_Int1Duration_Value() ]
]
def Get_DeviceId_Value(self):
"""Device Id."""
return self.__readFromRegister(self.__REG_R_WHO_AM_I, 0xff)
def Set_AxisX_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Xen, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_AxisX_Enabled(self):
"""Axis X enabled."""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Xen, self.__EnabledDict)
def Set_AxisY_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Yen, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_AxisY_Enabled(self):
"""Axis Y enabled."""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Yen, self.__EnabledDict)
def Set_AxisZ_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Zen, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_AxisZ_Enabled(self):
"""Axis Z enabled."""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_Zen, self.__EnabledDict)
def Set_PowerMode(self, mode):
if mode not in self.__PowerModeDict.keys():
raise Exception('Value:' + str(mode) + ' is not in range of: PowerModeEnum')
if self.__PowerModeDict[mode] == 0:
# Power-down
self.__writeToRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_PD, 0)
elif self.__PowerModeDict[mode] == 1:
# Sleep
self.__writeToRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_PD | self.__MASK_CTRL_REG1_Zen | self.__MASK_CTRL_REG1_Yen | self.__MASK_CTRL_REG1_Xen, 8)
elif self.__PowerModeDict[mode] == 2:
# Normal
self.__writeToRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_PD, 1)
def Get_PowerMode(self):
"""Power mode."""
powermode = self.__readFromRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_PD | self.__MASK_CTRL_REG1_Xen | self.__MASK_CTRL_REG1_Yen | self.__MASK_CTRL_REG1_Zen)
print(bin(powermode))
dictval = 4
if not bitOps.CheckBit(powermode, 3):
dictval = 0
elif powermode == 0b1000:
dictval = 1
elif bitOps.CheckBit(powermode, 3):
dictval = 2
for key in self.__PowerModeDict.keys():
if self.__PowerModeDict[key] == dictval:
return key
def Print_DataRateAndBandwidth_AvailableValues(self):
for dr in self.__DRBW.keys():
print('Output data rate: ' + dr + '[Hz]')
for bw in self.__DRBW[dr].keys():
print(' Bandwidth: ' + bw + ' (DRBW=' +'0b' + bin(self.__DRBW[dr][bw])[2:].zfill(4) +')')
def Set_DataRateAndBandwidth(self, datarate, bandwidth):
if datarate not in self.__DRBW.keys():
raise Exception('Data rate:' + str(datarate) + ' not in range of data rate values.')
if bandwidth not in self.__DRBW[datarate].keys():
raise Exception('Bandwidth: ' + str(bandwidth) + ' cannot be assigned to data rate: ' + str(datarate))
bits = self.__DRBW[datarate][bandwidth]
self.__writeToRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_DR | self.__MASK_CTRL_REG1_BW, bits)
def Get_DataRateAndBandwidth(self):
"""Data rate and bandwidth."""
current = self.__readFromRegister(self.__REG_RW_CTRL_REG1, self.__MASK_CTRL_REG1_DR | self.__MASK_CTRL_REG1_BW)
for dr in self.__DRBW.keys():
for bw in self.__DRBW[dr].keys():
if self.__DRBW[dr][bw] == current:
return (dr, bw)
def Print_HighPassFilterCutOffFrequency_AvailableValues(self):
for freq in self.__HPCF.keys():
print('High pass cut off: ' + freq + '[Hz]')
for odr in self.__HPCF[freq].keys():
print(' Output data rate: ' + odr + ' (HPCF=' + '0b' + bin(self.__HPCF[freq][odr])[2:].zfill(4) + ')')
def Set_HighPassCutOffFreq(self, freq):
if freq not in self.__HPCF.keys():
raise Exception('Frequency:' + str(freq) + ' is not in range of high pass frequency cut off values.')
datarate = self.Get_DataRateAndBandwidth()[0]
if datarate not in self.__HPCF[freq].keys():
raise Exception('Frequency: ' + str(freq) + ' cannot be assigned to data rate: ' + str(datarate))
bits = self.__HPCF[freq][datarate]
self.__writeToRegister(self.__REG_RW_CTRL_REG2, self.__MASK_CTRL_REG2_HPCF, bits)
def Get_HighPassCutOffFreq(self):
"""Cut off frequency."""
current = self.__readFromRegister(self.__REG_RW_CTRL_REG2, self.__MASK_CTRL_REG2_HPCF)
datarate = self.Get_DataRateAndBandwidth()[0]
for freq in self.__HPCF.keys():
for dr in self.__HPCF[freq]:
if dr == datarate:
if self.__HPCF[freq][datarate] == current:
return freq
def Set_HighPassFilterMode(self, mode):
if mode not in self.__HpmDict.keys():
raise Exception('EnabledEnum:' + str(mode) + ' is not in range of high pass frequency modes.')
bits = self.__HpmDict[mode]
self.__writeToRegister(self.__REG_RW_CTRL_REG2, self.__MASK_CTRL_REG2_HPM, bits)
def Get_HighPassFilterMode(self):
"""High pass filter mode"""
current = self.__readFromRegister(self.__REG_RW_CTRL_REG2, self.__MASK_CTRL_REG2_HPM)
for mode in self.__HpmDict.keys():
if self.__HpmDict[mode] == current:
return mode
def Set_INT1_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I1_Int1, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_INT1_Enabled(self):
"""INT1 Enabled"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I1_Int1, self.__EnabledDict)
def Set_BootStatusOnINT1_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I1_BOOT, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_BootStatusOnINT1_Enabled(self):
"""Boot status available on INT1"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I1_BOOT, self.__EnabledDict)
def Set_ActiveConfINT1_Level(self, level):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_H_LACTIVE, level, self.__LevelDict, 'LevelEnum')
def Get_ActiveConfINT1_Level(self):
"""Interrupt active configuration on INT1"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_H_LACTIVE, self.__LevelDict)
def Set_PushPullOrOpenDrain_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_PP_OD, value, self.__OutputDict, 'OutputEnum')
def Get_PushPullOrOpenDrain_Value(self):
"""Push-pull/open drain"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_PP_OD, self.__OutputDict)
def Set_DataReadyOnINT2_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_DRDY, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_DataReadyOnINT2_Enabled(self):
"""Date-ready on DRDY/INT2"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_DRDY, self.__EnabledDict)
def Set_FifoWatermarkOnINT2_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_WTM, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_FifoWatermarkOnINT2_Enabled(self):
"""FIFO watermark interrupt on DRDY/INT2"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_WTM, self.__EnabledDict)
def Set_FifoOverrunOnINT2_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_ORUN, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_FifoOverrunOnINT2_Enabled(self):
"""FIFO overrun interrupt in DRDY/INT2"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_ORUN, self.__EnabledDict)
def Set_FifoEmptyOnINT2_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_EMPTY, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_FifoEmptyOnINT2_Enabled(self):
"""FIFO empty interrupt on DRDY/INT2"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG3, self.__MASK_CTRL_REG3_I2_EMPTY, self.__EnabledDict)
def Set_SpiMode_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_SIM, value, self.__SimModeDict, 'SimModeEnum')
def Get_SpiMode_Value(self):
"""SPI mode"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_SIM, self.__SimModeDict)
def Set_FullScale_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_FS, value, self.__FullScaleDict, 'FullScaleEnum')
def Get_FullScale_Value(self):
"""Full scale selection"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_FS, self.__FullScaleDict)
def Set_BigLittleEndian_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_BLE, value, self.__BigLittleEndianDict, 'BigLittleEndianEnum')
def Get_BigLittleEndian_Value(self):
"""Big/Little endian"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_BLE, self.__BigLittleEndianDict)
def Set_BlockDataUpdate_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_BDU, value, self.__BlockDataUpdateDict, 'BlockDataUpdateEnum')
def Get_BlockDataUpdate_Value(self):
"""Block data update"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG4, self.__MASK_CTRL_REG4_BDU, self.__BlockDataUpdateDict)
def Set_BootMode_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_BOOT, value, self.__BootModeDict, 'BootModeEnum')
def Get_BootMode_Value(self):
"""Boot mode"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_BOOT, self.__BootModeDict)
def Set_Fifo_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_FIFO_EN, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_Fifo_Enabled(self):
"""Fifo enabled"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_FIFO_EN, self.__EnabledDict)
def Set_HighPassFilter_Enabled(self, enabled):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_HPEN, enabled, self.__EnabledDict, 'EnabledEnum')
def Get_HighPassFilter_Enabled(self):
"""High pass filter enabled"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_HPEN, self.__EnabledDict)
def Set_INT1Selection_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_INT_SEL, value, self.__IntSelDict, 'IntSelEnum')
def Get_INT1Selection_Value(self):
"""INT1 selection configuration"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_INT_SEL, self.__IntSelDict)
def Set_OutSelection_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_OUT_SEL, value, self.__OutSelDict, 'OutSelEnum')
def Get_OutSelection_Value(self):
"""Out selection configuration"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_CTRL_REG5, self.__MASK_CTRL_REG5_OUT_SEL, self.__OutSelDict)
def Set_Reference_Value(self, value):
self.__writeToRegister(self.__REG_RW_REFERENCE, 0xff, value)
def Get_Reference_Value(self):
"""Reference value for interrupt generation"""
return self.__readFromRegister(self.__REG_RW_REFERENCE, 0xff)
def Get_OutTemp_Value(self):
"""Output temperature"""
return self.__readFromRegister(self.__REG_R_OUT_TEMP, 0xff)
def Get_AxisOverrun_Value(self):
"""(X, Y, Z) axis overrun"""
zor = 0
yor = 0
xor = 0
if self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_ZYXOR) == 0x01:
zor = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_ZOR)
yor = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_YOR)
xor = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_XOR)
return (xor, yor, zor)
def Get_AxisDataAvailable_Value(self):
"""(X, Y, Z) data available"""
zda = 0
yda = 0
xda = 0
if self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_ZYXDA) == 0x01:
zda = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_ZDA)
yda = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_YDA)
xda = self.__readFromRegister(self.__REG_R_STATUS_REG, self.__MASK_STATUS_REG_XDA)
return (xda, yda, zda)
def Get_RawOutX_Value(self):
"""Raw X angular speed data"""
l = self.__readFromRegister(self.__REG_R_OUT_X_L, 0xff)
h_u2 = self.__readFromRegister(self.__REG_R_OUT_X_H, 0xff)
h = bitOps.TwosComplementToByte(h_u2)
if (h < 0):
return (h*256 - l) * self.gain
elif (h >= 0):
return (h*256 + l) * self.gain
def Get_RawOutY_Value(self):
"""Raw Y angular speed data"""
l = self.__readFromRegister(self.__REG_R_OUT_Y_L, 0xff)
h_u2 = self.__readFromRegister(self.__REG_R_OUT_Y_H, 0xff)
h = bitOps.TwosComplementToByte(h_u2)
if (h < 0):
return (h*256 - l) * self.gain
elif (h >= 0):
return (h*256 + l) * self.gain
def Get_RawOutZ_Value(self):
"""Raw Z angular speed data"""
l = self.__readFromRegister(self.__REG_R_OUT_Z_L, 0xff)
h_u2 = self.__readFromRegister(self.__REG_R_OUT_Z_H, 0xff)
h = bitOps.TwosComplementToByte(h_u2)
if (h < 0):
return (h*256 - l) * self.gain
elif (h >= 0):
return (h*256 + l) * self.gain
def Get_RawOut_Value(self):
"""Raw [X, Y, Z] values of angular speed"""
return [self.Get_RawOutX_Value(), self.Get_RawOutY_Value(), self.Get_RawOutZ_Value()]
def Get_CalOutX_Value(self):
"""Calibrated X angular speed data"""
x = self.Get_RawOutX_Value()
if(x >= self.minX and x <= self.maxX):
return 0
else:
return x - self.meanX
def Get_CalOutY_Value(self):
"""Calibrated Y angular speed data"""
y = self.Get_RawOutY_Value()
if(y >= self.minY and y <= self.maxY):
return 0
else:
return y - self.meanY
def Get_CalOutZ_Value(self):
"""Calibrated Z angular speed data"""
z = self.Get_RawOutZ_Value()
if(z >= self.minZ and z <= self.maxZ):
return 0
else:
return z - self.meanZ
def Get_CalOut_Value(self):
"""Calibrated [X, Y, Z] value of angular speed, calibrated"""
return [self.Get_CalOutX_Value(), self.Get_CalOutY_Value(), self.Get_CalOutZ_Value()]
def Set_FifoThreshold_Value(self, value):
self.__writeToRegister(self.__REG_RW_FIFO_CTRL_REG, self.__MASK_FIFO_CTRL_REG_WTM, value)
def Get_FifoThreshold_Value(self):
"""Fifo threshold - watermark level"""
return self.__readFromRegister(self.__REG_RW_FIFO_CTRL_REG, self.__MASK_FIFO_CTRL_REG_WTM)
def Set_FifoMode_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_FIFO_CTRL_REG, self.__MASK_FIFO_CTRL_REG_FM, value, self.__FifoModeDict, 'FifoModeEnum')
def Get_FifoMode_Value(self):
"""Fifo mode"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_FIFO_CTRL_REG, self.__MASK_FIFO_CTRL_REG_FM, self.__FifoModeDict)
def Get_FifoStoredDataLevel_Value(self):
"""Fifo stored data level"""
return self.__readFromRegister(self.__REG_R_FIFO_SRC_REG, self.__MASK_FIFO_SRC_REG_FSS)
def Get_IsFifoEmpty_Value(self):
"""Fifo empty"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_FIFO_SRC_REG, self.__MASK_FIFO_SRC_REG_EMPTY, self.__EnabledDict)
def Get_IsFifoFull_Value(self):
"""Fifo full"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_FIFO_SRC_REG, self.__MASK_FIFO_SRC_REG_OVRN, self.__EnabledDict)
def Get_IsFifoGreaterOrEqualThanWatermark_Value(self):
"""Fifo filling is greater or equal than watermark level"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_FIFO_SRC_REG, self.__MASK_FIFO_SRC_REG_WTM, self.__EnabledDict)
def Set_Int1Combination_Value(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ANDOR, value, self.__AndOrDict, 'AndOrEnum')
def Get_Int1Combination_Value(self):
"""Interrupt combination"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ANDOR, self.__AndOrDict)
def Set_Int1LatchRequest_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_LIR, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1LatchRequest_Enabled(self):
"""Latch interrupt request"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_LIR, self.__EnabledDict)
def Set_Int1GenerationOnZHigh_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ZHIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnZHigh_Enabled(self):
"""Int 1 generation on Z higher than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ZHIE, self.__EnabledDict)
def Set_Int1GenerationOnZLow_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ZLIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnZLow_Enabled(self):
"""Int 1 generation on Z lower than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_ZLIE, self.__EnabledDict)
def Set_Int1GenerationOnYHigh_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_YHIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnYHigh_Enabled(self):
"""Int 1 generation on Y higher than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_YHIE, self.__EnabledDict)
def Set_Int1GenerationOnYLow_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_YLIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnYLow_Enabled(self):
"""Int 1 generation on Y lower than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_YLIE, self.__EnabledDict)
def Set_Int1GenerationOnXHigh_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_XHIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnXHigh_Enabled(self):
"""Int 1 generation on X higher than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_XHIE, self.__EnabledDict)
def Set_Int1GenerationOnXLow_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_XLIE, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1GenerationOnXLow_Enabled(self):
"""Int 1 generation on X lower than threshold"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_CFG_REG, self.__MASK_INT1_CFG_XLIE, self.__EnabledDict)
def Get_Int1Active_Value(self):
"""Int1 active"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_IA, self.__EnabledDict)
def Get_ZHighEventOccured_Value(self):
"""Z high event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_ZH, self.__EnabledDict)
def Get_ZLowEventOccured_Value(self):
"""Z low event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_ZL, self.__EnabledDict)
def Get_YHighEventOccured_Value(self):
"""Y high event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_YH, self.__EnabledDict)
def Get_YLowEventOccured_Value(self):
"""Y low event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_YL, self.__EnabledDict)
def Get_XHighEventOccured_Value(self):
"""X high event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_XH, self.__EnabledDict)
def Get_XLowEventOccured_Value(self):
"""X low event occured"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_R_INT1_SRC_REG, self.__MASK_INT1_SRC_XL, self.__EnabledDict)
def Set_Int1ThresholdX_Value(self, value):
self.__writeToRegister(self.__REG_RW_INT1_THS_XH, self.__MASK_INT1_THS_H, (value & 0x7f00) >> 8)
self.__writeToRegister(self.__REG_RW_INT1_THS_XL, self.__MASK_INT1_THS_L, value & 0x00ff)
def Set_Int1ThresholdY_Value(self, value):
self.__writeToRegister(self.__REG_RW_INT1_THS_YH, self.__MASK_INT1_THS_H, (value & 0x7f00) >> 8)
self.__writeToRegister(self.__REG_RW_INT1_THS_YL, self.__MASK_INT1_THS_L, value & 0x00ff)
def Set_Int1ThresholdZ_Value(self, value):
self.__writeToRegister(self.__REG_RW_INT1_THS_ZH, self.__MASK_INT1_THS_H, (value & 0x7f00) >> 8)
self.__writeToRegister(self.__REG_RW_INT1_THS_ZL, self.__MASK_INT1_THS_L, value & 0x00ff)
def Get_Int1Threshold_Values(self):
"""(X,Y,Z) INT1 threshold value"""
xh = self.__readFromRegister(self.__REG_RW_INT1_THS_XH, self.__MASK_INT1_THS_H)
xl = self.__readFromRegister(self.__REG_RW_INT1_THS_XL, self.__MASK_INT1_THS_L)
yh = self.__readFromRegister(self.__REG_RW_INT1_THS_YH, self.__MASK_INT1_THS_H)
yl = self.__readFromRegister(self.__REG_RW_INT1_THS_YL, self.__MASK_INT1_THS_L)
zh = self.__readFromRegister(self.__REG_RW_INT1_THS_ZH, self.__MASK_INT1_THS_H)
zl = self.__readFromRegister(self.__REG_RW_INT1_THS_ZL, self.__MASK_INT1_THS_L)
return (xh*256 + xl, yh*256 + yl, zh*256 + zl)
def Set_Int1DurationWait_Enabled(self, value):
self.__writeToRegisterWithDictionaryCheck(self.__REG_RW_INT1_DURATION, self.__MASK_INT1_DURATION_WAIT, value, self.__EnabledDict, 'EnabledEnum')
def Get_Int1DurationWait_Enabled(self):
"""Int 1 duration wait"""
return self.__readFromRegisterWithDictionaryMatch(self.__REG_RW_INT1_DURATION, self.__MASK_INT1_DURATION_WAIT, self.__EnabledDict)
def Set_Int1Duration_Value(self, value):
self.__writeToRegister(self.__REG_RW_INT1_DURATION, self.__MASK_INT1_DURATION_D, value)
def Get_Int1Duration_Value(self):
"""Int 1 duration value"""
return self.__readFromRegister(self.__REG_RW_INT1_DURATION, self.__MASK_INT1_DURATION_D)
class Mpl3115a2(object):
_bus = None
def __init__(self, i2c_bus=0):
"""
:type i2c_bus: int specifying i2c bus number
"""
self._bus = SMBus(i2c_bus)
whoami = self._bus.read_byte_data(MPL3115A2_ADDRESS, MPL3115A2_WHOAMI)
if whoami != 0xc4:
print("MPL3115A2 not active.")
exit(1)
# Set MPL3115A2 oversampling to 128, put in Barometer mode, enabled standby on CTRL_REG1
self._bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_CTRL_REG1,
MPL3115A2_CTRL_REG1_SBYB |
MPL3115A2_CTRL_REG1_OS128 |
MPL3115A2_CTRL_REG1_BAR)
# Configure MPL3115A2
self._bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_PT_DATA_CFG,
MPL3115A2_PT_DATA_CFG_TDEFE |
MPL3115A2_PT_DATA_CFG_PDEFE |
MPL3115A2_PT_DATA_CFG_DREM)
def poll(self):
sta = 0
while not (sta & MPL3115A2_REGISTER_STATUS_PDR):
sta = self._bus.read_byte_data(MPL3115A2_ADDRESS, MPL3115A2_REGISTER_STATUS)
def get_altitude(self):
# print "Reading Altitude Data..."
self._bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_CTRL_REG1,
MPL3115A2_CTRL_REG1_SBYB |
MPL3115A2_CTRL_REG1_OS128 |
MPL3115A2_CTRL_REG1_ALT) # change to altimeter mode
self.poll()
msb, csb, lsb = self._bus.read_i2c_block_data(MPL3115A2_ADDRESS, MPL3115A2_REGISTER_PRESSURE_MSB, 3)
# print msb, csb, lsb
alt = float((((msb << 24) | (csb << 16) | lsb) * 10) / 65536)
# correct sign
if alt > (1 << 15):
alt -= 1 << 16
return alt
def get_pressure(self):
# print "Reading Pressure Data..."
self._bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_CTRL_REG1,
MPL3115A2_CTRL_REG1_SBYB |
MPL3115A2_CTRL_REG1_OS128 |
MPL3115A2_CTRL_REG1_BAR) # change to barometer mode
self.poll()
msb, csb, lsb = self._bus.read_i2c_block_data(MPL3115A2_ADDRESS, MPL3115A2_REGISTER_PRESSURE_MSB, 3)
# print msb, csb, lsb
return ((msb << 16) | (csb << 8) | lsb) / 64.
def calibrate(self):
# print "Calibrating..."
p = 0
t = 0
a = 0
calibration_rounds = 5
for _i in np.arange(0, calibration_rounds, 1):
p += self.get_pressure()
t += self.get_temperature()
a += self.get_altitude()
print("MPL3115A2 Calibration Round: {0} of {1}".format((_i+1), calibration_rounds))
pa = int((p / 10) / 2)
ta = (t / 10)
aa = (a / 10)
self._bus.write_i2c_block_data(MPL3115A2_ADDRESS, MPL3115A2_BAR_IN_MSB, [pa >> 8 & 0xff, pa & 0xff])
return [pa, ta, aa]
def get_temperature(self):
# print "Reading Temperature Data..."
self._bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_CTRL_REG1,
MPL3115A2_CTRL_REG1_SBYB |
MPL3115A2_CTRL_REG1_OS128 |
MPL3115A2_CTRL_REG1_BAR)
self.poll()
t_data = self._bus.read_i2c_block_data(MPL3115A2_ADDRESS, MPL3115A2_REGISTER_STATUS_PDR, 2)
# status = _bus.read_byte_data(MPL3115A2_ADDRESS, 0x00)
# print t_data
return t_data[0] + (t_data[1] >> 4) / 16.0
class Mpl3115A2(object):
#I2C ADDRESS/BITS
ADDRESS = (0x60)
#REGISTERS
REGISTER_STATUS = (0x00)
REGISTER_STATUS_TDR = 0x02
REGISTER_STATUS_PDR = 0x04
REGISTER_STATUS_PTDR = 0x08
REGISTER_PRESSURE_MSB = (0x01)
REGISTER_PRESSURE_CSB = (0x02)
REGISTER_PRESSURE_LSB = (0x03)
REGISTER_TEMP_MSB = (0x04)
REGISTER_TEMP_LSB = (0x05)
REGISTER_DR_STATUS = (0x06)
OUT_P_DELTA_MSB = (0x07)
OUT_P_DELTA_CSB = (0x08)
OUT_P_DELTA_LSB = (0x09)
OUT_T_DELTA_MSB = (0x0A)
OUT_T_DELTA_LSB = (0x0B)
BAR_IN_MSB = (0x14)
BAR_IN_LSB = (0x15)
WHOAMI = (0x0C)
#BITS
PT_DATA_CFG = 0x13
PT_DATA_CFG_TDEFE = 0x01
PT_DATA_CFG_PDEFE = 0x02
PT_DATA_CFG_DREM = 0x04
CTRL_REG1 = (0x26)
CTRL_REG1_SBYB = 0x01
CTRL_REG1_OST = 0x02
CTRL_REG1_RST = 0x04
CTRL_REG1_OS1 = 0x00
CTRL_REG1_OS2 = 0x08
CTRL_REG1_OS4 = 0x10
CTRL_REG1_OS8 = 0x18
CTRL_REG1_OS16 = 0x20
CTRL_REG1_OS32 = 0x28
CTRL_REG1_OS64 = 0x30
CTRL_REG1_OS128 = 0x38
CTRL_REG1_RAW = 0x40
CTRL_REG1_ALT = 0x80
CTRL_REG1_BAR = 0x00
CTRL_REG2 = (0x27)
CTRL_REG3 = (0x28)
CTRL_REG4 = (0x29)
CTRL_REG5 = (0x2A)
REGISTER_STARTCONVERSION = (0x12)
def __init__(self):
self.bus = SMBus(1)
self.verify_device()
self.initialize()
def verify_device(self):
whoami = self.bus.read_byte_data(self.ADDRESS, self.WHOAMI)
if whoami != 0xc4:
# 0xc4 is a default value, but it can be programmed to other
# values. Mine reports 0xee.
print "Device not active: %x != %x"%(whoami,0xc4)
return False
return True
def initialize(self):
self.bus.write_byte_data(
self.ADDRESS,
self.CTRL_REG1,
self.CTRL_REG1_SBYB |
self.CTRL_REG1_OS128 |
self.CTRL_REG1_ALT)
self.bus.write_byte_data(
self.ADDRESS,
self.PT_DATA_CFG,
self.PT_DATA_CFG_TDEFE |
self.PT_DATA_CFG_PDEFE |
self.PT_DATA_CFG_DREM)
def poll(self,flag=None):
if flag is None:
flag=self.REGISTER_STATUS_PDR
sta = 0
while not (sta & self.REGISTER_STATUS_PDR):
sta = self.bus.read_byte_data(self.ADDRESS, self.REGISTER_STATUS)
def altitude():
self.bus.write_byte_data(
self.ADDRESS,
self.CTRL_REG1,
self.CTRL_REG1_SBYB |
self.CTRL_REG1_OS128 |
self.CTRL_REG1_ALT)
self.poll()
msb, csb, lsb = self.bus.read_i2c_block_data(self.ADDRESS,
self.REGISTER_PRESSURE_MSB,3)
alt = ((msb<<24) | (csb<<16) | (lsb<<8)) / 65536.
# correct sign
if alt > (1<<15):
alt -= 1<<16
return alt
def temperature(self):
self.bus.write_byte_data(
self.ADDRESS,
self.CTRL_REG1,
self.CTRL_REG1_SBYB |
self.CTRL_REG1_OS128 |
self.CTRL_REG1_BAR)
self.poll()
return self.read_temperature()
def read_temperature(self):
msb, lsb = self.bus.read_i2c_block_data(self.ADDRESS,
self.REGISTER_TEMP_MSB,2)
# 12 bit, 2s-complement in degrees C.
return ( (msb<<8) | lsb) / 256.0
def pressure(self):
self.bus.write_byte_data(
self.ADDRESS,
self.CTRL_REG1,
self.CTRL_REG1_SBYB |
self.CTRL_REG1_OS128 |
self.CTRL_REG1_BAR)
self.poll()
return self.read_pressure()
def press_temp(self):
self.bus.write_byte_data(
self.ADDRESS,
self.CTRL_REG1,
self.CTRL_REG1_SBYB |
self.CTRL_REG1_OS128 |
self.CTRL_REG1_BAR)
self.poll(self.REGISTER_STATUS_PDR|self.REGISTER_STATUS_TDR)
press=self.read_pressure()
temp=self.read_temperature()
return press,temp
def read_pressure(self):
msb, csb, lsb = self.bus.read_i2c_block_data(self.ADDRESS,
self.REGISTER_PRESSURE_MSB,3)
return ((msb<<16) | (csb<<8) | lsb) / 64.
def calibrate(self):
pa = int(self.pressure()/2)
self.bus.write_i2c_block_data(self.ADDRESS,
self.BAR_IN_MSB,
[pa>>8 & 0xff, pa & 0xff])
class Position:
GTPA010_ADDRESS = 0x29
GTPA010_ID = 0b10101100
ID = 0x25
STATUS = 0x00
UPDATE_RATE = 0x21
START = 0x23
LATITUDE_MSB = 0x1
LATITUDE_AMSB = 0x2
LATITUDE_ALSB= 0x3
LATITUDE_LSB = 0x4
LONGITUDE_MSB = 0x5
LONGITUDE_AMSB = 0x6
LONGITUDE_ALSB= 0x7
LONGITUDE_LSB = 0x8
ALTITUDE_MSB = 0x17
ALTITUDE_AMSB = 0x18
ALTITUDE_ALSB= 0x19
ALTITUDE_LSB = 0x1A
def __init__(self, filtering_alpha=0.6):
self._i2c_bus = SMBus(2)
self.address = self.GTPA010_ADDRESS
self._filtering_alpha = filtering_alpha
self._position_estimate = Point2D()
if self._read_byte(self.ID) == self.GTPA010_ID:
logging.info("GPS: GTPA010 detected successfully.")
else:
logging.info("GPS: No GTPA010 detected")
self._configure()
@property
def latitude(self):
msb = self._read_byte(self.LATITUDE_MSB)
amsb = self._read_byte(self.LATITUDE_AMSB)
alsb = self._read_byte(self.LATITUDE_ALSB)
lsb = self._read_byte(self.LATITUDE_LSB)
return _combine_position_bytes(msb, amsb, alsb, lsb)
@property
def longitude(self):
msb = self._read_byte(self.LONGITUDE_MSB)
amsb = self._read_byte(self.LONGITUDE_AMSB)
alsb = self._read_byte(self.LONGITUDE_ALSB)
lsb = self._read_byte(self.LONGITUDE_LSB)
return _combine_position_bytes(msb, amsb, alsb, lsb)
@property
def position_raw(self):
return Point2D(self.latitude, self.longitude)
@property
def position(self):
self._position_estimate = (
self._filtering_alpha * self.position +
(1 - self._filtering_alpha) * self._position_estimate)
return self._position_estimate
@property
def altitude(self):
msb = self._read_byte(self.ALTITUDE_MSB)
amsb = self._read_byte(self.ALTITUDE_AMSB)
alsb = self._read_byte(self.ALTITUDE_ALSB)
lsb = self._read_byte(self.ALTITUDE_LSB)
return _combine_altitude_bytes(msb, amsb, alsb, lsb)
@property
def status(self):
status_byte = self._read_byte(self.STATUS)
return GPSStatus(status_byte)
def __str__(self):
return "Current position - Lat: {}, Long: {}".\
format(self.latitude, self.longitude)
def _write_byte(self, register, value):
self._i2c_bus.write_byte_data(self.address, register, value)
def _read_byte(self, register):
data = np.uint8(self._i2c_bus.read_byte_data(self.address, register))
return data
def _configure(self):
pass
#https://www.pololu.com/file/download/LPS25H.pdf?file_id=0J761
from smbus import SMBus
busNum = 1
b = SMBus(busNum)
PTS = 0x5d #Device I2C slave address
PTS_WHOAMI = 0b1011101 #Device self-id
(chip_id, version) = b.read_i2c_block_data(PTS, 0xD0, 2)
print "Chip Id:", chip_id, "Version:", version
if b.read_byte_data(PTS, 0x0f) == PTS_WHOAMI:
print 'LPS25H detected successfully.'
else:
print 'No LPS25H detected on bus '+str(busNum)+'.'
class BrightPI:
BrightPiAddress = 0x70
AddressControl = 0x00
AddressIR1 = 0x01
AddressLED1 = 0x02
AddressIR2 = 0x03
AddressLED2 = 0x04
AddressLED3 = 0x05
AddressIR3 = 0x06
AddressLED4 = 0x07
AddressIR4 = 0x08
AddressAllLed = 0x09
maxBrightness = 0x32
def __init__(self, I2CPORT_value):
self.I2CPORT = I2CPORT_value
self.bus = SMBus(self.I2CPORT)
def read_state(self, address):
return self.bus.read_byte_data(self.BrightPiAddress, address)
def led_1_on(self):
mask = 0x02
result = self.read_state(self.AddressControl) | mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_1_off(self):
mask = ~0x02
result = self.read_state(self.AddressControl) & mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_1_brightness(self, level):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressLED1, level)
def led_2_on(self):
mask = 0x08
result = self.read_state(self.AddressControl) | mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_2_off(self):
mask = ~0x08
result = self.read_state(self.AddressControl) & mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_2_brightness(self, level):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressLED2, level)
def led_3_on(self):
mask = 0x10
result = self.read_state(self.AddressControl) | mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_3_off(self):
mask = ~0x10
result = self.read_state(self.AddressControl) & mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_3_brightness(self, level):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressLED3, level)
def led_4_on(self):
mask = 0x40
result = self.read_state(self.AddressControl) | mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_4_off(self):
mask = ~0x40
result = self.read_state(self.AddressControl) & mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_4_brightness(self, level):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressLED4, level)
def led_all_brightness(self, level):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressAllLed, level)
def led_all_on(self):
mask = 0x5a
result = self.read_state(self.AddressControl) | mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def led_all_off(self):
mask = ~0x5a
result = self.read_state(self.AddressControl) & mask
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, result)
def reset(self):
self.bus.write_byte_data(self.BrightPiAddress, self.AddressControl, 0x00)
from smbus import SMBus
import time
# Special Chars
deg = u'\N{DEGREE SIGN}'
# I2C Constants
ADDR = 0x60
CTRL_REG1 = 0x26
PT_DATA_CFG = 0x13
bus = SMBus(1)
who_am_i = bus.read_byte_data(ADDR, 0x0C)
print hex(who_am_i)
if who_am_i != 0xc4:
print "Device not active."
exit(1)
# Set oversample rate to 128
setting = bus.read_byte_data(ADDR, CTRL_REG1)
newSetting = setting | 0x38
bus.write_byte_data(ADDR, CTRL_REG1, newSetting)
# Enable event flags
bus.write_byte_data(ADDR, PT_DATA_CFG, 0x07)
# Toggel One Shot
setting = bus.read_byte_data(ADDR, CTRL_REG1)
if (setting & 0x02) == 0:
bus.write_byte_data(ADDR, CTRL_REG1, (setting | 0x02))
# Read sensor data
print "Waiting for data..."
status = bus.read_byte_data(ADDR,0x00)
while (status & 0x08) == 0:
#print bin(status)
status = bus.read_byte_data(ADDR,0x00)
time.sleep(0.5)
class templogger():
_cal_AC1 = 0
_cal_AC2 = 0
_cal_AC3 = 0
_cal_AC4 = 0
_cal_AC5 = 0
_cal_AC6 = 0
_cal_B1 = 0
_cal_B2 = 0
_cal_MB = 0
_cal_MC = 0
_cal_MD = 0
bmp180 = 0
bh1750 = 0
lm75addr = 0
i2c1 = 0
db = 0
api = 0
feed = 0
tempds = 0
lightds = 0
pressds = 0
def __init__(self, filename):
self.lm75addr = 0x4f
self.db = sqlite3.connect(filename)
self.db.execute("CREATE TABLE IF NOT EXISTS temp_series(date datetime, event TEXT, value REAL, detail TEXT)")
self.db.commit()
self.bmp180 = 0x77
self.bh1750 = 0x23
self.i2c1 = SMBus(1)
self.calibration()
#self.api = xively.XivelyAPIClient(api_key)
#self.feed = self.api.feeds.get(feed_id)
#self.tempds = self.get_datastream(self.feed, "Temperature")
#self.lightds = self.get_datastream(self.feed, "Light")
#self.pressds = self.get_datastream(self.feed, "Pressure")
def get_datastream(self, feed, name):
try:
datastream = feed.datastreams.get(name)
return datastream
except:
datastream = feed.datastreams.create(name, tags="")
return datastream
def publish(self, datastream, value, time):
datastream.current_value = value
datastream.at = time
try:
datastream.update()
except requests.HTTPError as e:
print "HTTPError({0}): {1}".format(e.errno, e.strerror)
def calibration(self):
self._cal_AC1 = self.read_16bit_regs(self.bmp180, 0xaa)
self._cal_AC2 = self.read_16bit_regs(self.bmp180, 0xac)
self._cal_AC3 = self.read_16bit_regs(self.bmp180, 0xae)
self._cal_AC4 = self.read_16bit_regu(self.bmp180, 0xb0)
self._cal_AC5 = self.read_16bit_regu(self.bmp180, 0xb2)
self._cal_AC6 = self.read_16bit_regu(self.bmp180, 0xb4)
self._cal_B1 = self.read_16bit_regs(self.bmp180, 0xb6)
self._cal_B2 = self.read_16bit_regs(self.bmp180, 0xb8)
self._cal_MB = self.read_16bit_regs(self.bmp180, 0xba)
self._cal_MC = self.read_16bit_regs(self.bmp180, 0xbc)
self._cal_MD = self.read_16bit_regs(self.bmp180, 0xbe)
def readRawTemp(self):
self.i2c1.write_byte_data(self.bmp180, 0xf4, 0x2e)
time.sleep(0.005)
raw = self.read_16bit_regu(self.bmp180, 0xf6)
return raw
def readTemperature(self):
ut = self.readRawTemp()
x1 = ((ut - self._cal_AC6) * self._cal_AC5) >> 15
x2 = (self._cal_MC << 11) / (x1 + self._cal_MD)
b5 = x1 + x2
temp = ((b5 + 8) >> 4)/10.0
return temp
def readRawPressure(self):
self.i2c1.write_byte_data(self.bmp180, 0xf4, 0xf4) #Read ultra high resolution
time.sleep(0.026)
msb = self.i2c1.read_byte_data(self.bmp180, 0xf6)
lsb = self.i2c1.read_byte_data(self.bmp180, 0xf7)
xlsb = self.i2c1.read_byte_data(self.bmp180, 0xf8)
raw = (msb << 16) + (lsb << 8) + (xlsb) >> 5
return raw
def readPressure(self):
# Get raw temperature and pressure
ut = self.readRawTemp()
up = self.readRawPressure()
# Convert to actual compensated and calibrated pressure (see Datasheet)
x1 = ((ut - self._cal_AC6) * self._cal_AC5) >> 15
x2 = (self._cal_MC << 11) / (x1 + self._cal_MD)
b5 = x1 + x2
b6 = b5 - 4000
x1 = (self._cal_B2 * (b6 * b6) >> 12) >> 11
x2 = (self._cal_AC2 * b6) >> 11
x3 = x1 + x2
b3 = (((self._cal_AC1 * 4 + x3) << 3) + 2) / 4
x1 = (self._cal_AC3 * b6) >> 13
x2 = (self._cal_B1 * ((b6 * b6) >> 12)) >> 16
x3 = ((x1 + x2) + 2) >> 2
b4 = (self._cal_AC4 * (x3 + 32768)) >> 15
b7 = (up - b3) * (50000 >> 3)
if (b7 < 0x80000000):
p = (b7 * 2) / b4
else:
p = (b7 / b4) * 2
x1 = (p >> 8) * (p >> 8)
x1 = (x1 * 3038) >> 16
x2 = (-7357 * p) >> 16
p = p + ((x1 + x2 + 3791) >> 4)
return p
def readLM75Temperature(self):
# Measure temperature
temp = self.i2c1.read_word_data(self.lm75addr,0)
# Swap lower and higher byte
temp = (((temp & 0xff) << 8)|((temp >> 8) & 0xff))
value = temp >> 5
# Make signed integer
if (temp & 0x8000):
# one's complement
value = (~value & 0x1FF)
# two's complement
value = value - 1
# significance: means negative temp
value = -value
value = value / 8.0
return value
def readBH1750Light(self):
# Measure light
luxtmp = self.i2c1.read_word_data(self.bh1750, 0x10)
# Convert to actual lux (see Datasheet)
lux = (((luxtmp >> 8) & 0xff) | ((luxtmp & 0xff) << 8))/1.2
return lux
def measure(self):
thisdate = datetime.datetime.utcnow()
nu = datetime.datetime.utcnow()
print(thisdate)
value = self.readLM75Temperature()
#self.publish(self.tempds, value, nu)
self.db.execute(
'INSERT INTO temp_series(date, event, value, detail) VALUES(?,?,?,?)',
(
thisdate,
"Temperature",
value,
"Green House"
)
)
self.db.commit()
lux = self.readBH1750Light()
#self.publish(self.lightds, lux, nu)
self.db.execute(
'INSERT INTO temp_series(date, event, value, detail) VALUES(?, ?, ?, ?)',
(
thisdate,
"Light intensity",
lux,
"Greenhouse"
)
)
self.db.commit()
pressure = self.readPressure()
#self.publish(self.pressds, pressure, nu)
self.db.execute(
'INSERT INTO temp_series(date, event, value, detail) VALUES(?, ?, ?, ?)',
(
thisdate,
"Pressure",
pressure,
"Greenhouse"
)
)
self.db.commit()
def print_climate(self):
print "The temperature is: %i Degrees Celsius" % (self.readLM75Temperature())
print "The light intensity is: %i Lux" % (self.readBH1750Light())
print "The pressure is: %i Pascal" % (self.readPressure())
def read_16bit_regu(self, address, register):
a = self.i2c1.read_byte_data(address, register)
b = self.i2c1.read_byte_data(address, register+1)
return ((a << 8) | b)
def read_16bit_regs(self, address, register):
a = self.i2c1.read_byte_data(address, register)
b = self.i2c1.read_byte_data(address, register+1)
c = (a << 8)|b
if (c & 0x8000):
c = (~c & 0xffff)
c = c-1
c = -c
return c
oldTime = time()
counter = 0
sorry = True
while 1:
newTime = time()
counter += newTime - oldTime
if newTime - sfxStartTime > sfxLength:
p.stop()
adc.write_byte(33, 128)
knob = adc.read_word_data(33, 0)
knob = ((knob & 15) << 8 | knob >> 8) - 683
if knob < 0: knob = 0
elif knob > 2730: knob = 2730
Player0Bat = HEIGHT - int(round(knob * (HEIGHT - Player0Height) / 2731.)) - Player0Height
if sorry:
knob = adc.read_byte_data(36,0) - 9
if knob < 0: knob = 0
elif knob > 220: knob = 220
Player1Bat = HEIGHT - int(round(knob * (HEIGHT - Player1Height) / 220.)) - Player1Height
gpio.output(17, 1)
gpio.output(17, 0)
sorry = not sorry
if Player0SizeCounter < 15: Player0SizeCounter += newTime - oldTime
else: Player0Height = 3
if Player1SizeCounter < 15: Player1SizeCounter += newTime - oldTime
else: Player1Height = 3
if Serving:
if ServeCount < 5:
BallY = Player0Bat + Player0Height / 2
BallX = 3
else:
MPL3115A2_CTRL_REG1_OS64 = 0x30
MPL3115A2_CTRL_REG1_OS128 = 0x38
MPL3115A2_CTRL_REG1_RAW = 0x40
MPL3115A2_CTRL_REG1_ALT = 0x80
MPL3115A2_CTRL_REG1_BAR = 0x00
MPL3115A2_CTRL_REG2 = (0x27)
MPL3115A2_CTRL_REG3 = (0x28)
MPL3115A2_CTRL_REG4 = (0x29)
MPL3115A2_CTRL_REG5 = (0x2A)
MPL3115A2_REGISTER_STARTCONVERSION = (0x12)
bus = SMBus(1)
whoami = bus.read_byte_data(MPL3115A2_ADDRESS, MPL3115A2_WHOAMI)
if whoami != 0xc4:
print "Device not active. "+str(whoami)
bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_CTRL_REG1,
MPL3115A2_CTRL_REG1_SBYB |
MPL3115A2_CTRL_REG1_OS128 |
MPL3115A2_CTRL_REG1_ALT)
bus.write_byte_data(
MPL3115A2_ADDRESS,
MPL3115A2_PT_DATA_CFG,
MPL3115A2_PT_DATA_CFG_TDEFE |
MPL3115A2_PT_DATA_CFG_PDEFE |
class MPL3115A2:
""" Class to read Air pressure """
# Control constants
_SLAVE_ADDR = 0x60
_CTRL_REG1 = 0x26
_PT_DATA_CFG = 0x13
_WHOAMI_REG = 0x0C
# Class variables
_device_initialised = 0
def __init__(self,device_number= 1):
"""Opens the i2c device (assuming that the kernel modules have been loaded)"""
self.bus = SMBus(device_number)
time.sleep(0.005)
try:
whoami = self.bus.read_byte_data(self._SLAVE_ADDR,self._WHOAMI_REG)
except:
raise i2cError("i2c error occurs...")
if whoami == 0xC4:
if DEBUG:
print("Successfull Device initialisation")
self._device_initialised = 1
self.initBarometer(self)
self.setActive(self)
else:
raise SensorError("Cannot configure the MPL3115A2")
@staticmethod
def setActive(self):
""" Set the device active """
ctrl_reg1 = self.bus.read_byte_data(self._SLAVE_ADDR,self._CTRL_REG1)
ctrl_reg1 = ctrl_reg1 | 0x01
self.bus.write_byte_data(self._SLAVE_ADDR,self._CTRL_REG1,ctrl_reg1)
@staticmethod
def setStandby(self):
""" Set the device in Standby mode """
self.bus.read_byte_data(self._SLAVE_ADDR,self._CTRL_REG1)
ctrl_reg1 = ctrl_reg1 & ~0x01
self.bus.write_byte_data(self._SLAVE_ADDR,self._CTRL_REG1,ctrl_reg1)
def getAirPressure(self):
""" Reads the air pressure in Pa """
if self._device_initialised:
tmp = self.bus.read_byte_data(self._SLAVE_ADDR,0x01) << 8
tmp_ = self.bus.read_byte_data(self._SLAVE_ADDR,0x02)
tmp = tmp + tmp_
tmp = tmp << 8
tmp = tmp + self.bus.read_byte_data(self._SLAVE_ADDR,0x03)
tmp_m = tmp >> 6
tmp_l = tmp & 0x30
pressure = tmp_m + tmp_l/246.0
if DEBUG:
print("Air Pressure[Pa] = ",pressure)
return pressure
def getTemperature(self):
""" Reads the temperature """
if self._device_initialised:
tmp = (self.bus.read_byte_data(self._SLAVE_ADDR,0x04)<< 8)
+ self.bus.read_byte_data(self._SLAVE_ADDR,0x05)
tmp_m =(tmp >> 8) & 0xFF
tmp_l = tmp & 0xFF
if tmp_m > 0x7F:
tmp_m -= 256.0;
return tmp_m + tmp_l/256.0
@staticmethod
def initBarometer(self):
""" Initialise the Barometer """
self.bus.write_byte_data(self._SLAVE_ADDR,0x26,0x38)
self.bus.write_byte_data(self._SLAVE_ADDR,0x27,0x00)
self.bus.write_byte_data(self._SLAVE_ADDR,0x28,0x11)
self.bus.write_byte_data(self._SLAVE_ADDR,0x29,0x00)
self.bus.write_byte_data(self._SLAVE_ADDR,0x2a,0x00)
self.bus.write_byte_data(self._SLAVE_ADDR,0x13,0x07)
if DEBUG:
print("Barometer successfully initialised")
class MyIMU(object):
b = False
busNum = 1
## LSM303D Registers --------------------------------------------------------------
LSM = 0x1d #Device I2C slave address
LSM_WHOAMI_ADDRESS = 0x0F
LSM_WHOAMI_CONTENTS = 0b1001001 #Device self-id
#Control register addresses -- from LSM303D datasheet
LSM_CTRL_0 = 0x1F #General settings
LSM_CTRL_1 = 0x20 #Turns on accelerometer and configures data rate
LSM_CTRL_2 = 0x21 #Self test accelerometer, anti-aliasing accel filter
LSM_CTRL_3 = 0x22 #Interrupts
LSM_CTRL_4 = 0x23 #Interrupts
LSM_CTRL_5 = 0x24 #Turns on temperature sensor
LSM_CTRL_6 = 0x25 #Magnetic resolution selection, data rate config
LSM_CTRL_7 = 0x26 #Turns on magnetometer and adjusts mode
#Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
LSM_MAG_X_LSB = 0x08 # x
LSM_MAG_X_MSB = 0x09
LSM_MAG_Y_LSB = 0x0A # y
LSM_MAG_Y_MSB = 0x0B
LSM_MAG_Z_LSB = 0x0C # z
LSM_MAG_Z_MSB = 0x0D
#Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
LSM_ACC_X_LSB = 0x28 # x
LSM_ACC_X_MSB = 0x29
LSM_ACC_Y_LSB = 0x2A # y
LSM_ACC_Y_MSB = 0x2B
LSM_ACC_Z_LSB = 0x2C # z
LSM_ACC_Z_MSB = 0x2D
#Registers holding 12-bit right justified, twos-complemented temperature data -- from LSM303D datasheet
LSM_TEMP_MSB = 0x05
LSM_TEMP_LSB = 0x06
# L3GD20H registers ----------------------------------------------------
LGD = 0x6b #Device I2C slave address
LGD_WHOAMI_ADDRESS = 0x0F
LGD_WHOAMI_CONTENTS = 0b11010111 #Device self-id
LGD_CTRL_1 = 0x20 #turns on gyro
LGD_CTRL_2 = 0x21 #can set a high-pass filter for gyro
LGD_CTRL_3 = 0x22
LGD_CTRL_4 = 0x23
LGD_CTRL_5 = 0x24
LGD_CTRL_6 = 0x25
LGD_TEMP = 0x26
#Registers holding gyroscope readings
LGD_GYRO_X_LSB = 0x28
LGD_GYRO_X_MSB = 0x29
LGD_GYRO_Y_LSB = 0x2A
LGD_GYRO_Y_MSB = 0x2B
LGD_GYRO_Z_LSB = 0x2C
LGD_GYRO_Z_MSB = 0x2D
#Kalman filter variables
Q_angle = 0.02
Q_gyro = 0.0015
R_angle = 0.005
y_bias = 0.0
x_bias = 0.0
XP_00 = 0.0
XP_01 = 0.0
XP_10 = 0.0
XP_11 = 0.0
YP_00 = 0.0
YP_01 = 0.0
YP_10 = 0.0
YP_11 = 0.0
KFangleX = 0.0
KFangleY = 0.0
def __init__(self):
from smbus import SMBus
self.b = SMBus(self.busNum)
self.detectTest()
#Set up the chips for reading ----------------------
self.b.write_byte_data(self.LSM, self.LSM_CTRL_1, 0b1010111) # enable accelerometer, 50 hz sampling
self.b.write_byte_data(self.LSM, self.LSM_CTRL_2, 0x00) #set +/- 2g full scale
self.b.write_byte_data(self.LSM, self.LSM_CTRL_5, 0b01100100) #high resolution mode, thermometer off, 6.25hz ODR
self.b.write_byte_data(self.LSM, self.LSM_CTRL_6, 0b00100000) # set +/- 4 gauss full scale
self.b.write_byte_data(self.LSM, self.LSM_CTRL_7, 0x00) #get magnetometer out of low power mode
self.b.write_byte_data(self.LGD, self.LGD_CTRL_1, 0x0F) #turn on gyro and set to normal mode
#Read data from the chips ----------------------
#while True:
# time.sleep(0.5)
# print self.readSensors()
self.newRead()
def twos_comp_combine(self, msb, lsb):
twos_comp = 256*msb + lsb
if twos_comp >= 32768:
return twos_comp - 65536
else:
return twos_comp
def detectTest(self):
#Ensure chip is detected properly on the bus ----------------------
if self.b.read_byte_data(self.LSM, self.LSM_WHOAMI_ADDRESS) == self.LSM_WHOAMI_CONTENTS:
print 'LSM303D detected successfully on I2C bus '+str(self.busNum)+'.'
else:
print 'No LSM303D detected on bus on I2C bus '+str(self.busNum)+'.'
if self.b.read_byte_data(self.LGD, self.LGD_WHOAMI_ADDRESS) == self.LGD_WHOAMI_CONTENTS:
print 'L3GD20H detected successfully on I2C bus '+str(self.busNum)+'.'
else:
print 'No L3GD20H detected on bus on I2C bus '+str(self.busNum)+'.'
def readSensors(self):
data = (self.readMagX(), self.readMagY(), self.readMagZ(), self.readAccX(), self.readAccY(), self.readAccZ(), self.readGyroX(), self.readGyroY(), self.readGyroZ())
return data
def readMagX(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_MAG_X_MSB), self.b.read_byte_data(self.LSM, self.LSM_MAG_X_LSB))
def readMagY(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_MAG_Y_MSB), self.b.read_byte_data(self.LSM, self.LSM_MAG_Y_LSB))
def readMagZ(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_MAG_Z_MSB), self.b.read_byte_data(self.LSM, self.LSM_MAG_Z_LSB))
def readAccX(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_ACC_X_MSB), self.b.read_byte_data(self.LSM, self.LSM_ACC_X_LSB))
def readAccY(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_ACC_Y_MSB), self.b.read_byte_data(self.LSM, self.LSM_ACC_Y_LSB))
def readAccZ(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LSM, self.LSM_ACC_Z_MSB), self.b.read_byte_data(self.LSM, self.LSM_ACC_Z_LSB))
def readGyroX(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LGD, self.LGD_GYRO_X_MSB), self.b.read_byte_data(self.LGD, self.LGD_GYRO_X_LSB))
def readGyroY(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LGD, self.LGD_GYRO_Y_MSB), self.b.read_byte_data(self.LGD, self.LGD_GYRO_Y_LSB))
def readGyroZ(self):
return self.twos_comp_combine(self.b.read_byte_data(self.LGD, self.LGD_GYRO_Z_MSB), self.b.read_byte_data(self.LGD, self.LGD_GYRO_Z_LSB))
def newRead(self):
import datetime
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly
AA = 0.40 # Complementary filter constant
gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0
kalmanX = 0.0
kalmanY = 0.0
a = datetime.datetime.now()
while True:
#Read the accelerometer,gyroscope and magnetometer values
ACCx = self.readAccX()
ACCy = self.readAccY()
ACCz = self.readAccZ()
GYRx = self.readGyroX()
GYRy = self.readGyroY()
GYRz = self.readGyroZ()
MAGx = self.readMagX()
MAGy = self.readMagY()
MAGz = self.readMagZ()
##Calculate loop Period(LP). How long between Gyro Reads
b = datetime.datetime.now() - a
a = datetime.datetime.now()
LP = b.microseconds/(1000000*1.0)
print "Loop Time | %5.2f|" % ( LP ),
#Convert Gyro raw to degrees per second
rate_gyr_x = GYRx * G_GAIN
rate_gyr_y = GYRy * G_GAIN
rate_gyr_z = GYRz * G_GAIN
#Calculate the angles from the gyro.
gyroXangle+=rate_gyr_x*LP
gyroYangle+=rate_gyr_y*LP
gyroZangle+=rate_gyr_z*LP
##Convert Accelerometer values to degrees
AccXangle = (math.atan2(ACCy,ACCz)+M_PI)*RAD_TO_DEG
AccYangle = (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG
####################################################################
######################Correct rotation value########################
####################################################################
#Change the rotation value of the accelerometer to -/+ 180 and
#move the Y axis '0' point to up.
#
#Two different pieces of code are used depending on how your IMU is mounted.
#If IMU is up the correct way, Skull logo is facing down, Use these lines
AccXangle -= 180.0
if AccYangle > 90:
AccYangle -= 270.0
else:
AccYangle += 90.0
#
#
#
#
#If IMU is upside down E.g Skull logo is facing up;
#if AccXangle >180:
# AccXangle -= 360.0
#AccYangle-=90
#if (AccYangle >180):
# AccYangle -= 360.0
############################ END ##################################
#Complementary filter used to combine the accelerometer and gyro values.
CFangleX=AA*(CFangleX+rate_gyr_x*LP) +(1 - AA) * AccXangle
CFangleY=AA*(CFangleY+rate_gyr_y*LP) +(1 - AA) * AccYangle
#Kalman filter used to combine the accelerometer and gyro values.
kalmanY = self.kalmanFilterY(AccYangle, rate_gyr_y,LP)
kalmanX = self.kalmanFilterX(AccXangle, rate_gyr_x,LP)
####################################################################
############################MAG direction ##########################
####################################################################
#If IMU is upside down, then use this line. It isnt needed if the
# IMU is the correct way up
#MAGy = -MAGy
#
############################ END ##################################
#Calculate heading
heading = 180 * math.atan2(MAGy,MAGx)/M_PI
#Only have our heading between 0 and 360
if heading < 0:
heading += 360
#Normalize accelerometer raw values.
accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
####################################################################
###################Calculate pitch and roll#########################
####################################################################
#Us these two lines when the IMU is up the right way. Skull logo is facing down
pitch = math.asin(accXnorm)
roll = -math.asin(accYnorm/math.cos(pitch))
#
#Us these four lines when the IMU is upside down. Skull logo is facing up
#accXnorm = -accXnorm #flip Xnorm as the IMU is upside down
#accYnorm = -accYnorm #flip Ynorm as the IMU is upside down
#pitch = math.asin(accXnorm)
#roll = math.asin(accYnorm/math.cos(pitch))
#
############################ END ##################################
#Calculate the new tilt compensated values
magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)
magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch)
#Calculate tilt compensated heading
tiltCompensatedHeading = 180 * math.atan2(magYcomp,magXcomp)/M_PI
if tiltCompensatedHeading < 0:
tiltCompensatedHeading += 360
if 1: #Change to '0' to stop showing the angles from the accelerometer
print ("\033[1;34;40mACCX Angle %5.2f ACCY Angle %5.2f \033[0m " % (AccXangle, AccYangle)),
if 1: #Change to '0' to stop showing the angles from the gyro
print ("\033[1;31;40m\tGRYX Angle %5.2f GYRY Angle %5.2f GYRZ Angle %5.2f" % (gyroXangle,gyroYangle,gyroZangle)),
if 1: #Change to '0' to stop showing the angles from the complementary filter
print ("\033[1;35;40m \tCFangleX Angle %5.2f \033[1;36;40m CFangleY Angle %5.2f \33[1;32;40m" % (CFangleX,CFangleY)),
if 1: #Change to '0' to stop showing the heading
print ("HEADING %5.2f \33[1;37;40m tiltCompensatedHeading %5.2f" % (heading,tiltCompensatedHeading)),
if 1: #Change to '0' to stop showing the angles from the Kalmin filter
print ("\033[1;31;40m kalmanX %5.2f \033[1;35;40m kalmanY %5.2f " % (kalmanX,kalmanY))
#slow program down a bit, makes the output more readable
time.sleep(0.03)
def kalmanFilterY (self, accAngle, gyroRate, DT):
y=0.0
S=0.0
'''
global KFangleY
global Q_angle
global Q_gyro
global y_bias
global XP_00
global XP_01
global XP_10
global XP_11
global YP_00
global YP_01
global YP_10
global YP_11
'''
self.KFangleY = self.KFangleY + DT * (gyroRate - self.y_bias)
self.YP_00 = self.YP_00 + ( - DT * (self.YP_10 + self.YP_01) + self.Q_angle * DT )
self.YP_01 = self.YP_01 + ( - DT * self.YP_11 )
self.YP_10 = self.YP_10 + ( - DT * self.YP_11 )
self.YP_11 = self.YP_11 + ( + self.Q_gyro * DT )
y = accAngle - self.KFangleY
S = self.YP_00 + self.R_angle
K_0 = self.YP_00 / S
K_1 = self.YP_10 / S
self.KFangleY = self.KFangleY + ( K_0 * y )
self.y_bias = self.y_bias + ( K_1 * y )
self.YP_00 = self.YP_00 - ( K_0 * self.YP_00 )
self.YP_01 = self.YP_01 - ( K_0 * self.YP_01 )
self.YP_10 = self.YP_10 - ( K_1 * self.YP_00 )
self.YP_11 = self.YP_11 - ( K_1 * self.YP_01 )
return self.KFangleY
def kalmanFilterX (self, accAngle, gyroRate, DT):
x=0.0
S=0.0
'''
global KFangleX
global Q_angle
global Q_gyro
global x_bias
global XP_00
global XP_01
global XP_10
global XP_11
global YP_00
global YP_01
global YP_10
global YP_11
'''
self.KFangleX = self.KFangleX + DT * (gyroRate - self.x_bias)
self.YP_00 = self.YP_00 + ( - DT * (self.YP_10 + self.YP_01) + self.Q_angle * DT )
self.YP_01 = self.YP_01 + ( - DT * self.YP_11 )
self.YP_10 = self.YP_10 + ( - DT * self.YP_11 )
self.YP_11 = self.YP_11 + ( + self.Q_gyro * DT )
x = accAngle - self.KFangleX
S = self.YP_00 + self.R_angle
K_0 = self.YP_00 / S
K_1 = self.YP_10 / S
self.KFangleX = self.KFangleX + ( K_0 * x )
self.x_bias = self.x_bias + ( K_1 * x )
self.YP_00 = self.YP_00 - ( K_0 * self.YP_00 )
self.YP_01 = self.YP_01 - ( K_0 * self.YP_01 )
self.YP_10 = self.YP_10 - ( K_1 * self.YP_00 )
self.YP_11 = self.YP_11 - ( K_1 * self.YP_01 )
return self.KFangleX
