def read_raw_data(self):
self.gyro_raw_x = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_XOUT_H)
self.gyro_raw_y = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_YOUT_H)
self.gyro_raw_z = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_ZOUT_H)
# We convert these to radians for consistency and so we can easily combine later in the filter
self.gyro_scaled_x = math.radians(self.gyro_raw_x * L3G4200D.GYRO_SCALE[self.fs_scale][0])
self.gyro_scaled_y = math.radians(self.gyro_raw_y * L3G4200D.GYRO_SCALE[self.fs_scale][0])
self.gyro_scaled_z = math.radians(self.gyro_raw_z * L3G4200D.GYRO_SCALE[self.fs_scale][0])
def read_raw_data(self):
self.gyro_raw_x = I2CUtils.i2c_read_word_signed(self.bus, self.address, ITG3205.GYRO_XOUT_H) + ITG3205.X_OFFSET
self.gyro_raw_y = I2CUtils.i2c_read_word_signed(self.bus, self.address, ITG3205.GYRO_YOUT_H) + ITG3205.Y_OFFSET
self.gyro_raw_z = I2CUtils.i2c_read_word_signed(self.bus, self.address, ITG3205.GYRO_ZOUT_H) + ITG3205.Z_OFFSET
# We convert these to radians for consistency and so we can easily combine later in the filter
self.gyro_scaled_x = math.radians(self.gyro_raw_x * ITG3205.GYRO_SCALE)
self.gyro_scaled_y = math.radians(self.gyro_raw_y * ITG3205.GYRO_SCALE)
self.gyro_scaled_z = math.radians(self.gyro_raw_z * ITG3205.GYRO_SCALE)
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):
# 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.)
