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