# vim: set et sw=4 sts=4 fileencoding=utf-8:
#
# Alternative API for the Sense HAT
# Copyright (c) 2016-2018 Dave Jones <dave@waveform.org.uk>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the copyright holder nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
"""
Defines the :class:`SenseIMU`, :class:`IMUState`, :class:`IMUVector`, and
:class:`IMUOrient` classes for querying the inertial measurement unit on the
Sense HAT.
"""
from __future__ import (
unicode_literals,
absolute_import,
print_function,
division,
)
import time
from math import degrees
from collections import namedtuple
from .settings import SenseSettings
# Make Py2's str and range equivalent to Py3's
str = type('') # pylint: disable=redefined-builtin,invalid-name
[docs]class IMUState(namedtuple('IMUState', ('compass', 'gyro', 'accel', 'orient'))):
"""
A :func:`~collections.namedtuple` representing a single reading from the
Inertial Measurement Unit (IMU). The fields are as follows:
.. attribute:: compass
An :attr:`IMUVector` tuple containing the raw values from the
magnetometer in µT (`micro-teslas`_).
.. attribute:: gyro
An :attr:`IMUVector` tuple containing the raw values from the
gyroscope in `radians / second <radians-per-second>`_.
.. attribute:: accel
An :attr:`IMUVector` tuple containing the raw values from the
accelerometer in `standard gravities`_ (g).
.. attribute:: orient
The orientation of the HAT, as calculated from the three sensors,
presented as an :class:`IMUOrient` instance.
.. _micro-teslas: https://en.wikipedia.org/wiki/Tesla_(unit)
.. _radians-per-second: https://en.wikipedia.org/wiki/Radian_per_second
.. _standard gravities: https://en.wikipedia.org/wiki/Standard_gravity
"""
__slots__ = ()
[docs]class IMUVector(namedtuple('IMUVector', ('x', 'y', 'z'))):
"""
A :func:`~collections.namedtuple` representing a three-dimensional vector
with *x*, *y*, and *z* components. This is used to represent the output of
the three IMU sensors (magnetometer, gryoscope, and accelerometer).
.. attention:: TODO Add HAT-specific vector directions diagram
"""
# TODO Consider splitting Vector out of picraft and re-using it here
__slots__ = ()
def __repr__(self):
return 'IMUVector(x=%g, y=%g, z=%g)' % self
[docs]class IMUOrient(namedtuple('IMUOrient', ('roll', 'pitch', 'yaw'))):
"""
A :func:`~collections.namedtuple` representing the orientation of the Sense
HAT in radians (though the display is provided in degrees for human
convenience) as `roll, pitch, and yaw`_.
.. attention:: TODO add HAT-specific roll, pitch, yaw diagram
.. _roll, pitch, and yaw: https://en.wikipedia.org/wiki/Aircraft_principal_axes
"""
__slots__ = ()
def __repr__(self):
return 'IMUOrient(roll=%g (%.1f°), pitch=%g (%.1f°), yaw=%g (%.1f°))' % (
self.roll, degrees(self.roll), self.pitch, degrees(self.pitch),
self.yaw, degrees(self.yaw))
[docs]class SenseIMU(object):
"""
The :class:`SenseIMU` class represents the Inertial Measurement Unit (IMU)
on the Sense HAT. Users can either instantiate the class themselves, or can
access an instance from :attr:`SenseHAT.imu`.
The *settings* parameter can be used to point to alternate settings files
but it is strongly recommended you leave this at the default as this can
affect the calibration of the IMU.
If the *emulate* parameter is ``True``, the instance will connect to the
IMU in the `desktop Sense HAT emulator`_ instead of the "real" Sense HAT
IMU.
.. _desktop Sense HAT emulator: https://sense-emu.readthedocs.io/
"""
__slots__ = (
'_rotation',
'_settings',
'_imu',
'_interval',
'_sensors',
'_readings',
'_last_read',
)
def __init__(self, settings=None, emulate=False):
if emulate:
from sense_emu import RTIMU
else:
import RTIMU
if not isinstance(settings, SenseSettings):
settings = SenseSettings(settings)
self._settings = settings
self._imu = RTIMU.RTIMU(self._settings.settings)
if not self._imu.IMUInit():
raise RuntimeError('IMU initialization failed')
self._interval = self._imu.IMUGetPollInterval() / 1000.0 # seconds
self._imu.setCompassEnable(True)
self._imu.setGyroEnable(True)
self._imu.setAccelEnable(True)
self._sensors = frozenset(('compass', 'gyro', 'accel'))
self._readings = IMUState(
IMUVector(None, None, None),
IMUVector(None, None, None),
IMUVector(None, None, None),
IMUOrient(None, None, None)
)
self._rotation = 0
self._last_read = None
[docs] def close(self):
"""
Call the :meth:`close` method to close the inertial measurement unit
interface and free up any background resources. The method is
idempotent (you can call it multiple times without error) and after it
is called, any operations on the inertial measurement unit may return
an error (but are not guaranteed to do so).
"""
self._imu = None
self._settings = None
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, exc_tb):
self.close()
def __iter__(self):
while True:
value = self.read()
if value.orient is not None:
yield value
[docs] def read(self):
"""
Return the current state of the inertial measurement unit as an
:class:`IMUState` tuple.
.. note::
This method will wait until the next set of readings are available,
and then return them. Hence it is suitable for use in a loop
without additional waits, although it may be simpler to simply
treat the instance as an iterator in that case.
This is in contrast to reading the :attr:`gyro`, :attr:`accel`,
:attr:`compass`, and :attr:`orient` attributes which always return
immediately.
"""
self._read(True)
return self._readings
def _read(self, wait):
now = time.time()
if self._last_read is not None:
if wait:
time.sleep(max(0.0, self._interval - (now - self._last_read)))
elif now - self._last_read < self._interval:
return
if self._imu.IMURead():
dat = self._imu.getIMUData()
self._readings = IMUState(
IMUVector(*self._rotate(*dat['compass']))
if dat.get('compassValid', False) else None,
IMUVector(*self._rotate(*dat['gyro']))
if dat.get('gyroValid', False) else None,
IMUVector(*self._rotate(*dat['accel']))
if dat.get('accelValid', False) else None,
# TODO what about rotation for fusion-pose?
IMUOrient(*dat['fusionPose'])
if dat.get('fusionPoseValid', False) else None,
)
self._last_read = now
def _rotate(self, x, y, z):
return {
0: (x, y, z),
90: (-y, x, z),
180: (-x, -y, z),
270: (y, -x, z),
}[self._rotation]
@property
def name(self):
"""
Returns the name of the IMU chip. On the Sense HAT this should always
be "LSM9DS1".
"""
return self._imu.IMUName()
@property
def compass(self):
"""
Return the current reading from the magnetometer as a 3-dimensional
:class:`IMUVector` tuple. The reading is measured in in µT
(`micro-teslas`_).
.. _micro-teslas: https://en.wikipedia.org/wiki/Tesla_(unit)
"""
self._read(False)
return self._readings.compass
@property
def gyro(self):
"""
Return the current reading from the gyroscope as a 3-dimensional
:class:`IMUVector` tuple. The reading is measured in
`radians-per-second`_.
.. _radians-per-second: https://en.wikipedia.org/wiki/Radian_per_second
"""
self._read(False)
return self._readings.gyro
@property
def accel(self):
"""
Return the current reading from the accelerometer as a 3-dimensional
:class:`IMUVector` tuple. The reading is measured in
`standard gravities`_.
.. _standard gravities: https://en.wikipedia.org/wiki/Standard_gravity
"""
self._read(False)
return self._readings.accel
@property
def orient(self):
"""
Return the current calculated orientation of the board as a
:class:`IMUOrient` tuple containing `roll, pitch, and yaw`_ in
`radians`_.
.. note::
The sensors that are used in determining the orientation are
specified in the :attr:`sensors` property.
The orientation of the board is only calculated when the sensors
are read. The drift of certain sensors (the gyroscope in
particular) mean that reading the orientation more frequently can
result in greater accuracy.
.. _radians: https://en.wikipedia.org/wiki/Radian
.. _roll, pitch, and yaw: https://en.wikipedia.org/wiki/Aircraft_principal_axes
"""
self._read(False)
return self._readings.orient
@property
def sensors(self):
"""
Controls which sensors are used for calculating the :attr:`orient`
property.
"""
return self._sensors
@sensors.setter
def sensors(self, value):
if isinstance(value, (bytes, str)):
value = {value}
value = {
s.decode('ascii')
if isinstance(s, bytes) else s for s in value
}
clean = {'compass', 'gyro', 'accel'} & set(value)
if clean != value:
raise ValueError('invalid sensor "%s"' % (value - clean).pop())
self._sensors = frozenset(clean)
self._imu.setCompassEnable('compass' in self._sensors)
self._imu.setGyroEnable('gyro' in self._sensors)
self._imu.setAccelEnable('accel' in self._sensors)
@property
def rotation(self):
"""
Specifies the rotation about the Z axis applied to IMU readings as a
multiple of 90 degrees. When rotation is 0 (the default), positive X
is toward the joystick, and positive Y is away from the GPIO pins:
.. image:: images/rotation_0.*
When rotation is 90, positive X is toward the GPIO pins, and positive
Y is toward the joystick:
.. image:: images/rotation_90.*
The other two rotations are trivial to derive from this.
.. note::
This property is updated by the unifying :attr:`SenseHAT.rotation`
attribute.
"""
return self._rotation
@rotation.setter
def rotation(self, value):
# TODO If rotation is modified we should update the current
# self._readings
if value % 90:
raise ValueError('rotation must be a multiple of 90')
self._rotation = value % 360