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
def get_temp():
# zlecenie konwersji
i2c_bus = SMBus(1)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 10, 1)
sleep(1)
cel = i2c_bus.read_word_data(0x20, 5)
cel = cel >> 8
return cel
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)
class PiGlowService(rpyc.Service): def on_connect(self): pass def on_disconnect(self): pass def exposed_init(self): self.bus = SMBus(1) self.bus.write_byte_data(0x54, 0x00, 0x01) self.bus.write_byte_data(0x54, 0x13, 0xFF) self.bus.write_byte_data(0x54, 0x14, 0xFF) self.bus.write_byte_data(0x54, 0x15, 0xFF) def exposed_colours(self, red, orange, yellow, green, blue, white): try: self.bus.write_i2c_block_data(0x54, 0x01, [red, orange, yellow, green, blue, green, red, orange, yellow, white, white, blue, white, green, blue, yellow, orange, red]) self.bus.write_byte_data(0x54, 0x16, 0xFF) except IOError: pass def exposed_all_off(self): try: self.bus.write_i2c_block_data(0x54, 0x01, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) self.bus.write_byte_data(0x54, 0x16, 0xFF) except IOError: pass
class i2cDevice:
def __init__(self, bus_number):
self.BC_addr = 0x25
self.bus = SMBus(bus_number)
def read_register(self, address):
self.bus.write_byte(self.BC_addr, address)
time.sleep(0.02)
data = struct.pack('B', self.bus.read_byte(self.BC_addr))
return data
def write_register(self, address, data):
self.bus.write_byte_data(self.BC_addr, address, data)
time.sleep(0.02)
def get(self):
lStatus = 'ok'
lArgs = self.__mParser.parse_args()
lBusId = int(lArgs['bus_id'], 0)
lAddress = int(lArgs['address'], 0)
lValue = int(lArgs['value'], 0)
lBus = SMBus(lBusId)
try:
if lArgs['cmd'] is None:
lBus.write_byte(lAddress, lValue)
else:
lCommand = int(lArgs['cmd'], 0)
lBus.write_byte_data(lAddress, lCommand, lValue)
except IOError, pExc:
lStatus = "Error writing data: " + str(pExc)
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 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 PCF8574(object):
class GPIO(object):
def __init__(self, expander, bit, inverted=False):
self.expander = expander
self.bit = bit
self.inverted = inverted
def on(self):
self.expander.write_bit(self.bit, not self.inverted)
def off(self):
self.expander.write_bit(self.bit, self.inverted)
def __init__(self, address, bus=1, byte=0):
self.address = address
self.bus = SMBus(bus)
self.byte = byte
self.data = 0xFF
def __del__(self):
self.write_byte(0xFF)
def commit(self):
self.bus.write_byte_data(self.address, self.byte, self.data)
def write_byte(self, data):
self.data = data
self.commit()
def write_bit(self, bit, value):
mask = 1 << bit
if value:
self.data |= mask
else:
self.data &= ~mask
self.commit()
def get_gpio(self, bit, inverted=False):
return self.GPIO(self, bit, inverted)
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
def heatProcI2C(cycle_time, duty_cycle, conn):
p = current_process()
print "Starting:", p.name, p.pid
bus = SMBus(0)
bus.write_byte_data(0x26, 0x00, 0x00) # set I/0 to write
while True:
while conn.poll(): # get last
cycle_time, duty_cycle = conn.recv()
conn.send([cycle_time, duty_cycle])
if duty_cycle == 0:
bus.write_byte_data(0x26, 0x09, 0x00)
time.sleep(cycle_time)
elif duty_cycle == 100:
bus.write_byte_data(0x26, 0x09, 0x01)
time.sleep(cycle_time)
else:
on_time, off_time = getonofftime(cycle_time, duty_cycle)
bus.write_byte_data(0x26, 0x09, 0x01)
time.sleep(on_time)
bus.write_byte_data(0x26, 0x09, 0x00)
time.sleep(off_time)
def reset():
i2c_bus = SMBus(1)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 0, 0)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 1, 0)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 2, 0)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 3, 0)
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 4, 255)
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 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)
def set_fun_speed(level):
i2c_bus = SMBus(1)
if level == 0:
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 4, 255)
if level == 1:
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 4, 125)
if level == 2:
i2c_bus.write_byte_data(Register.LIGHT2_ADDRESS, 4, 0)
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 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
class sgh_PCF8591P:
# Constructor
def __init__(self, busNum):
#print "init PCF8591"
if busNum == 0:
self.__bus = SMBus(0) # on a Rev 1 board
#print "bus 0"
else:
self.__bus = SMBus(1) # on a Rev 2 board
self.__addr = self.__checkI2Caddress(0x48)
self.__DACEnabled = 0x00
#print self.readADC() # dummy call to raise exception if no chip present on the i2c bus
#print "PCF8591 init completed"
# self.__bus = __i2cBus
# self.__addr = self.__checkI2Caddress(__addr)
# self.__DACEnabled = 0x00
# i2c = SMBus(0) # on a Rev 1 board
# # i2c = SMBus(1) # if on a Rev 2 board
# # Create a PCF8591P object
# sensor = PCF8591P(i2c, 0x48)
# Read single ADC Channel
def readADC(self, __chan = 0):
__checkedChan = self.__checkChannelNo(__chan)
self.__bus.write_byte(self.__addr, 0x40 | __checkedChan & 0x03) # mod my Max - says it more reliable
# self.__bus.write_byte(self.__addr, __checkedChan | self.__DACEnabled)
__reading = self.__bus.read_byte(self.__addr) # seems to need to throw away first reading
__reading = self.__bus.read_byte(self.__addr) # read A/D
return __reading
# Read all ADC channels
def readAllADC(self):
__readings = []
self.__bus.write_byte(self.__addr, 0x04 | self.__DACEnabled)
__reading = self.__bus.read_byte(self.__addr) # seems to need to throw away first reading
for i in range (4):
__readings.append(self.__bus.read_byte(self.__addr)) # read ADC
return __readings
# Set DAC value and enable output
def writeDAC(self, __val=0):
__checkedVal = self.__checkDACVal(__val)
self.__DACEnabled = 0x40
self.__bus.write_byte_data(self.__addr, self.__DACEnabled, __checkedVal)
# Enable DAC output
def enableDAC(self):
self.__DACEnabled = 0x40
self.__bus.write_byte(self.__addr, self.__DACEnabled)
# Disable DAC output
def disableDAC(self):
self.__DACEnabled = 0x00
self.__bus.write_byte(self.__addr, self.__DACEnabled)
# Check I2C address is within bounds
def __checkI2Caddress(self, __addr):
if type(__addr) is not int:
raise I2CaddressOutOfBoundsError
elif (__addr < 0):
raise I2CaddressOutOfBoundsError
elif (__addr > 127):
raise I2CaddressOutOfBoundsError
return __addr
# Check if ADC channel number is within bounds
def __checkChannelNo(self, __chan):
if type(__chan) is not int:
raise PCF8591PchannelOutOfBoundsError
elif (__chan < 0):
raise PCF8591PchannelOutOfBoundsError
elif (__chan > 3):
raise PCF8591PchannelOutOfBoundsError
return __chan
# Check if DAC output value is within bounds
def __checkDACVal(self, __val):
if type(__val) is not int:
raise PCF8591PDACvalueOutOfBoundsError
elif (__val < 0):
raise PCF8591PDACvalueOutOfBoundsError
elif (__val > 255):
raise PCF8591PDACvalueOutOfBoundsError
return __val
class DS3231:
"""Class to read/write time from/to DS3231"""
## Control constants
_SLAVE_ADDR = 0x68
_TIME_START_ADDR = 0x00
_TEMP_START_ADDR = 0x11
_REG_SECONDS = 0x00
_REG_MINUTES = 0x01
_REG_HOURS = 0x02
_REG_DAY = 0x03
_REG_DATE = 0x04
_REG_MONTH = 0x05
_REG_YEAR = 0x06
_REG_CONTROL = 0x07
def __init__(self, device_number = 1):
"""Opens the i2c device (assuming that the kernel modules have been
loaded) & run soft reset. (user register leaved to default value)"""
try:
self.bus = SMBus(device_number)
except i2cError:
#raise i2cError
pass
if DEBUG:
print("DS3231 init done.")
def getTime(self):
""" """
try:
data = self.bus.read_i2c_block_data(self._SLAVE_ADDR,0x00,7)
except i2cError:
#raise i2cError
pass
sec = self._bcd_to_int(data[0] & 0x7F)
min = self._bcd_to_int(data[1] & 0x3F)
hour = self._bcd_to_int(data[2])
day = self._bcd_to_int(data[3])
date = self._bcd_to_int(data[4])
month = self._bcd_to_int(data[5])
year = self._bcd_to_int(data[6])
if DEBUG:
print(datetime((21-1)*100 + year, month,date,hour,min,sec,0,tzinfo
= None))
#print(*data, sep='\t')
return datetime((21-1)*100 + year, \
month,date,hour,min,sec,0,tzinfo=None)
def setTime(self,timetuple):
"""
write Time/date Information to Ds3231
Range: seconds [0,59], minutes [0,59], hours [0,23],
day [0,7], date [1-31], month [1-12], year [0-99].
"""
if self.checkTimeDataValidity(timetuple)is True:
#write seconds
regSec = self._int_to_bcd(timetuple[5])
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_SECONDS,regSec)
#write minutes
regMin = self._int_to_bcd(timetuple[4])
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_MINUTES,regMin)
#write hours
regHr = self._int_to_bcd(timetuple[3])
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_HOURS,regHr)
#write Date
regDate = self._int_to_bcd(timetuple[2])
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_DATE,regDate)
#write month
regMonth = self._int_to_bcd(timetuple[1])
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_MONTH,regMonth)
#write year
regYr = self._int_to_bcd(timetuple[0]-2000)
self.bus.write_byte_data(self._SLAVE_ADDR,self._REG_YEAR,regYr)
def setTimeNow(self):
"""
Set time to datetime.now(). Return tuple needed by setTime()
"""
#TODO: Get a better function to check if the pi is online
if self.checkIfOnline() is not None:
tmp = datetime.now()
self.setTime((tmp.year,tmp.month, tmp.day,tmp.hour,tmp.minute,tmp.second ))
else:
raise ConnectionError("Please connect your Pi to the internet first...")
@staticmethod
def checkIfOnline():
""" Check if the pi is online """
try:
import requests
except ImportError:
print('Please make sure Python library requests is installed')
r = requests.get('https://api.github.com/events')
if r is True:
return True
else:
return False
@staticmethod
def checkTimeDataValidity(timetuple):
"""
Checks time data inputs
"""
if 2000 <= timetuple[0] <= 2099 and \
1 <= timetuple[1] <= 12 and \
1 <= timetuple[2] <= 31 and \
0 <= timetuple[3] <= 23 and \
0 <= timetuple[4] <= 59 and \
0 <= timetuple[5] <= 59:
return True
else:
return False
@staticmethod
def _bcd_to_int(bcd):
"""
Decode a 2x4bit BCD to a integer.
"""
out = 0
for d in (bcd >> 4, bcd):
for p in (1, 2, 4 ,8):
if d & 1:
out += p
d >>= 1
out *= 10
return int(out / 10)
@staticmethod
def _int_to_bcd(n):
"""
Encode a one or two digits number to the BCD.
"""
bcd = 0
for i in (n // 10, n % 10):
for p in (8, 4, 2, 1):
if i >= p:
bcd += 1
i -= p
bcd <<= 1
return bcd >> 1
class PyGlow:
def __init__(
self,
brightness=None, speed=None, pulse=None, pulse_dir=None,
i2c_bus=None):
if i2c_bus is None:
i2c_bus = self.get_i2c_bus()
# Enables the LEDs
self.bus = SMBus(i2c_bus)
# Tell the SN3218 to enable output
self.bus.write_byte_data(I2C_ADDR, EN_OUTPUT_ADDR, 0x01)
# Enable each LED arm
self.bus.write_byte_data(I2C_ADDR, EN_ARM1_ADDR, 0xFF)
self.bus.write_byte_data(I2C_ADDR, EN_ARM2_ADDR, 0xFF)
self.bus.write_byte_data(I2C_ADDR, EN_ARM3_ADDR, 0xFF)
# Set default brightness and pulsing params
self.brightness = brightness
self.speed = speed
self.pulse = pulse
self.pulse_dir = pulse_dir
# Define the LED state variable
self.__STATE = {'leds': {}, 'params': {}}
def get_i2c_bus(self):
"""Get which I2C bus will be used.
If RPi.GPIO is available, it will be used to exactly determine
which bus to use. If RPi.GPIO is not available, 0 is returned if
/dev/i2c-0 exists, otherwise 1.
"""
try:
import RPi.GPIO as rpi
except ImportError:
# RPi.GPIO isn't available. Just get the first available I2C
# bus, which usually works.
if os.path.exists("/dev/i2c-0"):
return 0
else:
return 1
else:
# Check what Raspberry Pi version we got
if rpi.RPI_REVISION == 1:
return 0
elif rpi.RPI_REVISION == 2 or rpi.RPI_REVISION == 3:
return 1
else:
raise PyGlowException(
self, "Unknown Raspberry Pi hardware revision: %s" %
(rpi.RPI_REVISION))
def led(
self,
led, brightness=None, speed=None, pulse=None, pulse_dir=None):
# Make it list if it's not a list
if isinstance(led, int):
led = [led]
# Light up the choosen LED
self.set_leds(led, brightness, speed, pulse, pulse_dir).update_leds()
def color(
self,
color, brightness=None, speed=None, pulse=None, pulse_dir=None):
leds = ()
# Check if an available color is choosen
if color in COLOR_LIST:
leds = COLOR_LED_LIST[color - 1]
elif color in COLOR_NAME_LIST:
leds = COLOR_LED_LIST[COLOR_NAME_LIST.index(color)]
else:
raise PyGlowException(
self, "Invalid color: %s. Color must be a number from 1 to 6 "
"or a name (%s)." % (color, ', '.join(COLOR_NAME_LIST)))
# Light up the choosen LEDs
self.set_leds(leds, brightness, speed, pulse, pulse_dir).update_leds()
def arm(
self,
arm, brightness=None, speed=None, pulse=None, pulse_dir=None):
leds = ()
# Check if an existing arm is choosen
if arm in ARM_LIST:
leds = ARM_LED_LIST[arm - 1]
else:
raise PyGlowException(
self, "Invalid arm number: %s. Arm must be a number from 1 to "
"3." % (arm))
# Light up the choosen LEDs
self.set_leds(leds, brightness, speed, pulse, pulse_dir).update_leds()
def all(self, brightness=None, speed=None, pulse=None, pulse_dir=None):
# Light up all LEDs
self.set_leds(
LED_LIST, brightness, speed, pulse, pulse_dir).update_leds()
def set_leds(
self,
leds, brightness=None, speed=None, pulse=None, pulse_dir=None):
p = self.__STATE['params']
# Store parameters value
if brightness is not None:
p['brightness'] = brightness
else:
brightness = self.brightness
if speed is not None:
p['speed'] = speed
if pulse is not None:
p['pulse'] = pulse
if pulse_dir is not None:
p['pulse_dir'] = pulse_dir
# Check brightness value
if not 0 <= brightness <= 255:
raise PyGlowException(
self, "Invalid brightness level: %s. Brightness level must be "
"a number from 0 to 255." % (brightness))
# Pick the gamma-corrected value
gc_value = GAMMA_TABLE[brightness]
for led in leds:
m = re.match('^([a-z]+)([1-3])$', str(led))
if m:
color = m.group(1)
arm = int(m.group(2))
color_index = None
# Check if the color has a valid name
if color in COLOR_NAME_LIST:
color_index = COLOR_NAME_LIST.index(color)
else:
raise PyGlowException(
self,
"Invalid color name: %s. Color name must be one of "
"the following: %s." %
(color, ', '.join(COLOR_NAME_LIST)))
# Check if the arm has a valid number
if arm in ARM_LIST:
led = LED_HEX_LIST[6 * (arm - 1) + 5 - color_index]
else:
raise PyGlowException(
self, "Invalid arm number: %s. Arm must be a number "
"from 1 to 3." % (arm))
elif (
isinstance(led, int) and
1 <= led <= 18):
led = LED_HEX_LIST[led - 1]
else:
raise PyGlowException(
self, "Invalid LED number %s. LED must be a number from 1 "
"to 18." % (led))
# Store the LED and brightness value
self.__STATE['leds'][led] = gc_value
return self
def update_leds(self):
p = self.__STATE['params']
# Set default parameters value
if 'brightness' not in p or p['brightness'] is None:
p['brightness'] = self.brightness
if 'speed' not in p or p['speed'] is None:
p['speed'] = self.speed
if 'pulse' not in p or p['pulse'] is None:
p['pulse'] = self.pulse
if 'pulse_dir' not in p or p['pulse_dir'] is None:
p['pulse_dir'] = self.pulse_dir
# Decide whether to pulse or just to light up
if p['brightness'] > 0 and p['pulse']:
self.__pulse(
self.__STATE['leds'].keys(),
p['brightness'], p['speed'], p['pulse_dir'])
else:
self.__write_data(self.__STATE['leds'].items())
# Reset the SET variable
self.__STATE = {'leds': {}, 'params': {}}
def __pulse(self, leds, brightness, speed, pulse_dir):
# Check speed value
if speed < 1:
raise PyGlowException(
self, "Invalid speed: %s. Speed must be a positive non-zero "
"number." % (speed))
# Choose pulsation style
elif pulse_dir == UP:
self.__pulse_loop(leds, 0, brightness, speed, UP)
elif pulse_dir == DOWN:
self.__pulse_loop(leds, brightness, 0, speed, DOWN)
else:
self.__pulse_loop(leds, 0, brightness, speed / 2, UP)
self.__pulse_loop(leds, brightness, 0, speed / 2, DOWN)
def __pulse_loop(self, leds, b_from, b_to, speed, direction):
# Bigger from the pair
b_max = float(max([b_from, b_to]))
# Starting value
b_val = float(b_from)
# Maximum steps per second
s_max = 20.0
# Compensation constant
# (the comp_const velue is a compensation of the delay of the other
# commands in the loop - it's influenced by the s_max value)
comp_const = 1030.0
# Number of steps of the loop
steps = speed / 1000.0 * s_max
# Default increment value
inc_val = b_max / steps
# Step up the brightness
for n in range(1, int(steps) + 1):
# Calculate new brightness value
b_val += inc_val * direction
# Round the final brightness value to the desired value
if n == int(steps) or b_val < 0:
b_val = b_to
# Set the brightness
self.__write_data(
tuple(
(n, GAMMA_TABLE[int(b_val)])
for n in self.__STATE['leds'].keys()))
# Sleep for certain period
sleep(speed / steps / comp_const)
def __write_data(self, leds):
# Set LED brightness
for led, brightness in leds:
self.bus.write_byte_data(
I2C_ADDR,
int(led),
brightness)
# Light up all chosen LEDs
self.bus.write_byte_data(I2C_ADDR, UPD_PWM_ADDR, 0xFF)
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
# 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)
print "Reading sensor data..."
p_data = bus.read_i2c_block_data(ADDR,0x01,3)
class Sonar:
'Class to interact with the sonar'
def __init__(self, address, sonar_node_nb, rate, max_range):
'Check that the sonar sensor is working before creating an object'
self.max_range = max_range
self._i2c = SMBus(1)
self._address = 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 = 28 # cm
#This is mostly for debugging and testing
self.num_bursts_sent = 0
# We want to check how often we get exceptions
self.error_counter = 0
self._ros_rate = rospy.Rate(rate)
self.node_state = State.Not_read
self.dist_to_obj = 0
self.was_in_semi_auto = True
#check that the sensor is present - read the version
self.verion = self._read_version()
def start_init(self):
# rospy.loginfo("The address used for sonar_" + sonar_node_nb + " is :" + str(self._address))
rospy.loginfo("Starting node sonar_" + sonar_node_nb)
rospy.loginfo("Checking if the sonar nb " + sonar_node_nb +
" is working...")
failure_count = 0
for tick in range(rate): # initialization phase during 1s,
distance, error = self.update()
# rospy.loginfo("The distance is :" + str(distance))
if error == SENSOR_ERROR_LOW_RANGE or error == SENSOR_ERROR_HIGH_RANGE:
failure_count += 1
self._ros_rate.sleep()
if failure_count > 10:
raise Faulty_sonar
def process_data(self, pub):
'Reads the data from the sonar_sensor and sends it on its topic'
global values
med = 0
#values = np.zeros(5)
msg = sonar_msg()
if self.node_state == State.Semi_automatic:
rospy.logwarn("The semi-automatic mode is now active")
self.was_in_semi_auto = True
while (not rospy.is_shutdown()) and (self.node_state
== State.Semi_automatic):
distance, error = self.update()
#rospy.loginfo("Distance: " + str(distance) + " ERROR: " + str(error))
if error == SENSOR_ERROR_GOOD_RANGE: #Take the value only if it's in the good range
values = np.roll(values, 1)
values[0] = float(distance)
med = np.median(values)
#rospy.loginfo(values)
if error == SENSOR_ERROR_LOW_RANGE:
raise Faulty_sonar()
elif error == SENSOR_ERROR_HIGH_RANGE:
self.dist_to_obj = 0
msg.dist_to_obj = self.dist_to_obj
# rospy.loginfo("sonar_" + sonar_node_nb + " talking : distance to obstacle : OUT OF RANGE")
pub.publish(msg)
else:
self.dist_to_obj = med
msg.dist_to_obj = med
# rospy.loginfo("sonar_" + sonar_node_nb + " talking : distance to obstacle : " + str(med))
pub.publish(msg)
self._ros_rate.sleep()
def read_topic_user_mode(self, msg):
if msg.resend_data == 1:
to_msg = sonar_msg()
to_msg.dist_to_obj = self.dist_to_obj
pub.publish(to_msg)
else:
if msg.semi_automatic == 0:
self.node_state = State.Manual
else:
self.node_state = State.Semi_automatic
# 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()
if self._waiting_for_echo:
# make sure we wait at least some amount of time before we read
if time_since_last_burst > 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 > 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 > 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._address, 0, 0x50
) #0x51 in cm (Give stange values don't know why) / 0x50 in inches
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._address, 2) / 255 * 2.54
self.mindistance = self._i2c.read_word_data(self._address,
4) / 255 * 2.54
# 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._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 SENSOR_ERROR_LOW_RANGE
# good distance, nice measurement
elif distance <= MAX_RANGE:
return SENSOR_ERROR_GOOD_RANGE
# object too far
else:
return SENSOR_ERROR_HIGH_RANGE
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])
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
GYO_Y_LSB = 0x2A # y
GYO_Y_MSB = 0x2B
GYO_Z_LSB = 0x2C # z
GYO_Z_MSB = 0x2D
if b.read_byte_data(LSM, 0x0f) == LSM_WHOAMI_LSM303D:
print 'LSM303D detected successfully.'
else:
print 'No LSM303D detected on bus ' + str(busNum) + '.'
if b.read_byte_data(LSM, 0x0f) == LSM_WHOAMI_L3GD20H:
print 'L3GD20H detected successfully...'
else:
print 'Error detecting L3GD20H on bus ' + str(busNum) + '... '
b.write_byte_data(LSM, CTRL_1,
0b01010111) # enable accelerometer, 50 hz sampling
b.write_byte_data(
LSM, CTRL_2,
0b00011000) #set +/- 2g full scale. 0x00 is 2g,0x04 should be 16g?
b.write_byte_data(
LSM, CTRL_5,
0b01100100) #high resolution mode, thermometer off, 6.25hz ODR
b.write_byte_data(LSM, CTRL_6, 0b01000000) # set +/- 4 gauss full scale
b.write_byte_data(LSM, CTRL_7,
0b00000000) #get magnetometer out of low power mode
b.write_byte_data(LSM_GYO, CTRL_1,
0b00001111) # normal mode with XYZ enable
#getting the initial value of accleration first
oaccx = twos_comp_combine_acc(b.read_byte_data(LSM, ACC_X_MSB),
b.read_byte_data(LSM, ACC_X_LSB),
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"
# except:
# print "Unknown error occured"
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.connect('10.11.0.26', 18088, 60)
# Blocking call that processes network traffic, dispatches callbacks and
# handles reconnecting.
# Other loop*() functions are available that give a threaded interface and a
# manual interface.
b2 = SMBus(2)
b2.write_byte_data(0x23, 0x03,0x0)
client.loop_start()
# client.loop_forever()
class sgh_PCF8591P:
# Constructor
def __init__(self, busNum):
#print( "init PCF8591" )
if busNum == 0:
self.__bus = SMBus(0) # on a Rev 1 board
#print "bus 0"
else:
self.__bus = SMBus(1) # on a Rev 2 board
self.__addr = self.__checkI2Caddress(0x48)
self.__DACEnabled = 0x00
#print self.readADC() # dummy call to raise exception if no chip present on the i2c bus
#print "PCF8591 init completed"
# self.__bus = __i2cBus
# self.__addr = self.__checkI2Caddress(__addr)
# self.__DACEnabled = 0x00
# i2c = SMBus(0) # on a Rev 1 board
# # i2c = SMBus(1) # if on a Rev 2 board
# # Create a PCF8591P object
# sensor = PCF8591P(i2c, 0x48)
# Read single ADC Channel
def readADC(self, __chan=0):
__checkedChan = self.__checkChannelNo(__chan)
self.__bus.write_byte(
self.__addr,
0x40 | __checkedChan & 0x03) # mod my Max - says it more reliable
# self.__bus.write_byte(self.__addr, __checkedChan | self.__DACEnabled)
__reading = self.__bus.read_byte(
self.__addr) # seems to need to throw away first reading
__reading = self.__bus.read_byte(self.__addr) # read A/D
return __reading
# Read all ADC channels
def readAllADC(self):
__readings = []
self.__bus.write_byte(self.__addr, 0x04 | self.__DACEnabled)
__reading = self.__bus.read_byte(
self.__addr) # seems to need to throw away first reading
for i in range(4):
__readings.append(self.__bus.read_byte(self.__addr)) # read ADC
return __readings
# Set DAC value and enable output
def writeDAC(self, __val=0):
__checkedVal = self.__checkDACVal(__val)
self.__DACEnabled = 0x40
self.__bus.write_byte_data(self.__addr, self.__DACEnabled,
__checkedVal)
# Enable DAC output
def enableDAC(self):
self.__DACEnabled = 0x40
self.__bus.write_byte(self.__addr, self.__DACEnabled)
# Disable DAC output
def disableDAC(self):
self.__DACEnabled = 0x00
self.__bus.write_byte(self.__addr, self.__DACEnabled)
# Check I2C address is within bounds
def __checkI2Caddress(self, __addr):
if type(__addr) is not int:
raise I2CaddressOutOfBoundsError
elif (__addr < 0):
raise I2CaddressOutOfBoundsError
elif (__addr > 127):
raise I2CaddressOutOfBoundsError
return __addr
# Check if ADC channel number is within bounds
def __checkChannelNo(self, __chan):
if type(__chan) is not int:
raise PCF8591PchannelOutOfBoundsError
elif (__chan < 0):
raise PCF8591PchannelOutOfBoundsError
elif (__chan > 3):
raise PCF8591PchannelOutOfBoundsError
return __chan
# Check if DAC output value is within bounds
def __checkDACVal(self, __val):
if type(__val) is not int:
raise PCF8591PDACvalueOutOfBoundsError
elif (__val < 0):
raise PCF8591PDACvalueOutOfBoundsError
elif (__val > 255):
raise PCF8591PDACvalueOutOfBoundsError
return __val
def __init__(self, name, i2c_addr):
self.I2Caddr = i2c_addr
directory = "/tmp/" + name
if not os.path.exists(directory):
os.makedirs(directory)
self.FptrVoc = directory + "/voc"
self.FptrBase = directory + "/base"
self.FptrError = directory + "/error"
# to retreave RH values for compensation
self.FptrTemp = "/tmp/HTU21D/tc"
self.FptrHumid = "/tmp/HTU21D/rh"
# for maintaining the moving averages
self.voc_buff = [0] * CCS811_MAX
self.voc_ptr = 0
# for tracking baseline value changes
self.newcal = 0
self.oldcal = 0
# to track RH compenation event
self.comp_minute = CCS811_T_MAX
# Long term T & RH averaging
self.temp_buff = [23] * CCS811_T_MAX
self.rh_buff = [50] * CCS811_T_MAX
bus = SMBus(I2CBUS)
resp = bus.read_byte_data(self.I2Caddr, CCS811_REG_STATUS)
print "ccs811.init() status reg: 0x%02x" % resp
if resp & CCS811_APP_VALID:
bus.write_byte(self.I2Caddr, CCS811_APP_START)
print "ccs811.init() starting application"
else:
print "ccs811.init() failed to start application"
bus.close()
sys.exit(10)
time.sleep(1)
# Start making measurements
bus.write_byte_data(self.I2Caddr, CCS811_REG_MEAS_MODE,
CCS811_MEAS_MODE_ON)
resp = bus.read_byte_data(self.I2Caddr, CCS811_REG_ERROR)
if resp:
print "ccs811.init() error reg: 0x%02x" % resp
sys.exit(11)
# Display CCS811 information on startup
resp = bus.read_byte_data(self.I2Caddr, CCS811_HW_ID)
print "ccs811.inint() HW ID: 0x%02x" % resp
resp = bus.read_byte_data(self.I2Caddr, CCS811_VER_HW)
print "ccs811.inint() HW Ver: 0x%02x" % resp
resp = bus.read_i2c_block_data(self.I2Caddr, CCS811_VER_APP, 2)
major = resp[0] & 0xf0 >> 4
minor = resp[0] & 0x0f
trivial = resp[1]
print "css811.init() app ver: %d.%d.%d" % (major, minor, trivial)
resp = bus.read_byte_data(self.I2Caddr, CCS811_REG_ERROR)
if resp:
print "ccs811.init() error reg: 0x%02x" % resp
sys.exit(12)
# Init envronmental compensation registers
temp_c = 23
r_humidity = 50
self.set_comp(temp_c, r_humidity)
# will implement periodically in loop
#print "CCS811 init complete"
bus.close()
0 # Bit 7 NOT CONNECTED on Bonnet
]
addr = 0x20 # I2C Address of MCP23017
irqPin = 26 # IRQ pin for MCP23017
os.system("sudo modprobe uinput")
ui = UInput({e.EV_KEY: key}, name="retrogame", bustype=e.BUS_USB)
bus = SMBus(3)
IODIR = 0x00
IOCON = 0x05
INTCAP = 0x08
# Initial MCP23008 config:
bus.write_byte_data(addr, IOCON, 0x44) # seq, OD IRQ
# Read/modify/write remaining MCP23008 config:
cfg = bus.read_i2c_block_data(addr, IODIR, 7)
cfg[0] = 0xF # Input bits
cfg[1] = 0x0 # Polarity
cfg[2] = 0xF # Interrupt pins
cfg[6] = 0xF # Pull-ups
bus.write_i2c_block_data(addr, IODIR, cfg)
# Clear interrupt by reading INTCAP and GPIO registers
x = bus.read_i2c_block_data(addr, INTCAP, 2)
#oldState = x[2] | (x[3] << 8)
oldState = x[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 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)
class PyGlow:
def __init__(self, i2c_bus=1):
self.bus = SMBus(i2c_bus)
self.current = self._zerolist()
def init(self):
self.write_i2c(CMD_ENABLE_OUTPUT, 0x01)
self.write_i2c(CMD_ENABLE_LEDS, [0xFF, 0xFF, 0xFF])
self.all_off()
def write_i2c(self, reg, value):
if not isinstance(value, list):
value = [value]
self.bus.write_i2c_block_data(I2C_ADDR, reg, value)
def update_leds(self, values):
self.write_i2c(CMD_SET_PWM_VALUES, values)
self.update()
def update(self):
self.bus.write_byte_data(I2C_ADDR, CMD_UPDATE, 0xFF)
def set(self, leds, intensity=DEFAULT_INTENSITY):
if not isinstance(leds, list):
leds = [leds]
for led in leds:
self.bus.write_byte_data(I2C_ADDR, LED_ADDRS[led], intensity)
def all_off(self):
self.current = self._zerolist()
self.update_leds(self.current)
def turn_off(self, leds):
self.set(leds, 0x00)
self.update()
def light(self, leds, intensity=DEFAULT_INTENSITY):
if not isinstance(leds, list):
leds = [leds]
self.all_off()
self.set(leds, intensity)
self.update()
def fade_in(self, leds, intensity=DEFAULT_INTENSITY,
speed=DEFAULT_FADE_SPEED, step=DEFAULT_FADE_STEP):
cur_inten = 0x00
while cur_inten <= intensity:
self.light(leds, cur_inten)
cur_inten += step
if cur_inten > 0xFF:
break
time.sleep(speed)
def fade_out(self, leds, intensity=DEFAULT_INTENSITY,
speed=DEFAULT_FADE_SPEED, step=DEFAULT_FADE_STEP):
while intensity >= 0x00:
self.light(leds, intensity)
intensity -= step
if intensity < 0x00:
self.turn_off(leds)
break
time.sleep(speed)
def crossfade(self, leds_start, leds_end, intensity=DEFAULT_INTENSITY,
speed=DEFAULT_FADE_SPEED, step=DEFAULT_FADE_STEP):
cur_inten_up = 0x00
cur_inten_down = intensity
while cur_inten_up < intensity:
cur_inten_down -= step
cur_inten_up += step
if cur_inten_down <= 0x00:
self.turn_off(leds_start)
break
if cur_inten_up >= 0xFF:
self.set(leds_end, 0xFF)
self.update()
break
self.set(leds_start, cur_inten_down)
self.set(leds_end, cur_inten_up)
self.update()
time.sleep(speed)
def _zerolist(self):
return [0x00 for x in range(18)]
class I2C(object):
"""
I2C Wrapper.
Provides helper methods for SMBus class used on a Raspberry Pi.
"""
# Base Methods
def __init__(self, bus_id=1):
"""
Initialize the I2C bus.
"""
self._i2c = SMBus(bus_id)
def __del__(self):
"""
Clean up.
"""
try:
del self._i2c
except:
pass
# Reading and Writing Registers
def write_register(self, address, register, value):
"""
Write a single byte to a I2C register. Return the value the
register had before the write.
:param address: The I2C address of the device.
:type address: int
:param register: The register of the device.
:type register: int
:param value: The value to write.
:type value: int
"""
self._i2c.write_byte_data(address, register, value)
def read_register(self, address, register):
"""
Read a single I2C register.
:param address: The I2C address of the device.
:type address: int
:param register: The register of the device.
:type register: int
:return: The value of the register.
:rtype: int
"""
return self._i2c.read_byte_data(address, register)
# Getting Values
def get_unsigned_value_from_bytes(self, low_byte, high_byte):
"""
Combine low and high bytes to an unsigned 16 bit value.
:param low_byte: The low byte of a 16 bit value.
:type low_byte: int
:param high_byte: The high byte of a 16 bit value.
:type high_byte: int
:return: An unsigned value from two bytes.
:rtype: int
"""
return ((high_byte & 0x00FF) << 8) | (low_byte & 0x00FF)
def get_signed_value_from_bytes(self, low_byte, high_byte):
"""
Combine low and high bytes to an signed 16 bit value.
:param low_byte: The low byte of a 16 bit value.
:type low_byte: int
:param high_byte: The high byte of a 16 bit value.
:type high_byte: int
:return: A signed value from two bytes.
:rtype: int
"""
unsigned_value = self.get_unsigned_value_from_bytes(low_byte, high_byte)
return self.twos_complement(unsigned_value)
def twos_complement(self, value, bits=16):
"""
Compute the 2's complement of a given value
:param value: The value.
:type value: int
:param bits: The number of bits of the value. Defaulted to 16 bits.
:type bits: int
:return: The two's complement of a given value.
:rtype: int
"""
if (value & (1 << (bits - 1))) != 0:
value = value - (1 << bits)
return value
# Reading 3D Sensor
def read_sensor(self, address, registers):
"""
Read vector of the given sensor.
:param address: The address of the device.
:type address: int
:param registers: A list of registers to read.
:type registers: [int]
:return: A vector representing the device measurement.
:rtype: [int]
"""
# Read all the registers
x_low = self.read_register(address, registers[0])
x_hi = self.read_register(address, registers[1])
y_low = self.read_register(address, registers[2])
y_hi = self.read_register(address, registers[3])
z_low = self.read_register(address, registers[4])
z_hi = self.read_register(address, registers[5])
# Combine the low and high bytes into signed numbers
x_val = self.get_signed_value_from_bytes(x_low, x_hi)
y_val = self.get_signed_value_from_bytes(y_low, y_hi)
z_val = self.get_signed_value_from_bytes(z_low, z_hi)
return [x_val, y_val, z_val]
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()
stopbits=serial.STOPBITS_ONE,
# interCharTimeout = 0.2,
timeout=1.0,
xonxoff=False,
rtscts=False,
dsrdtr=False)
i2c0 = SMBus(0)
i2c2 = SMBus(2)
print(icsp_cmd(ser, b'Z')) # tristate MCLR (icsp)
if sel == "A": # take A_!RST low
i2c2.write_byte(0x70, 0x5) # steer mux
ioa = i2c0.read_byte_data(0x23, 0x14)
i2c0.write_byte_data(0x23, 0x14, ioa & ~0x10)
elif sel == "B": # take B_!RST low
i2c2.write_byte(0x70, 0x4) # steer mux
iob = i2c0.read_byte_data(0x22, 0x14)
i2c0.write_byte_data(0x22, 0x14, iob & ~0x10)
print(icsp_cmd(ser, b'L')) # take MCLR low (icsp)
print(icsp_cmd(ser, b'#', 9)) # reset checksum
# print(icsp_cmd(ser, b'%', 5)) # reset stats
icsp_cmd(ser, b'[^]', 0) # enter LVP
icsp_cmd(ser, b'[X0=]', 0) # switch to config mem
# print(icsp_cmd(ser, b'%', 5)) # check stats
# print(icsp_cmd(ser, b'[R?+R?+]', 8))
class TemperaturePressure:
address = 0x77
oversampling = 3 # 0..3
def __init__(self):
# 0 for R-Pi Rev. 1, 1 for Rev. 2
self.bus = SMBus(1)
# Read whole calibration EEPROM data
cal = self.bus.read_i2c_block_data(self.address, 0xAA, 22)
# Convert byte data to word values
self.ac1 = get_short(cal, 0)
self.ac2 = get_short(cal, 2)
self.ac3 = get_short(cal, 4)
self.ac4 = get_ushort(cal, 6)
self.ac5 = get_ushort(cal, 8)
self.ac6 = get_ushort(cal, 10)
self.b1 = get_short(cal, 12)
self.b2 = get_short(cal, 14)
self.mb = get_short(cal, 16)
self.mc = get_short(cal, 18)
self.md = get_short(cal, 20)
return
def read(self):
# temperature conversion
self.bus.write_byte_data(self.address, 0xF4, 0x2E)
sleep(0.005)
(msb, lsb) = self.bus.read_i2c_block_data(self.address, 0xF6, 2)
ut = (msb << 8) + lsb
# pressure conversion
self.bus.write_byte_data(self.address, 0xF4, 0x34 + (self.oversampling << 6))
sleep(0.04)
(msb, lsb, xsb) = self.bus.read_i2c_block_data(self.address, 0xF6, 3)
up = ((msb << 16) + (lsb << 8) + xsb) >> (8 - self.oversampling)
# calculate temperature
x1 = ((ut - self.ac6) * self.ac5) >> 15
x2 = (self.mc << 11) / (x1 + self.md)
b5 = x1 + x2
t = (b5 + 8) >> 4
# calculate pressure
b6 = b5 - 4000
b62 = b6 * b6 >> 12
x1 = (self.b2 * b62) >> 11
x2 = self.ac2 * b6 >> 11
x3 = x1 + x2
b3 = (((self.ac1 * 4 + x3) << self.oversampling) + 2) >> 2
x1 = self.ac3 * b6 >> 13
x2 = (self.b1 * b62) >> 16
x3 = ((x1 + x2) + 2) >> 2
b4 = (self.ac4 * (x3 + 32768)) >> 15
b7 = (up - b3) * (50000 >> self.oversampling)
p = (b7 * 2) / b4
x1 = (p >> 8) * (p >> 8)
x1 = (x1 * 3038) >> 16
x2 = (-7357 * p) >> 16
p = p + ((x1 + x2 + 3791) >> 4)
return (t/10.0, p / 100)
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 = 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, 0x50
) #0x51 in cm (Give stange values don't know why) / 0x50 in inches
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 * 2.54
self.mindistance = self._i2c.read_word_data(self._i2c_address,
4) / 255 * 2.54
# 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
# Read whole calibration EEPROM data cal = bus.read_i2c_block_data(addr, 0xAA, 22) # Convert byte data to word values ac1 = get_short(cal, 0) ac2 = get_short(cal, 2) ac3 = get_short(cal, 4) ac4 = get_ushort(cal, 6) ac5 = get_ushort(cal, 8) ac6 = get_ushort(cal, 10) b1 = get_short(cal, 12) b2 = get_short(cal, 14) mb = get_short(cal, 16) mc = get_short(cal, 18) md = get_short(cal, 20) #print "Starting temperature conversion..." bus.write_byte_data(addr, 0xF4, 0x2E) sleep(0.005) (msb, lsb) = bus.read_i2c_block_data(addr, 0xF6, 2) ut = (msb << 8) + lsb #print "Starting pressure conversion..." bus.write_byte_data(addr, 0xF4, 0x34 + (oversampling << 6)) sleep(0.04) (msb, lsb, xsb) = bus.read_i2c_block_data(addr, 0xF6, 3) up = ((msb << 16) + (lsb << 8) + xsb) >> (8 - oversampling) #print "Calculating temperature..." x1 = ((ut - ac6) * ac5) >> 15 x2 = (mc << 11) / (x1 + md) b5 = x1 + x2 t = (b5 + 8) >> 4 #print "Calculating pressure..." b6 = b5 - 4000
spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI) cs5 = digitalio.DigitalInOut(board.D5) cs6 = digitalio.DigitalInOut(board.D6) cs13 = digitalio.DigitalInOut(board.D13) mcp5 = MCP.MCP3008(spi, cs5) mcp6 = MCP.MCP3008(spi, cs6) mcp13 = MCP.MCP3008(spi, cs13) # INITIALIZE IMUs # channel = 1 imu_address_1 = 0x69 imu_address_2 = 0x68 bus = SMBus(channel) bus.write_byte_data(imu_address_1, 0x06, 0x01) bus.write_byte_data(imu_address_2, 0x06, 0x01) time.sleep(0.5) accel_x_1 = -1 accel_y_1 = -1 accel_z_1 = -1 accel_x_2 = -1 accel_y_2 = -1 accel_z_2 = -1 gyro_x_1 = -1 gyro_y_1 = -1 gyro_z_1 = -1 gyro_x_2 = -1
