class PCA9685:
"""
Initialise the PCA9685 chip at ``address`` on ``i2c_bus``.
The internal reference clock is 25mhz but may vary slightly with environmental conditions and
manufacturing variances. Providing a more precise ``reference_clock_speed`` can improve the
accuracy of the frequency and duty_cycle computations. See the ``calibration.py`` example for
how to derive this value by measuring the resulting pulse widths.
:param ~busio.I2C i2c_bus: The I2C bus which the PCA9685 is connected to.
:param int address: The I2C address of the PCA9685.
:param int reference_clock_speed: The frequency of the internal reference clock in Hertz.
"""
# Registers:
mode1_reg = UnaryStruct(0x00, '<B')
prescale_reg = UnaryStruct(0xFE, '<B')
pwm_regs = StructArray(0x06, '<HH', 16)
def __init__(self,
i2c_bus,
*,
address=0x40,
reference_clock_speed=25000000):
self.i2c_device = i2c_device.I2CDevice(i2c_bus, address)
self.channels = PCAChannels(self)
"""Sequence of 16 `PWMChannel` objects. One for each channel."""
self.reference_clock_speed = reference_clock_speed
"""The reference clock speed in Hz."""
self.reset()
def reset(self):
"""Reset the chip."""
self.mode1_reg = 0x00 # Mode1
@property
def frequency(self):
"""The overall PWM frequency in Hertz."""
return self.reference_clock_speed / 4096 / self.prescale_reg
@frequency.setter
def frequency(self, freq):
prescale = int(self.reference_clock_speed / 4096.0 / freq + 0.5)
if prescale < 3:
raise ValueError("PCA9685 cannot output at the given frequency")
old_mode = self.mode1_reg # Mode 1
self.mode1_reg = (old_mode & 0x7F) | 0x10 # Mode 1, sleep
self.prescale_reg = prescale # Prescale
self.mode1_reg = old_mode # Mode 1
time.sleep(0.005)
self.mode1_reg = old_mode | 0xa1 # Mode 1, autoincrement on
def __enter__(self):
return self
def __exit__(self, exception_type, exception_value, traceback):
self.deinit()
def deinit(self):
"""Stop using the pca9685."""
self.reset()
class MPU6050:
"""Driver for the MPU6050 6-DoF accelerometer and gyroscope.
:param ~busio.I2C i2c_bus: The I2C bus the MPU6050 is connected to.
:param address: The I2C slave address of the sensor
"""
def __init__(self, i2c_bus, address=_MPU6050_DEFAULT_ADDRESS):
self.i2c_device = i2c_device.I2CDevice(i2c_bus, address)
if self._device_id != _MPU6050_DEVICE_ID:
raise RuntimeError("Failed to find MPU6050 - check your wiring!")
self.reset()
self._sample_rate_divisor = 0
self._filter_bandwidth = Bandwidth.BAND_260_HZ
self._gyro_range = GyroRange.RANGE_500_DPS
self._accel_range = Range.RANGE_2_G
sleep(0.100)
self._clock_source = 1 # set to use gyro x-axis as reference
sleep(0.100)
self.sleep = False
sleep(0.010)
def reset(self):
"""Reinitialize the sensor"""
self._reset = True
while self._reset is True:
sleep(0.001)
sleep(0.100)
_signal_path_reset = 0b111 # reset all sensors
sleep(0.100)
_clock_source = RWBits(3, _MPU6050_PWR_MGMT_1, 0)
_device_id = ROUnaryStruct(_MPU6050_WHO_AM_I, ">B")
_reset = RWBit(_MPU6050_PWR_MGMT_1, 7, 1)
_signal_path_reset = RWBits(3, _MPU6050_SIG_PATH_RESET, 3)
_gyro_range = RWBits(2, _MPU6050_GYRO_CONFIG, 3)
_accel_range = RWBits(2, _MPU6050_ACCEL_CONFIG, 3)
_filter_bandwidth = RWBits(2, _MPU6050_CONFIG, 3)
_raw_accel_data = StructArray(_MPU6050_ACCEL_OUT, ">h", 3)
_raw_gyro_data = StructArray(_MPU6050_GYRO_OUT, ">h", 3)
_raw_temp_data = ROUnaryStruct(_MPU6050_TEMP_OUT, ">h")
_cycle = RWBit(_MPU6050_PWR_MGMT_1, 5)
_cycle_rate = RWBits(2, _MPU6050_PWR_MGMT_2, 6, 1)
sleep = RWBit(_MPU6050_PWR_MGMT_1, 6, 1)
"""Shuts down the accelerometers and gyroscopes, saving power. No new data will
be recorded until the sensor is taken out of sleep by setting to `False`"""
sample_rate_divisor = UnaryStruct(_MPU6050_SMPLRT_DIV, ">B")
"""The sample rate divisor. See the datasheet for additional detail"""
@property
def temperature(self):
"""The current temperature in º C"""
raw_temperature = self._raw_temp_data
temp = (raw_temperature / 340.0) + 36.53
return temp
@property
def acceleration(self):
"""Acceleration X, Y, and Z axis data in m/s^2"""
raw_data = self._raw_accel_data
raw_x = raw_data[0][0]
raw_y = raw_data[1][0]
raw_z = raw_data[2][0]
accel_range = self._accel_range
accel_scale = 1
if accel_range == Range.RANGE_16_G:
accel_scale = 2048
if accel_range == Range.RANGE_8_G:
accel_scale = 4096
if accel_range == Range.RANGE_4_G:
accel_scale = 8192
if accel_range == Range.RANGE_2_G:
accel_scale = 16384
# setup range dependant scaling
accel_x = (raw_x / accel_scale) * STANDARD_GRAVITY
accel_y = (raw_y / accel_scale) * STANDARD_GRAVITY
accel_z = (raw_z / accel_scale) * STANDARD_GRAVITY
return (accel_x, accel_y, accel_z)
@property
def gyro(self):
"""Gyroscope X, Y, and Z axis data in º/s"""
raw_data = self._raw_gyro_data
raw_x = raw_data[0][0]
raw_y = raw_data[1][0]
raw_z = raw_data[2][0]
gyro_scale = 1
gyro_range = self._gyro_range
if gyro_range == GyroRange.RANGE_250_DPS:
gyro_scale = 131
if gyro_range == GyroRange.RANGE_500_DPS:
gyro_scale = 65.5
if gyro_range == GyroRange.RANGE_1000_DPS:
gyro_scale = 32.8
if gyro_range == GyroRange.RANGE_2000_DPS:
gyro_scale = 16.4
# setup range dependant scaling
gyro_x = (raw_x / gyro_scale)
gyro_y = (raw_y / gyro_scale)
gyro_z = (raw_z / gyro_scale)
return (gyro_x, gyro_y, gyro_z)
@property
def cycle(self):
"""Enable or disable perodic measurement at a rate set by `cycle_rate`.
If the sensor was in sleep mode, it will be waken up to cycle"""
return self._cycle
@cycle.setter
def cycle(self, value):
self.sleep = not value
self._cycle = value
@property
def gyro_range(self):
"""The measurement range of all gyroscope axes. Must be a `GyroRange`"""
return self._gyro_range
@gyro_range.setter
def gyro_range(self, value):
if (value < 0) or (value > 3):
raise ValueError("gyro_range must be a GyroRange")
self._gyro_range = value
sleep(0.01)
@property
def accelerometer_range(self):
"""The measurement range of all accelerometer axes. Must be a `Range`"""
return self._accel_range
@accelerometer_range.setter
def accelerometer_range(self, value):
if (value < 0) or (value > 3):
raise ValueError("accelerometer_range must be a Range")
self._accel_range = value
sleep(0.01)
@property
def filter_bandwidth(self):
"""The bandwidth of the gyroscope Digital Low Pass Filter. Must be a `GyroRange`"""
return self._filter_bandwidth
@filter_bandwidth.setter
def filter_bandwidth(self, value):
if (value < 0) or (value > 6):
raise ValueError("filter_bandwidth must be a Bandwidth")
self._filter_bandwidth = value
sleep(0.01)
@property
def cycle_rate(self):
"""The rate that measurements are taken while in `cycle` mode. Must be a `Rate`"""
return self._cycle_rate
@cycle_rate.setter
def cycle_rate(self, value):
if (value < 0) or (value > 3):
raise ValueError("cycle_rate must be a Rate")
self._cycle_rate = value
sleep(0.01)
class LSM303_Accel: #pylint:disable=too-many-instance-attributes
"""Driver for the LSM303's accelerometer."""
# Class-level buffer for reading and writing data with the sensor.
# This reduces memory allocations but means the code is not re-entrant or
# thread safe!
_chip_id = UnaryStruct(_REG_ACCEL_WHO_AM_I, "B")
_int2_int1_enable = RWBit(_REG_ACCEL_CTRL_REG6_A, 6)
_int2_int2_enable = RWBit(_REG_ACCEL_CTRL_REG6_A, 5)
_int1_latching = RWBit(_REG_ACCEL_CTRL_REG5_A, 3)
_int2_latching = RWBit(_REG_ACCEL_CTRL_REG5_A, 1)
_bdu = RWBit(_REG_ACCEL_CTRL_REG4_A, 7)
_int2_activity_enable = RWBit(_REG_ACCEL_CTRL_REG6_A, 3)
_int_pin_active_low = RWBit(_REG_ACCEL_CTRL_REG6_A, 1)
_act_threshold = UnaryStruct(_REG_ACCEL_ACT_THS_A, "B")
_act_duration = UnaryStruct(_REG_ACCEL_ACT_DUR_A, "B")
"""
.. code-block:: python
import board
i2c = board.I2C()
import adafruit_lsm303_accel
accel = adafruit_lsm303_accel.LSM303_Accel(i2c)
accel._act_threshold = 20
accel._act_duration = 1
accel._int2_activity_enable = True
# toggle pins, defaults to False
accel._int_pin_active_low = True
"""
_data_rate = RWBits(4, _REG_ACCEL_CTRL_REG1_A, 4)
_enable_xyz = RWBits(3, _REG_ACCEL_CTRL_REG1_A, 0)
_raw_accel_data = StructArray(_REG_ACCEL_OUT_X_L_A, "<h", 3)
_low_power = RWBit(_REG_ACCEL_CTRL_REG1_A, 3)
_high_resolution = RWBit(_REG_ACCEL_CTRL_REG4_A, 3)
_range = RWBits(2, _REG_ACCEL_CTRL_REG4_A, 4)
_int1_src = UnaryStruct(_REG_ACCEL_INT1_SOURCE_A, "B")
_tap_src = UnaryStruct(_REG_ACCEL_CLICK_SRC_A, "B")
_tap_interrupt_enable = RWBit(_REG_ACCEL_CTRL_REG3_A, 7, 1)
_tap_config = UnaryStruct(_REG_ACCEL_CLICK_CFG_A, "B")
_tap_interrupt_active = ROBit(_REG_ACCEL_CLICK_SRC_A, 6, 1)
_tap_threshold = UnaryStruct(_REG_ACCEL_CLICK_THS_A, "B")
_tap_time_limit = UnaryStruct(_REG_ACCEL_TIME_LIMIT_A, "B")
_tap_time_latency = UnaryStruct(_REG_ACCEL_TIME_LATENCY_A, "B")
_tap_time_window = UnaryStruct(_REG_ACCEL_TIME_WINDOW_A, "B")
_BUFFER = bytearray(6)
def __init__(self, i2c):
self._accel_device = I2CDevice(i2c, _ADDRESS_ACCEL)
self.i2c_device = self._accel_device
self._data_rate = 2
self._enable_xyz = 0b111
self._int1_latching = True
self._int2_latching = True
self._bdu = True
# self._write_register_byte(_REG_CTRL5, 0x80)
# time.sleep(0.01) # takes 5ms
self._cached_mode = 0
self._cached_range = 0
def set_tap(self,
tap,
threshold,
*,
time_limit=10,
time_latency=20,
time_window=255,
tap_cfg=None):
"""
The tap detection parameters.
:param int tap: 0 to disable tap detection, 1 to detect only single taps, and 2 to detect \
only double taps.
:param int threshold: A threshold for the tap detection. The higher the value the less\
sensitive the detection. This changes based on the accelerometer range. Good values\
are 5-10 for 16G, 10-20 for 8G, 20-40 for 4G, and 40-80 for 2G.
:param int time_limit: TIME_LIMIT register value (default 10).
:param int time_latency: TIME_LATENCY register value (default 20).
:param int time_window: TIME_WINDOW register value (default 255).
:param int click_cfg: CLICK_CFG register value.
"""
if (tap < 0 or tap > 2) and tap_cfg is None:
raise ValueError(
'Tap must be 0 (disabled), 1 (single tap), or 2 (double tap)!')
if threshold > 127 or threshold < 0:
raise ValueError('Threshold out of range (0-127)')
if tap == 0 and tap_cfg is None:
# Disable click interrupt.
self._tap_interrupt_enable = False
self._tap_config = 0
return
self._tap_interrupt_enable = True
if tap_cfg is None:
if tap == 1:
tap_cfg = 0x15 # Turn on all axes & singletap.
if tap == 2:
tap_cfg = 0x2A # Turn on all axes & doubletap.
# Or, if a custom tap configuration register value specified, use it.
self._tap_config = tap_cfg
self._tap_threshold = threshold # why and?
self._tap_time_limit = time_limit
self._tap_time_latency = time_latency
self._tap_time_window = time_window
@property
def tapped(self):
"""
True if a tap was detected recently. Whether its a single tap or double tap is
determined by the tap param on ``set_tap``. ``tapped`` may be True over
multiple reads even if only a single tap or single double tap occurred.
"""
tap_src = self._tap_src
return tap_src & 0b1000000 > 0
@property
def _raw_acceleration(self):
self._read_bytes(self._accel_device, _REG_ACCEL_OUT_X_L_A | 0x80, 6,
self._BUFFER)
return struct.unpack_from('<hhh', self._BUFFER[0:6])
@property
def acceleration(self):
"""The measured accelerometer sensor values.
A 3-tuple of X, Y, Z axis values in m/s^2 squared that are signed floats.
"""
raw_accel_data = self._raw_acceleration
x = self._scale_data(raw_accel_data[0])
y = self._scale_data(raw_accel_data[1])
z = self._scale_data(raw_accel_data[2])
return (x, y, z)
def _scale_data(self, raw_measurement):
lsb, shift = self._lsb_shift()
return (raw_measurement >> shift) * lsb * _SMOLLER_GRAVITY
def _lsb_shift(self): #pylint:disable=too-many-branches
# the bit depth of the data depends on the mode, and the lsb value
# depends on the mode and range
lsb = -1 # the default, normal mode @ 2G
if self._cached_mode is Mode.MODE_HIGH_RESOLUTION: # 12-bit
shift = 4
if self._cached_range is Range.RANGE_2G:
lsb = 0.98
elif self._cached_range is Range.RANGE_4G:
lsb = 1.95
elif self._cached_range is Range.RANGE_8G:
lsb = 3.9
elif self._cached_range is Range.RANGE_16G:
lsb = 11.72
elif self._cached_mode is Mode.MODE_NORMAL: # 10-bit
shift = 6
if self._cached_range is Range.RANGE_2G:
lsb = 3.9
elif self._cached_range is Range.RANGE_4G:
lsb = 7.82
elif self._cached_range is Range.RANGE_8G:
lsb = 15.63
elif self._cached_range is Range.RANGE_16G:
lsb = 46.9
elif self._cached_mode is Mode.MODE_LOW_POWER: # 8-bit
shift = 8
if self._cached_range is Range.RANGE_2G:
lsb = 15.63
elif self._cached_range is Range.RANGE_4G:
lsb = 31.26
elif self._cached_range is Range.RANGE_8G:
lsb = 62.52
elif self._cached_range is Range.RANGE_16G:
lsb = 187.58
if lsb is -1:
raise AttributeError(
"'impossible' range or mode detected: range: %d mode: %d" %
(self._cached_range, self._cached_mode))
return (lsb, shift)
@property
def data_rate(self):
"""Select the rate at which the sensor takes measurements. Must be a `Rate`"""
return self._data_rate
@data_rate.setter
def data_rate(self, value):
if value < 0 or value > 9:
raise AttributeError("data_rate must be a `Rate`")
self._data_rate = value
@property
def range(self):
"""Adjusts the range of values that the sensor can measure, from +- 2G to +-16G
Note that larger ranges will be less accurate. Must be a `Range`"""
return self._cached_range
@range.setter
def range(self, value):
if value < 0 or value > 3:
raise AttributeError("range must be a `Range`")
self._range = value
self._cached_range = value
@property
def mode(self):
"""Sets the power mode of the sensor. The mode must be a `Mode`. Note that the
mode and range will both affect the accuracy of the sensor"""
return self._cached_mode
@mode.setter
def mode(self, value):
if value < 0 or value > 2:
raise AttributeError("mode must be a `Mode`")
self._high_resolution = value & 0b01
self._low_power = (value & 0b10) >> 1
self._cached_mode = value
def _read_u8(self, device, address):
with device as i2c:
self._BUFFER[0] = address & 0xFF
i2c.write_then_readinto(self._BUFFER,
self._BUFFER,
out_end=1,
in_end=1)
return self._BUFFER[0]
def _write_u8(self, device, address, val):
with device as i2c:
self._BUFFER[0] = address & 0xFF
self._BUFFER[1] = val & 0xFF
i2c.write(self._BUFFER, end=2)
@staticmethod
def _read_bytes(device, address, count, buf):
with device as i2c:
buf[0] = address & 0xFF
i2c.write_then_readinto(buf, buf, out_end=1, in_end=count)
class DS1841:
"""Driver for the DS3502 I2C Digital Potentiometer.
:param ~busio.I2C i2c_bus: The I2C bus the DS3502 is connected to.
:param address: The I2C device address for the sensor. Default is ``0x28``.
"""
_lut_address = UnaryStruct(_DS1841_LUTAR, ">B")
_wiper_register = UnaryStruct(_DS1841_WR, ">B")
_temperature_register = UnaryStruct(_DS1841_TEMP, ">b")
_voltage_register = UnaryStruct(_DS1841_VOLTAGE, ">B")
# NV-capable settings
_disable_save_to_eeprom = RWBit(_DS1841_CR0, 7)
# Can be shadowed by EEPROM
_initial_value_register = UnaryStruct(_DS1841_IVR, ">B")
_adder_mode_bit = RWBit(_DS1841_CR1, 1)
_update_mode = RWBit(_DS1841_CR1, 0)
_manual_lut_address = RWBit(_DS1841_CR2, 1)
_manual_wiper_value = RWBit(_DS1841_CR2, 2)
_lut = StructArray(_DS1841_LUT, ">B", 72)
def __init__(self, i2c_bus, address=_DS1841_DEFAULT_ADDRESS):
self.i2c_device = i2c_device.I2CDevice(i2c_bus, address)
self._disable_save_to_eeprom = True # turn off eeprom updates to IV and CR0
self._adder_mode_bit = False # Don't add IV to WR
# UPDATE MODE MUST BE FALSE FOR WIPER TO SHADOW IV
self._manual_lut_address = True #
self._manual_wiper_value = True # update WR by I2C
self._lut_mode_enabled = False
self._update_mode = True
@property
def wiper(self):
"""The value of the potentionmeter's wiper.
:param wiper_value: The value from 0-127 to set the wiper to.
"""
return self._wiper_register
@wiper.setter
def wiper(self, value):
if value > 127:
raise AttributeError("wiper must be from 0-127")
self._wiper_register = value
@property
def wiper_default(self):
"""Sets the wiper's default value and current value to the given value
:param new_default: The value from 0-127 to set as the wiper's default.
"""
return self._initial_value_register
@wiper_default.setter
def wiper_default(self, value):
if value > 127:
raise AttributeError("initial_value must be from 0-127")
self._disable_save_to_eeprom = False
# allows for IV to pass through to WR.
# this setting is also saved to EEPROM so IV will load into WR on boot
self._update_mode = False
sleep(0.2)
self._initial_value_register = value
sleep(0.2)
self._disable_save_to_eeprom = True
# Turn update mode back on so temp and voltage update
# and LUT usage works
self._update_mode = True
@property
def temperature(self):
"""The current temperature in degrees celcius"""
return self._temperature_register
@property
def voltage(self):
"""The current voltage between VCC and GND"""
return self._voltage_register * _DS1841_VCC_LSB
######## LUTS on LUTS on LUTS
@property
def lut_mode_enabled(self):
"""Enables LUT mode. LUT mode takes sets the value of the Wiper based on the entry in a
72-entry Look Up Table. The LUT entry is selected using the `lut_selection`
property to set an index from 0-71
"""
return self._lut_mode_enabled
@lut_mode_enabled.setter
def lut_mode_enabled(self, value):
self._manual_lut_address = value
self._update_mode = True
self._manual_wiper_value = not value
self._lut_mode_enabled = value
def set_lut(self, index, value):
"""Set the value of an entry in the Look Up Table.
:param index: The index of the entry to set, from 0-71.
:param value: The value to set at the given index. The `wiper` will be set to this
value when the LUT entry is selected using `lut_selection`
"""
if value > 127:
raise IndexError("set_lut value must be from 0-127")
lut_value_byte = bytearray([value])
self._lut[index] = lut_value_byte
sleep(0.020)
@property
def lut_selection(self):
"""Choose the entry in the Look Up Table to use to set the wiper.
:param index: The index of the entry to use, from 0-71.
"""
if not self._lut_mode_enabled:
raise RuntimeError(
"lut_mode_enabled must be equal to True to use lut_selection"
)
return self._lut_address - _DS1841_LUT
@lut_selection.setter
def lut_selection(self, value):
if value > 71 or value < 0:
raise IndexError("lut_selection value must be from 0-71")
self._lut_address = value + _DS1841_LUT
sleep(0.020)
class FanSpeedLUT:
"""A class used to provide a dict-like interface to the EMC2101's Temperature to Fan speed
Look Up Table.
Keys are integer temperatures, values are fan duty cycles between 0 and 100.
A max of 8 values may be stored.
To remove a single stored point in the LUT, assign it as `None`.
"""
# 8 (Temperature, Speed) pairs in increasing order
_fan_lut = StructArray(_LUT_BASE, "<B", 16)
def __init__(self, fan_obj):
self.emc_fan = fan_obj
self.lut_values = {}
self.i2c_device = fan_obj.i2c_device
def __getitem__(self, index):
if not isinstance(index, int):
raise IndexError
if not index in self.lut_values:
raise IndexError
return self.lut_values[index]
def __setitem__(self, index, value):
if not isinstance(index, int):
raise IndexError
if value is None:
# Assign None to remove this entry
del self.lut_values[index]
elif value > 100.0 or value < 0:
# Range check
raise AttributeError("LUT values must be a fan speed from 0-100%")
else:
self.lut_values[index] = value
self._update_lut()
def __repr__(self):
"""return the official string representation of the LUT"""
return "FanSpeedLUT <%x>" % id(self)
def __str__(self):
"""return the official string representation of the LUT"""
value_strs = []
lut_keys = tuple(sorted(self.lut_values.keys()))
for temp in lut_keys:
fan_drive = self.lut_values[temp]
value_strs.append("%d deg C => %.1f%% duty cycle" %
(temp, fan_drive))
return "\n".join(value_strs)
def __len__(self):
return len(self.lut_values)
# this function does a whole lot of work to organized the user-supplied lut dict into
# their correct spot within the lut table as pairs of set registers, sorted with the lowest
# temperature first
def _update_lut(self):
# Make sure we're not going to try to set more entries than we have slots
if len(self.lut_values) > 8:
raise AttributeError("LUT can only contain a maximum of 8 items")
# Backup state
current_mode = self.emc_fan.lut_enabled
# Disable the lut to allow it to be updated
self.emc_fan.lut_enabled = False
# we want to assign the lowest temperature to the lowest LUT slot, so we sort the keys/temps
# get and sort the new lut keys so that we can assign them in order
for idx, current_temp in enumerate(sorted(self.lut_values.keys())):
# We don't want to make `_speed_to_lsb()` public, it is only needed here.
# pylint: disable=protected-access
current_speed = self.emc_fan._speed_to_lsb(
self.lut_values[current_temp])
self._set_lut_entry(idx, current_temp, current_speed)
# Set the remaining LUT entries to the default (Temp/Speed = max value)
for idx in range(len(self.lut_values), 8):
self._set_lut_entry(idx, MAX_LUT_TEMP, MAX_LUT_SPEED)
self.emc_fan.lut_enabled = current_mode
def _set_lut_entry(self, idx, temp, speed):
self._fan_lut[idx * 2] = bytearray((temp, ))
self._fan_lut[idx * 2 + 1] = bytearray((speed, ))
class MPU6050:
"""Driver for the MPU6050 6-DoF accelerometer and gyroscope.
:param ~busio.I2C i2c_bus: The I2C bus the MPU6050 is connected to.
:param address: The I2C slave address of the sensor
"""
def __init__(self, i2c_bus, address=_MPU6050_DEFAULT_ADDRESS):
self._logger = logging.getLogger('adafruit_mpu6050')
self.i2c_device = i2c_device.I2CDevice(i2c_bus, address)
if self._device_id != _MPU6050_DEVICE_ID:
raise RuntimeError("Failed to find MPU6050 - check your wiring!")
self.reset()
self._sample_rate_divisor = 0
self._filter_bandwidth = Bandwidth.BAND_260_HZ
self._gyro_range = GyroRange.RANGE_500_DPS
self._accel_range = Range.RANGE_2_G
sleep(0.100)
self._clock_source = 1 # set to use gyro x-axis as reference
sleep(0.100)
self.sleep = False
sleep(0.010)
def reset(self):
"""Reinitialize the sensor"""
self._reset = True
while self._reset is True:
sleep(0.001)
sleep(0.100)
_signal_path_reset = 0b111 # reset all sensors
sleep(0.100)
def perform_calibration(
self,
averaging_size: int = 1000,
discarding_size: int = 100,
accelerometer_tolerance: int = 8,
gyroscope_tolerance: int = 1,
accelerometer_step: int = 8,
gyroscope_step: int = 3,
accelerometer_x_goal=0,
accelerometer_y_goal=0,
accelerometer_z_goal=16384,
gyroscope_x_goal=0,
gyroscope_y_goal=0,
gyroscope_z_goal=0) -> Tuple[int, int, int, int, int, int]:
"""
This method calculates the sensor offsets for the accelerometer and gyroscope by averaging values
while the sensor is NOT in motion and the PCB is placed on a flat surface, facing upwards.
(Be aware of the fact, that the calibration offsets are not persistent,
they have to be set manually, after each new i2c connection.)
:param averaging_size: Number of reading sensor values used to compute average.
Make it higher to get more precision but the sketch will be lower.
Default value is set to 1000.
:param discarding_size: Number of reading sensor values to discard after setting a new offset.
Default value is set to 100.
:param accelerometer_tolerance: Error range allowed for accelerometer calibration offsets.
Make it lower to get more precision, but sketch may not converge.
Default value is set to 8.
:param gyroscope_tolerance: Error range allowed for gyroscope calibration offsets.
Make it lower to get more precision, but sketch may not converge.
Default value is set to 1.
:param accelerometer_step: Step value, tuned for accelerometer, used in each step of calibration.
Default value is set to 8.
:param gyroscope_step: Step value, tuned for gyroscope, used in each step of calibration.
Default value is set to 3.
:param accelerometer_x_goal: The goal value for the x-axis of the accelerometer.
Default value is 0.
:param accelerometer_y_goal: The goal value for the x-axis of the accelerometer.
Default value is 0.
:param accelerometer_z_goal: The goal value for the x-axis of the accelerometer.
Default value is 16384, which is the equivalent of 1g,
indicating that the sensor is under gravity.
:param gyroscope_z_goal: The goal value for the x-axis of the gyroscope.
Default value is 0.
:param gyroscope_y_goal: The goal value for the y-axis of the gyroscope.
Default value is 0.
:param gyroscope_x_goal: The goal value for the z-axis of the gyroscope.
Default value is 0.
"""
offsets = {
"accelerometer": {
"x": 0,
"y": 0,
"z": 0
},
"gyroscope": {
"x": 0,
"y": 0,
"z": 0
}
}
calibration_states = {
"accelerometer": {
"x": False,
"y": False,
"z": False
},
"gyroscope": {
"x": False,
"y": False,
"z": False
}
}
goals = {
"accelerometer": {
"x": accelerometer_x_goal,
"y": accelerometer_y_goal,
"z": accelerometer_z_goal
},
"gyroscope": {
"x": gyroscope_x_goal,
"y": gyroscope_y_goal,
"z": gyroscope_z_goal
}
}
tolerances = {
"accelerometer": accelerometer_tolerance,
"gyroscope": gyroscope_tolerance
}
steps = {
"accelerometer": accelerometer_step,
"gyroscope": gyroscope_step
}
self._logger.info('Current sensors offsets:')
self._logger.info(
f'Accelerometer X: {self._raw_accel_offset_x}, Y: {self._raw_accel_offset_y}, '
f'Z: {self._raw_accel_offset_z}')
self._logger.info(
f'Gyroscope X: {self._raw_gyro_offset_x}, Y: {self._raw_gyro_offset_y}, '
f'Z: {self._raw_gyro_offset_z}')
while not self.__is_calibrated(calibration_states):
self.__write_offsets_to_sensor(offsets)
self.__discard_unreliable_values(discarding_size)
averages = self.__calculate_average_values(averaging_size)
self.__update_offsets(offsets, averages, steps, goals, tolerances,
calibration_states, "accelerometer")
self.__update_offsets(offsets, averages, steps, goals, tolerances,
calibration_states, "gyroscope")
self._logger.info(f'New offsets:')
self._logger.info(
f'Accelerometer X: {self._raw_accel_offset_x}, Y: {self._raw_accel_offset_y}, '
f'Z: {self._raw_accel_offset_z}')
self._logger.info(
f'Gyroscope X: {self._raw_gyro_offset_x}, Y: {self._raw_gyro_offset_y}, '
f'Z: {self._raw_gyro_offset_z}')
return (offsets['accelerometer']['x'], offsets['accelerometer']['y'],
offsets['accelerometer']['z'], offsets['gyroscope']['x'],
offsets['gyroscope']['y'], offsets['gyroscope']['z'])
# noinspection DuplicatedCode
def __write_offsets_to_sensor(self, offsets):
self._logger.debug(f'Write offsets:')
self._logger.debug(
f'Accelerometer X: {offsets["accelerometer"]["x"]}, Y: {offsets["accelerometer"]["y"]}, '
f'Z: {offsets["accelerometer"]["z"]}')
self._logger.debug(
f'Gyroscope X: {offsets["gyroscope"]["x"]}, Y: {offsets["gyroscope"]["y"]}, '
f'Z: {offsets["gyroscope"]["z"]}')
self._raw_accel_offset_x = offsets['accelerometer']['x']
self._raw_accel_offset_y = offsets['accelerometer']['y']
self._raw_accel_offset_z = offsets['accelerometer']['z']
self._raw_gyro_offset_x = offsets['gyroscope']['x']
self._raw_gyro_offset_y = offsets['gyroscope']['y']
self._raw_gyro_offset_z = offsets['gyroscope']['z']
def __discard_unreliable_values(self, discarding_size):
for _ in range(discarding_size):
temp = self._raw_accel_data
temp = self._raw_gyro_data
# wait 2 ms to avoid getting same values over and over
sleep(0.002)
def __calculate_average_values(self, averaging_size):
averages = {
"accelerometer": {
"x": 0,
"y": 0,
"z": 0
},
"gyroscope": {
"x": 0,
"y": 0,
"z": 0
}
}
sums = {
"accelerometer": {
"x": 0,
"y": 0,
"z": 0
},
"gyroscope": {
"x": 0,
"y": 0,
"z": 0
}
}
for _ in range(averaging_size):
raw_accel_data = self._raw_accel_data
raw_accel_x = raw_accel_data[0][0]
raw_accel_y = raw_accel_data[1][0]
raw_accel_z = raw_accel_data[2][0]
raw_gyro_data = self._raw_gyro_data
raw_gyro_x = raw_gyro_data[0][0]
raw_gyro_y = raw_gyro_data[1][0]
raw_gyro_z = raw_gyro_data[2][0]
sums['accelerometer']['x'] += raw_accel_x
sums['accelerometer']['y'] += raw_accel_y
sums['accelerometer']['z'] += raw_accel_z
sums['gyroscope']['x'] += raw_gyro_x
sums['gyroscope']['y'] += raw_gyro_y
sums['gyroscope']['z'] += raw_gyro_z
# wait 2 ms to avoid getting same values over and over
sleep(0.002)
# calculate averages for accelerometer and gyroscope
averages['accelerometer']['x'] = int(sums['accelerometer']['x'] /
averaging_size)
averages['accelerometer']['y'] = int(sums['accelerometer']['y'] /
averaging_size)
averages['accelerometer']['z'] = int(sums['accelerometer']['z'] /
averaging_size)
averages['gyroscope']['x'] = int(sums['gyroscope']['x'] /
averaging_size)
averages['gyroscope']['y'] = int(sums['gyroscope']['y'] /
averaging_size)
averages['gyroscope']['z'] = int(sums['gyroscope']['z'] /
averaging_size)
self._logger.debug(f'Averages:')
self._logger.debug(
f'Accelerometer X: {averages["accelerometer"]["x"]}, Y: {averages["accelerometer"]["y"]}, '
f'Z: {averages["accelerometer"]["z"]}')
self._logger.debug(
f'Gyroscope X: {averages["gyroscope"]["x"]}, Y: {averages["gyroscope"]["y"]}, '
f'Z: {averages["gyroscope"]["z"]}')
return averages
# noinspection PyMethodMayBeStatic
def __update_offsets(self, offsets, averages, steps, goals, tolerances,
calibration_states, sensor):
for axis, calibrated in calibration_states[sensor].items():
if not calibrated:
difference = goals[sensor][axis] - averages[sensor][axis]
if abs(difference) > tolerances[sensor]:
offsets[sensor][axis] += int((difference / steps[sensor]))
else:
calibration_states[sensor][axis] = True
self._logger.debug(
f'{sensor.capitalize()} {axis}-axis is calibrated with difference: {difference} '
f'and offset: {offsets[sensor][axis]}')
# noinspection PyMethodMayBeStatic
def __is_calibrated(self, calibration_states):
for sensor, axes in calibration_states.items():
for axis, calibrated in axes.items():
if not calibrated:
return False
return True
_clock_source = RWBits(3, _MPU6050_PWR_MGMT_1, 0)
_device_id = ROUnaryStruct(_MPU6050_WHO_AM_I, ">B")
_reset = RWBit(_MPU6050_PWR_MGMT_1, 7, 1)
_signal_path_reset = RWBits(3, _MPU6050_SIG_PATH_RESET, 3)
_gyro_range = RWBits(2, _MPU6050_GYRO_CONFIG, 3)
_accel_range = RWBits(2, _MPU6050_ACCEL_CONFIG, 3)
_filter_bandwidth = RWBits(2, _MPU6050_CONFIG, 3)
_raw_accel_data = StructArray(_MPU6050_ACCEL_OUT, ">h", 3)
_raw_gyro_data = StructArray(_MPU6050_GYRO_OUT, ">h", 3)
_raw_temp_data = ROUnaryStruct(_MPU6050_TEMP_OUT, ">h")
_raw_accel_offset_x = UnaryStruct(_MPU6050_ACCEL_OFFSET_X, ">h")
_raw_accel_offset_y = UnaryStruct(_MPU6050_ACCEL_OFFSET_Y, ">h")
_raw_accel_offset_z = UnaryStruct(_MPU6050_ACCEL_OFFSET_Z, ">h")
_raw_gyro_offset_x = UnaryStruct(_MPU6050_GYRO_OFFSET_X, ">h")
_raw_gyro_offset_y = UnaryStruct(_MPU6050_GYRO_OFFSET_Y, ">h")
_raw_gyro_offset_z = UnaryStruct(_MPU6050_GYRO_OFFSET_Z, ">h")
_cycle = RWBit(_MPU6050_PWR_MGMT_1, 5)
_cycle_rate = RWBits(2, _MPU6050_PWR_MGMT_2, 6, 1)
sleep = RWBit(_MPU6050_PWR_MGMT_1, 6, 1)
"""Shuts down the accelerometers and gyroscopes, saving power. No new data will
be recorded until the sensor is taken out of sleep by setting to `False`"""
sample_rate_divisor = UnaryStruct(_MPU6050_SMPLRT_DIV, ">B")
"""The sample rate divisor. See the datasheet for additional detail"""
@property
def temperature(self):
"""The current temperature in º C"""
raw_temperature = self._raw_temp_data
temp = (raw_temperature / 340.0) + 36.53
return temp
@property
def acceleration(self):
"""Acceleration X, Y, and Z axis data in m/s^2"""
raw_data = self._raw_accel_data
raw_x = raw_data[0][0]
raw_y = raw_data[1][0]
raw_z = raw_data[2][0]
accel_range = self._accel_range
accel_scale = 1
if accel_range == Range.RANGE_16_G:
accel_scale = 2048
if accel_range == Range.RANGE_8_G:
accel_scale = 4096
if accel_range == Range.RANGE_4_G:
accel_scale = 8192
if accel_range == Range.RANGE_2_G:
accel_scale = 16384
# setup range dependant scaling
accel_x = (raw_x / accel_scale) * STANDARD_GRAVITY
accel_y = (raw_y / accel_scale) * STANDARD_GRAVITY
accel_z = (raw_z / accel_scale) * STANDARD_GRAVITY
return (accel_x, accel_y, accel_z)
@property
def gyro(self):
"""Gyroscope X, Y, and Z axis data in º/s"""
raw_data = self._raw_gyro_data
raw_x = raw_data[0][0]
raw_y = raw_data[1][0]
raw_z = raw_data[2][0]
gyro_scale = 1
gyro_range = self._gyro_range
if gyro_range == GyroRange.RANGE_250_DPS:
gyro_scale = 131
if gyro_range == GyroRange.RANGE_500_DPS:
gyro_scale = 65.5
if gyro_range == GyroRange.RANGE_1000_DPS:
gyro_scale = 32.8
if gyro_range == GyroRange.RANGE_2000_DPS:
gyro_scale = 16.4
# setup range dependant scaling
gyro_x = raw_x / gyro_scale
gyro_y = raw_y / gyro_scale
gyro_z = raw_z / gyro_scale
return (gyro_x, gyro_y, gyro_z)
@property
def cycle(self):
"""Enable or disable perodic measurement at a rate set by `cycle_rate`.
If the sensor was in sleep mode, it will be waken up to cycle"""
return self._cycle
@cycle.setter
def cycle(self, value):
self.sleep = not value
self._cycle = value
@property
def gyro_range(self):
"""The measurement range of all gyroscope axes. Must be a `GyroRange`"""
return self._gyro_range
@gyro_range.setter
def gyro_range(self, value):
if (value < 0) or (value > 3):
raise ValueError("gyro_range must be a GyroRange")
self._gyro_range = value
sleep(0.01)
@property
def accelerometer_range(self):
"""The measurement range of all accelerometer axes. Must be a `Range`"""
return self._accel_range
@accelerometer_range.setter
def accelerometer_range(self, value):
if (value < 0) or (value > 3):
raise ValueError("accelerometer_range must be a Range")
self._accel_range = value
sleep(0.01)
@property
def filter_bandwidth(self):
"""The bandwidth of the gyroscope Digital Low Pass Filter. Must be a `GyroRange`"""
return self._filter_bandwidth
@filter_bandwidth.setter
def filter_bandwidth(self, value):
if (value < 0) or (value > 6):
raise ValueError("filter_bandwidth must be a Bandwidth")
self._filter_bandwidth = value
sleep(0.01)
@property
def cycle_rate(self):
"""The rate that measurements are taken while in `cycle` mode. Must be a `Rate`"""
return self._cycle_rate
@cycle_rate.setter
def cycle_rate(self, value):
if (value < 0) or (value > 3):
raise ValueError("cycle_rate must be a Rate")
self._cycle_rate = value
sleep(0.01)
@property
def accel_offset_x(self):
"""The sensor-internal offset for the accelerometer in x-axis."""
return self._raw_accel_offset_x
@accel_offset_x.setter
def accel_offset_x(self, value):
self._raw_accel_offset_x = value
@property
def accel_offset_y(self):
"""The sensor-internal offset for the accelerometer in y-axis."""
return self._raw_accel_offset_y
@accel_offset_y.setter
def accel_offset_y(self, value):
self._raw_accel_offset_y = value
@property
def accel_offset_z(self):
"""The sensor-internal offset for the accelerometer in z-axis."""
return self._raw_accel_offset_z
@accel_offset_z.setter
def accel_offset_z(self, value):
self._raw_accel_offset_z = value
@property
def gyro_offset_x(self):
"""The sensor-internal offset for the gyroscope in x-axis."""
return self._raw_gyro_offset_x
@gyro_offset_x.setter
def gyro_offset_x(self, value):
self._raw_gyro_offset_x = value
@property
def gyro_offset_y(self):
"""The sensor-internal offset for the gyroscope in y-axis."""
return self._raw_gyro_offset_y
@gyro_offset_y.setter
def gyro_offset_y(self, value):
self._raw_gyro_offset_y = value
@property
def gyro_offset_z(self):
"""The sensor-internal offset for the gyroscope in z-axis."""
return self._raw_gyro_offset_z
@gyro_offset_z.setter
def gyro_offset_z(self, value):
self._raw_gyro_offset_z = value