def __init__(self, bus, address, name, fs_scale=FS_250):
'''
Constructor
'''
self.bus = bus
self.address = address
self.name = name
self.fs_scale = fs_scale
self.gyro_raw_x = 0
self.gyro_raw_y = 0
self.gyro_raw_z = 0
self.gyro_scaled_x = 0
self.gyro_scaled_y = 0
self.gyro_scaled_z = 0
self.raw_temp = 0
self.scaled_temp = 0
# Wake up the deice and get output for each of the three axes,X, Y & Z
I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG1,
0b00001111)
# Select Big endian so we can use existing I2C library and include the scaling
ctrl_reg4 = 1 << 6 | L3G4200D.GYRO_SCALE[fs_scale][1] << 4
I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG4,
ctrl_reg4)
def calculate(self):
# The sensor has a block of factory set calibration values we need to read
# these are then used in a length calculation to get the temperature and pressure
# copy these into convenience variables
ac1 = self.get_word(self.calibration, 0, True)
ac2 = self.get_word(self.calibration, 2, True)
ac3 = self.get_word(self.calibration, 4, True)
ac4 = self.get_word(self.calibration, 6, False)
ac5 = self.get_word(self.calibration, 8, False)
ac6 = self.get_word(self.calibration, 10, False)
b1 = self.get_word(self.calibration, 12, True)
b2 = self.get_word(self.calibration, 14, True)
mb = self.get_word(self.calibration, 16, True)
mc = self.get_word(self.calibration, 18, True)
md = self.get_word(self.calibration, 20, True)
oss = self.oss
# This code is a direct translation from the datasheet
# and should be optimised for real world use
# Read raw temperature
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x2E) # Tell the sensor to take a temperature reading
time.sleep(self.temp_wait_period) # Wait for the conversion to take place
temp_raw = I2CUtils.i2c_read_word_signed(self.bus, self.address, 0xF6)
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x34 + (self.oss << 6)) # Tell the sensor to take a pressure reading
time.sleep(self.pressure_wait_period) # Wait for the conversion to take place
pressure_raw = ((I2CUtils.i2c_read_byte(self.bus, self.address, 0xF6) << 16) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF7) << 8) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF8))) >> (8 - self.oss)
# Calculate temperature
x1 = ((temp_raw - ac6) * ac5) / 32768
x2 = (mc * 2048) / (x1 + md)
b5 = x1 + x2
t = (b5 + 8) / 16
# Now calculate the pressure
b6 = b5 - 4000
x1 = (b2 * (b6 * b6 >> 12)) >> 11
x2 = ac2 * b6 >> 11
x3 = x1 + x2
b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2
x1 = (ac3 * b6) >> 13
x2 = (b1 * (b6 * b6 >> 12)) >> 16
x3 = ((x1 + x2) + 2) >> 2
b4 = ac4 * (x3 + 32768) >> 15
b7 = (pressure_raw - b3) * (50000 >> oss)
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(t / 10., p / 100.)
def __init__(self, bus, address, name, fs_scale=FS_250):
'''
Constructor
'''
self.bus = bus
self.address = address
self.name = name
self.fs_scale = fs_scale
self.gyro_raw_x = 0
self.gyro_raw_y = 0
self.gyro_raw_z = 0
self.gyro_scaled_x = 0
self.gyro_scaled_y = 0
self.gyro_scaled_z = 0
self.raw_temp = 0
self.scaled_temp = 0
# Wake up the deice and get output for each of the three axes,X, Y & Z
I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG1, 0b00001111)
# Select Big endian so we can use existing I2C library and include the scaling
ctrl_reg4 = 1 << 6 | L3G4200D.GYRO_SCALE[fs_scale][1] << 4
I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG4, ctrl_reg4)
def calculate(self):
# The sensor has a block of factory set calibration values we need to read
# these are then used in a length calculation to get the temperature and pressure
# copy these into convenience variables
ac1 = self.get_word(self.calibration, 0, True)
ac2 = self.get_word(self.calibration, 2, True)
ac3 = self.get_word(self.calibration, 4, True)
ac4 = self.get_word(self.calibration, 6, False)
ac5 = self.get_word(self.calibration, 8, False)
ac6 = self.get_word(self.calibration, 10, False)
b1 = self.get_word(self.calibration, 12, True)
b2 = self.get_word(self.calibration, 14, True)
mb = self.get_word(self.calibration, 16, True)
mc = self.get_word(self.calibration, 18, True)
md = self.get_word(self.calibration, 20, True)
oss = self.oss
# This code is a direct translation from the datasheet
# and should be optimised for real world use
# Read raw temperature
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x2E) # Tell the sensor to take a temperature reading
time.sleep(self.temp_wait_period) # Wait for the conversion to take place
temp_raw = I2CUtils.i2c_read_word_signed(self.bus, self.address, 0xF6)
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x34 + (self.oss << 6)) # Tell the sensor to take a pressure reading
time.sleep(self.pressure_wait_period) # Wait for the conversion to take place
pressure_raw = ((I2CUtils.i2c_read_byte(self.bus, self.address, 0xF6) << 16) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF7) << 8) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF8))) >> (8 - self.oss)
# Calculate temperature
x1 = ((temp_raw - ac6) * ac5) / 32768
x2 = (mc * 2048) / (x1 + md)
b5 = x1 + x2
t = (b5 + 8) / 16
# Now calculate the pressure
b6 = b5 - 4000
x1 = (b2 * (b6 * b6 >> 12)) >> 11
x2 = ac2 * b6 >> 11
x3 = x1 + x2
b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2
x1 = (ac3 * b6) >> 13
x2 = (b1 * (b6 * b6 >> 12)) >> 16
x3 = ((x1 + x2) + 2) >> 2
b4 = ac4 * (x3 + 32768) >> 15
b7 = (pressure_raw - b3) * (50000 >> oss)
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(t / 10., p / 100.)
def calculate(self):
ac1 = self.get_word(self.calibration, 0, True)
ac2 = self.get_word(self.calibration, 2, True)
ac3 = self.get_word(self.calibration, 4, True)
ac4 = self.get_word(self.calibration, 6, False)
ac5 = self.get_word(self.calibration, 8, False)
ac6 = self.get_word(self.calibration, 10, False)
b1 = self.get_word(self.calibration, 12, True)
b2 = self.get_word(self.calibration, 14, True)
mb = self.get_word(self.calibration, 16, True)
mc = self.get_word(self.calibration, 18, True)
md = self.get_word(self.calibration, 20, True)
oss = self.oss
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x2E) # Tell the sensor to take a temperature reading
time.sleep(self.temp_wait_period) # Wait for the conversion to take place
temp_raw = I2CUtils.i2c_read_word_signed(self.bus, self.address, 0xF6)
I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x34 + (self.oss << 6)) # Tell the sensor to take a pressure reading
time.sleep(self.pressure_wait_period) # Wait for the conversion to take place
pressure_raw = ((I2CUtils.i2c_read_byte(self.bus, self.address, 0xF6) << 16) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF7) << 8) \
+ (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF8))) >> (8 - self.oss)
# Calculate temperature
x1 = ((temp_raw - ac6) * ac5) / 32768
x2 = (mc * 2048) / (x1 + md)
b5 = x1 + x2
t = (b5 + 8) / 16
# Calculate the pressure
b6 = b5 - 4000
x1 = (b2 * (b6 * b6 >> 12)) >> 11
x2 = ac2 * b6 >> 11
x3 = x1 + x2
b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2
x1 = (ac3 * b6) >> 13
x2 = (b1 * (b6 * b6 >> 12)) >> 16
x3 = ((x1 + x2) + 2) >> 2
b4 = ac4 * (x3 + 32768) >> 15
b7 = (pressure_raw - b3) * (50000 >> oss)
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(t / 10., p / 100.)
def __init__(self, bus, address, name, samples=3, rate=4, gain=1, sampling_mode=0, x_offset=0, y_offset=0, z_offset=0):
self.bus = bus
self.address = address
self.name = name
self.samples = samples
self.gain = gain
self.sampling_mode = sampling_mode
self.x_offset = x_offset
self.y_offset = y_offset
self.z_offset = z_offset
#Turn off magnetometer as slave and create a connection as multimaster
I2CUtils.i2c_write_byte(self.bus, 0x68, 0x37, 0x32)
# Set the number of samples
conf_a = (samples << 5) + (rate << 2)
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_A, conf_a)
# Set the gain
conf_b = gain << 5
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_B, conf_b)
# Set the operation mode
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.MODE_REG, self.sampling_mode)
self.raw_data = [0, 0, 0, 0, 0, 0]
# Now read all the values as the first read after a gain change returns the old value
self.read_raw_data()
def __init__(self, bus, address, name, afs_scale=AFS_2g):
'''
Constructor
'''
self.bus = bus
self.address = address
self.name = name
self.afs_scale = afs_scale
self.raw_accel_data = [0, 0, 0, 0, 0, 0]
self.accel_raw_x = 0
self.accel_raw_y = 0
self.accel_raw_z = 0
self.accel_scaled_x = 0
self.accel_scaled_y = 0
self.accel_scaled_z = 0
self.pitch = 0.0
self.roll = 0.0
self.last_time = time.time()
self.time_diff = 0
# Wake up the device
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.POWER_CTL, 0b00001000)
# Set data to FULL_RES and user defined scale
data_format = 1 << 3 | afs_scale
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.DATA_FORMAT, data_format)
# Disable FIFO mode
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.FIFO_CTL, 0b00000000)
def __init__(self, bus, address, name, samples=3, rate=4, gain=1, sampling_mode=0, x_offset=0, y_offset=0, z_offset=0):
self.bus = bus
self.address = address
self.name = name
self.samples = samples
self.gain = gain
self.sampling_mode = sampling_mode
self.x_offset = x_offset
self.y_offset = y_offset
self.z_offset = z_offset
# Set the number of samples
conf_a = (samples << 5) + (rate << 2)
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_A, conf_a)
# Set the gain
conf_b = gain << 5
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_B, conf_b)
# Set the operation mode
I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.MODE_REG, self.sampling_mode)
self.raw_data = [0, 0, 0, 0, 0, 0]
# Now read all the values as the first read after a gain change returns the old value
self.read_raw_data()
def __init__(self, bus, address, name, afs_scale=AFS_2g):
"""
Constructor
"""
self.bus = bus
self.address = address
self.name = name
self.afs_scale = afs_scale
self.raw_accel_data = [0, 0, 0, 0, 0, 0]
self.accel_raw_x = 0
self.accel_raw_y = 0
self.accel_raw_z = 0
self.accel_scaled_x = 0
self.accel_scaled_y = 0
self.accel_scaled_z = 0
self.pitch = 0.0
self.roll = 0.0
self.last_time = time.time()
self.time_diff = 0
# Wake up the device
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.POWER_CTL, 0b00001000)
# Set data to FULL_RES and user defined scale
data_format = 1 << 3 | afs_scale
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.DATA_FORMAT, data_format)
# Disable FIFO mode
I2CUtils.i2c_write_byte(self.bus, self.address, ADXL345.FIFO_CTL, 0b00000000)
def __init__(self, bus, address, name, fs_scale=FS_250, afs_scale=AFS_2g):
'''
Constructor
'''
self.bus = bus
self.address = address
self.name = name
self.fs_scale = fs_scale
self.afs_scale = afs_scale
self.raw_gyro_data = [0, 0, 0, 0, 0, 0]
self.raw_accel_data = [0, 0, 0, 0, 0, 0]
self.raw_temp_data = [0, 0]
self.gyro_raw_x = 0
self.gyro_raw_y = 0
self.gyro_raw_z = 0
self.gyro_scaled_x = 0
self.gyro_scaled_y = 0
self.gyro_scaled_z = 0
self.raw_temp = 0
self.scaled_temp = 0
self.accel_raw_x = 0
self.accel_raw_y = 0
self.accel_raw_z = 0
self.accel_scaled_x = 0
self.accel_scaled_y = 0
self.accel_scaled_z = 0
self.pitch = 0.0
self.roll = 0.0
# We need to wake up the module as it start in sleep mode
I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.PWR_MGMT_1, 0)
# Set the gryo resolution
I2CUtils.i2c_write_byte(self.bus, self.address,
MPU6050.FS_SEL, self.fs_scale << 3)
# Set the accelerometer resolution
I2CUtils.i2c_write_byte(self.bus, self.address,
MPU6050.AFS_SEL, self.afs_scale << 3)
def __init__(self, bus, address, name, fs_scale=FS_250, afs_scale=AFS_2g):
'''
Constructor
'''
self.bus = bus
self.address = address
self.name = name
self.fs_scale = fs_scale
self.afs_scale = afs_scale
self.raw_gyro_data = [0, 0, 0, 0, 0, 0]
self.raw_accel_data = [0, 0, 0, 0, 0, 0]
self.raw_temp_data = [0, 0]
self.gyro_raw_x = 0
self.gyro_raw_y = 0
self.gyro_raw_z = 0
self.gyro_scaled_x = 0
self.gyro_scaled_y = 0
self.gyro_scaled_z = 0
self.raw_temp = 0
self.scaled_temp = 0
self.accel_raw_x = 0
self.accel_raw_y = 0
self.accel_raw_z = 0
self.accel_scaled_x = 0
self.accel_scaled_y = 0
self.accel_scaled_z = 0
self.pitch = 0.0
self.roll = 0.0
# We need to wake up the module as it start in sleep mode
I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.PWR_MGMT_1, 0)
# Set the gryo resolution
I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.FS_SEL,
self.fs_scale << 3)
# Set the accelerometer resolution
I2CUtils.i2c_write_byte(self.bus, self.address, MPU6050.AFS_SEL,
self.afs_scale << 3)
def addOption(self, register, *function_set): options = self.bus.read_byte(register) for function in function_set: options = options | function #self.bus.write_byte(register, options) I2CUtils.i2c_write_byte(self.bus, self.address, register, options)
