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