Exemplo n.º 1
7
Arquivo: i2c.py Projeto: jcollie/rpiwr
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
Exemplo n.º 2
0
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
Exemplo n.º 3
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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))
Exemplo n.º 7
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
Exemplo n.º 8
0
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
Exemplo n.º 10
0
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
Exemplo n.º 12
0
 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")
Exemplo n.º 13
0
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
Exemplo n.º 14
0
Arquivo: imu.py Projeto: beckel/carpi
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);
Exemplo n.º 15
0
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
Exemplo n.º 16
0
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]
Exemplo n.º 17
0
    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
Exemplo n.º 18
0
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)
Exemplo n.º 19
0
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
Exemplo n.º 20
0
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
Exemplo n.º 21
0
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
Exemplo n.º 22
0
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
Exemplo n.º 23
0
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
Exemplo n.º 24
0
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()
Exemplo n.º 26
0
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
Exemplo n.º 27
0
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)
Exemplo n.º 28
0
class Magnetometer(RPCComponentThreaded):
    """
    HMC5883L I2C magnetometer
    """

    def main_prepare(self):
        self.bus = SMBus(self.Configuration['bus'])
        self.address = self.Configuration['address']
        self.bus.write_byte_data(self.address, 2, 0)
        self.values = [0, 0, 0]

    def mainthread(self):
        for i, offset in enumerate(range(3, 8, 2)):
            msb = self.bus.read_byte_data(self.address, offset)
            lsb = self.bus.read_byte_data(self.address, offset + 1)
            value = (msb << 8) + lsb
            if value >= 32768:
                value -= 65536
            self.values[i] = value
        for subscriber, func in self.subscribers.items():
            self.send(Message(sender=self.name, recipient=subscriber, func=func,
                              arg={'x': self.values[0], 'y': self.values[2], 'z': self.values[1]}),
                      "outbox")
        sleep(0.1)
Exemplo n.º 29
0
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.'
Exemplo n.º 30
0
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
Exemplo n.º 31
0
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
Exemplo n.º 32
0
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
Exemplo n.º 33
0
    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)
Exemplo n.º 34
0
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
Exemplo n.º 35
0
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)
Exemplo n.º 36
0
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)
Exemplo n.º 37
0
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()
Exemplo n.º 38
0
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)
Exemplo n.º 39
0
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)
Exemplo n.º 40
0
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"
Exemplo n.º 41
0
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()
Exemplo n.º 42
0
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()
Exemplo n.º 43
0
    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

Exemplo n.º 45
0
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
Exemplo n.º 46
0
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
Exemplo n.º 47
0
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)
Exemplo n.º 48
0
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
Exemplo n.º 49
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])
Exemplo n.º 50
0
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
Exemplo n.º 51
0
#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)+'.'
Exemplo n.º 52
0
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)
Exemplo n.º 54
0
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	
Exemplo n.º 55
0
Arquivo: main.py Projeto: gfsduog/prom
 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:
Exemplo n.º 56
0
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 |
Exemplo n.º 57
0
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")
Exemplo n.º 58
0
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