pynq.iop.grove_imu — Python productivity for Zynq (Pynq) v1.0

# Source code for pynq.iop.grove_imu

    #   Copyright (c) 2016, Xilinx, Inc.
    #   All rights reserved.
    #
    #   Redistribution and use in source and binary forms, with or without
    #   modification, are permitted provided that the following conditions are met:
    #
    #   1.  Redistributions of source code must retain the above copyright notice,
    #       this list of conditions and the following disclaimer.
    #
    #   2.  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.
    #
    #   3.  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.

    __author__      = "Yun Rock Qu"
    __copyright__   = "Copyright 2016, Xilinx"
    __email__       = "pynq_support@xilinx.com"

    import struct
    import math
    from time import sleep
    from pynq import MMIO
    from pynq.iop import request_iop
    from pynq.iop import iop_const
    from pynq.iop import PMODA
    from pynq.iop import PMODB
    from pynq.iop import ARDUINO
    from pynq.iop import PMOD_GROVE_G3
    from pynq.iop import PMOD_GROVE_G4
    from pynq.iop import ARDUINO_GROVE_I2C

    PMOD_GROVE_IMU_PROGRAM = "pmod_grove_imu.bin"
    ARDUINO_GROVE_IMU_PROGRAM = "arduino_grove_imu.bin"

    class Grove_IMU(object):
        """This class controls the Grove IIC IMU.

        Grove IMU 10DOF is a combination of grove IMU 9DOF (MPU9250) and grove
        barometer sensor (BMP180). MPU-9250 is a 9-axis motion tracking device
        that combines a 3-axis gyroscope, 3-axis accelerometer, 3-axis
        magnetometer and a Digital Motion Processor (DMP). BMP180 is a high
        precision, low power digital pressure sensor. Hardware version: v1.1.

        Attributes
        ----------
        iop : _IOP
            I/O processor instance used by Grove_IMU.
        mmio : MMIO
            Memory-mapped I/O instance to read and write instructions and data.

        """
        def __init__(self, if_id, gr_pin):
            """Return a new instance of an Grove IMU object.

            Parameters
            ----------
            if_id : int
                IOP ID (1, 2, 3) corresponding to (PMODA, PMODB, ARDUINO).
            gr_pin: list
                A group of pins on stickit connector or arduino shield.

            """
            if if_id in [PMODA, PMODB]:
                if not gr_pin in [PMOD_GROVE_G3, \
                                  PMOD_GROVE_G4]:
                    raise ValueError("IMU group number can only be G3 - G4.")
                GROVE_IMU_PROGRAM = PMOD_GROVE_IMU_PROGRAM
            elif if_id in [ARDUINO]:
                if not gr_pin in [ARDUINO_GROVE_I2C]:
                    raise ValueError("IMU group number can only be I2C.")
                GROVE_IMU_PROGRAM = ARDUINO_GROVE_IMU_PROGRAM
            else:
                raise ValueError("No such IOP for grove device.")

            self.iop = request_iop(if_id, GROVE_IMU_PROGRAM)
            self.mmio = self.iop.mmio
            self.iop.start()

            if if_id in [PMODA, PMODB]:
                # Write SCL and SDA pin config
                self.mmio.write(iop_const.MAILBOX_OFFSET, gr_pin[0])
                self.mmio.write(iop_const.MAILBOX_OFFSET+4, gr_pin[1])

                # Write configuration and wait for ACK
                self.mmio.write(iop_const.MAILBOX_OFFSET + \
                                iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 1)
                while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                                      iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 1):
                    pass

            self.reset()

        def reset(self):
            """Reset all the sensors on the grove IMU.

            Parameters
            ----------
            None

            Returns
            -------
            None

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0xF)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0xF):
                pass

        def get_accl(self):
            """Get the data from the accelerometer.

            Parameters
            ----------
            None

            Returns
            -------
            list
                A list of the acceleration data along X-axis, Y-axis, and Z-axis.

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0x3)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0x3):
                pass
            ax = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET))
            ay = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+4))
            az = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+8))
            return [float("{0:.2f}".format(ax/16384)), \
                    float("{0:.2f}".format(ay/16384)), \
                    float("{0:.2f}".format(az/16384))]

        def get_gyro(self):
            """Get the data from the gyroscope.

            Parameters
            ----------
            None

            Returns
            -------
            list
                A list of the gyro data along X-axis, Y-axis, and Z-axis.

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0x5)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0x5):
                pass
            gx = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET))
            gy = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+4))
            gz = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+8))
            return [float("{0:.2f}".format(gx*250/32768)), \
                    float("{0:.2f}".format(gy*250/32768)), \
                    float("{0:.2f}".format(gz*250/32768))]

        def get_compass(self):
            """Get the data from the magnetometer.

            Parameters
            ----------
            None

            Returns
            -------
            list
                A list of the compass data along X-axis, Y-axis, and Z-axis.

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0x7)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0x7):
                pass
            mx = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET))
            my = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+4))
            mz = self._reg2int(self.mmio.read(iop_const.MAILBOX_OFFSET+8))
            return [float("{0:.2f}".format(mx*1200/4096)), \
                    float("{0:.2f}".format(my*1200/4096)), \
                    float("{0:.2f}".format(mz*1200/4096))]

        def get_heading(self):
            """Get the value of the heading.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The angle deviated from the X-axis, toward the positive Y-axis.

            """
            [mx, my, mz] = self.get_compass()
            heading = 180 * math.atan2(my, mx) / math.pi
            if (heading < 0):
                heading += 360
            return float("{0:.2f}".format(heading))

        def get_tiltheading(self):
            """Get the value of the tilt heading.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The tilt heading value.

            """
            [ax, ay, az] = self.get_accl()
            [mx, my, mz] = self.get_compass()

            pitch = math.asin(-ax)
            roll = math.asin(ay / math.cos(pitch))
            xh = mx * math.cos(pitch) + mz * math.sin(pitch)
            yh = mx * math.sin(roll) * math.sin(pitch) + \
                    my * math.cos(roll) - mz * math.sin(roll) * math.cos(pitch)
            zh = -mx * math.cos(roll) * math.sin(pitch) + \
                    my * math.sin(roll) + mz * math.cos(roll) * math.cos(pitch)
            tiltheading = 180 * math.atan2(yh, xh) / math.pi
            if (yh < 0):
                tiltheading += 360
            return float("{0:.2f}".format(tiltheading))

        def get_temperature(self):
            """Get the current temperature in degree C.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The temperature value.

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0xB)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0xB):
                pass
            value = self.mmio.read(iop_const.MAILBOX_OFFSET)
            return self._reg2float(value)

        def get_pressure(self):
            """Get the current pressure in Pa.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The pressure value.

            """
            self.mmio.write(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET, 0xD)
            while (self.mmio.read(iop_const.MAILBOX_OFFSET + \
                            iop_const.MAILBOX_PY2IOP_CMD_OFFSET) == 0xD):
                pass
            value = self.mmio.read(iop_const.MAILBOX_OFFSET)
            return self._reg2float(value)

        def get_atm(self):
            """Get the current pressure in relative atmosphere.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The related atmosphere.

            """
            return float("{0:.2f}".format(self.get_pressure()/101325))

        def get_altitude(self):
            """Get the current altitude.

            Parameters
            ----------
            None

            Returns
            -------
            float
                The altitude value.

            """
            pressure = self.get_pressure()
            A = pressure/101325
            B = 1/5.255
            C = 1-pow(A,B)
            altitude = 44300 * C
            return float("{0:.2f}".format(altitude))

        def _reg2float(self, reg):
            """Converts 32-bit register value to floats in Python.

            Parameters
            ----------
            reg: int
                A 32-bit register value read from the mailbox.

            Returns
            -------
            float
                A float number translated from the register value.

            """
            if reg == 0:
                return 0.0
            sign = (reg & 0x80000000) >> 31 & 0x01
            exp = ((reg & 0x7f800000) >> 23)-127
            if (exp == 0):
                man = (reg & 0x007fffff)/pow(2,23)
            else:
                man = 1+(reg & 0x007fffff)/pow(2,23)
            result = pow(2,exp)*(man)*((sign*-2) +1)
            return float("{0:.2f}".format(result))

        def _reg2int(self, reg):
            """Converts 32-bit register value to signed integer in Python.

            Parameters
            ----------
            reg: int
                A 32-bit register value read from the mailbox.

            Returns
            -------
            int
                A signed integer translated from the register value.

            """
            result = -(reg>>31 & 0x1)*(1<<31)
            for i in range(31):
                result += (reg>>i & 0x1)*(1<<i)
            return result