navx.py

from multiprocessing import Process, Pipe, Lock
import time
import logging

if __name__ == "__main__":
    from ahrs import AHRS
    from process_commands import ProcessCommands
    from units import *
    from ourlogging import setup_logging
else:
    from .ahrs import AHRS
    from .process_commands import ProcessCommands
    from .units import *
    from .ourlogging import setup_logging


setup_logging(__file__)
logger = logging.getLogger(__name__)


class get_navx:
    '''
        Controlled environment for reading from the NavX.
    '''
    def __init__(self, update_rate_hz=None):
        self.update_rate_hz = update_rate_hz

    def __enter__(self):
        if self.update_rate_hz is None:
            self.navx = NavX()
        else:
            self.navx = NavX(self.update_rate_hz)
        return self.navx

    def __exit__(self, type, value, traceback):
        self.navx.free()


class NavX:
    def __init__(self, update_rate_hz=None):
        if update_rate_hz is None:
            self.ahrs = AHRS()
        else:
            self.ahrs = AHRS(update_rate_hz=update_rate_hz)
        self.start()

    def start(self):
        self.parent_conn, child_conn = Pipe()
        self.lock = Lock()
        self.p = Process(target=self.ahrs.response, name="NavX", args=(child_conn,))
        self.p.start()

    def stop(self):
        self.parent_conn.send(ProcessCommands.STOP)

    def free(self):
        self.parent_conn.send(ProcessCommands.FREE)

        self.p.join(timeout=5)
        if self.p.is_alive():
            self.p.terminate()

    def get_last_time_stamp(self):
        '''
            Returns the last time stamp reported by the sensor.

            :return The last time stamp reported by the sensor
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.TIME)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_pitch(self):
        '''
            Returns the current pitch value (in degrees, from -180 to 180)
            reported by the sensor.  Pitch is a measure of rotation around
            the X Axis.

            :return The current pitch value in degrees (-180 to 180).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.PITCH)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_roll(self):
        '''
            Returns the current roll value (in degrees, from -180 to 180)
            reported by the sensor.  Roll is a measure of rotation around
            the X Axis.

            :return The current roll value in degrees (-180 to 180).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.ROLL)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_yaw(self):
        '''
            Returns the current yaw value (in degrees, from -180 to 180)
            reported by the sensor.  Yaw is a measure of rotation around
            the Z Axis (which is perpendicular to the earth).

            Note that the returned yaw value will be offset by a user-specified
            offset value; this user-specified offset value is set by
            invoking the zero_yaw() method.

            :return The current yaw value in degrees (-180 to 180).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.YAW)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_compass_heading(self):
        '''
            Returns the current tilt-compensated compass heading
            value (in degrees, from 0 to 360) reported by the sensor.

            Note that this value is sensed by a magnetometer,
            which can be affected by nearby magnetic fields (e.g., the
            magnetic fields generated by nearby motors).

            Before using this value, ensure that (a) the magnetometer
            has been calibrated and (b) that a magnetic disturbance is
            not taking place at the instant when the compass heading
            was generated.
            :return The current tilt-compensated compass heading, in degrees (0-360).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.COMPASS_HEADING)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def zero_yaw(self):
        '''
            Sets the user-specified yaw offset to the current
            yaw value reported by the sensor.

            This user-specified yaw offset is automatically
            subtracted from subsequent yaw values reported by
            the get_yaw() method.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.ZERO_YAW)
        response = self.parent_conn.recv()
        assert response == "ok"
        self.lock.release()

    def is_calibrating(self):
        '''
            Returns true if the sensor is currently performing automatic
            gyro/accelerometer calibration.  Automatic calibration occurs
            when the sensor is initially powered on, during which time the
            sensor should be held still, with the Z-axis pointing up
            (perpendicular to the earth).

            NOTE:  During this automatic calibration, the yaw, pitch and roll
            values returned may not be accurate.

            Once calibration is complete, the sensor will automatically remove
            an internal yaw offset value from all reported values.

            :return Returns true if the sensor is currently automatically
            calibrating the gyro and accelerometer sensors.
        '''
        self.parent_conn.send(ProcessCommands.IS_CALIBRATING)
        return self.parent_conn.recv()

    def is_connected(self):
        '''
            Indicates whether the sensor is currently connected
            to the host computer.  A connection is considered established
            whenever communication with the sensor has occurred recently.

            :returns: Returns true if a valid update has been recently received
                  from the sensor.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.IS_CONNECTED)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_byte_count(self):
        '''
            Returns the count in bytes of data received from the
            sensor. This could can be useful for diagnosing
            connectivity issues.

            If the byte count is increasing, but the update count
            (see :meth:`get_update_count`) is not, this indicates a software
            misconfiguration.

            :returns: The number of bytes received from the sensor.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.BYTE_COUNT)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_update_count(self):
        '''
            Returns the count of valid updates which have
            been received from the sensor.  This count should increase
            at the same rate indicated by the configured update rate.

            :returns: The number of valid updates received from the sensor.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.UPDATE_COUNT)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_world_linear_accel_x(self):
        '''
            Returns the current linear acceleration in the X-axis (in G).

            World linear acceleration refers to raw acceleration data, which
            has had the gravity component removed, and which has been rotated to
            the same reference frame as the current yaw value.  The resulting
            value represents the current acceleration in the x-axis of the
            body (e.g., the robot) on which the sensor is mounted.

            :return Current world linear acceleration in the X-axis (in G).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.WORLD_LINEAR_ACCEL_X)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_world_linear_accel_y(self):
        '''
            Returns the current linear acceleration in the Y-axis (in G).

            World linear acceleration refers to raw acceleration data, which
            has had the gravity component removed, and which has been rotated to
            the same reference frame as the current yaw value.  The resulting
            value represents the current acceleration in the Y-axis of the
            body (e.g., the robot) on which the sensor is mounted.

            :return Current world linear acceleration in the Y-axis (in G).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.WORLD_LINEAR_ACCEL_Y)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_world_linear_accel_z(self):
        '''
            Returns the current linear acceleration in the Z-axis (in G).

            World linear acceleration refers to raw acceleration data, which
            has had the gravity component removed, and which has been rotated to
            the same reference frame as the current yaw value.  The resulting
            value represents the current acceleration in the Z-axis of the
            body (e.g., the robot) on which the sensor is mounted.

            :return Current world linear acceleration in the Z-axis (in G).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.WORLD_LINEAR_ACCEL_Z)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def is_moving(self):
        '''
            Indicates if the sensor is currently detecting motion,
            based upon the X and Y-axis world linear acceleration values.
            If the sum of the absolute values of the X and Y axis exceed
            a "motion threshold", the motion state is indicated.

            :return Returns true if the sensor is currently detecting motion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.IS_MOVING)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def is_rotating(self):
        '''
            Indicates if the sensor is currently detecting yaw rotation,
            based upon whether the change in yaw over the last second
            exceeds the "Rotation Threshold."

            Yaw Rotation can occur either when the sensor is rotating, or
            when the sensor is not rotating AND the current gyro calibration
            is insufficiently calibrated to yield the standard yaw drift rate.

            :return Returns true if the sensor is currently detecting motion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.IS_ROTATING)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def getBarometricPressure(self):
        '''
            Returns the current barometric pressure, based upon calibrated readings
            from the onboard pressure sensor.  This value is in units of millibar.

            NOTE:  This value is only valid for a navX Aero.  To determine
            whether this value is valid, see is_altitude_valid().

            :return Returns current barometric pressure (navX Aero only).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.BAROMETRIC_PRESSUE)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def getAltitude(self):
        '''
            Returns the current altitude, based upon calibrated readings
            from a barometric pressure sensor, and the currently-configured
            sea-level barometric pressure [navX Aero only].  This value is in units of meters.

            NOTE:  This value is only valid sensors including a pressure
            sensor.  To determine whether this value is valid, see
            is_altitude_valid().

            :return Returns current altitude in meters (as long as the sensor includes
            an installed on-board pressure sensor).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.ALTITUDE)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def is_altitude_valid(self):
        '''
            Indicates whether the current altitude (and barometric pressure) data is
            valid. This value will only be true for a sensor with an onboard
            pressure sensor installed.

            If this value is false for a board with an installed pressure sensor,
            this indicates a malfunction of the onboard pressure sensor.

            :return Returns true if a working pressure sensor is installed.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.ALTITUDE_VALID)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_fused_heading(self):
        '''
            Returns the "fused" (9-axis) heading.

            The 9-axis heading is the fusion of the yaw angle, the tilt-corrected
            compass heading, and magnetic disturbance detection.  Note that the
            magnetometer calibration procedure is required in order to
            achieve valid 9-axis headings.

            The 9-axis Heading represents the sensor's best estimate of current heading,
            based upon the last known valid Compass Angle, and updated by the change in the
            Yaw Angle since the last known valid Compass Angle.  The last known valid Compass
            Angle is updated whenever a Calibrated Compass Angle is read and the sensor
            has recently rotated less than the Compass Noise Bandwidth (~2 degrees).

            :return Fused Heading in Degrees (range 0-360)
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.FUSED_HEADING)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def is_magnetic_disturbance(self):
        '''
            Indicates whether the current magnetic field strength diverges from the
            calibrated value for the earth's magnetic field by more than the currently-
            configured Magnetic Disturbance Ratio.

            This function will always return false if the sensor's magnetometer has
            not yet been calibrated; see is_magnetometer_calibrated().

            :return true if a magnetic disturbance is detected (or the magnetometer is uncalibrated).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.MAGNETIC_DISTURBANCE)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def is_magnetometer_calibrated(self):
        '''
            Indicates whether the magnetometer has been calibrated.

            Magnetometer Calibration must be performed by the user.

            Note that if this function does indicate the magnetometer is calibrated,
            this does not necessarily mean that the calibration quality is sufficient
            to yield valid compass headings.

            :return Returns true if magnetometer calibration has been performed.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.IS_MAGNETOMETER_CALIBRATED)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    # Unit Quaternions

    def get_quaternion_w(self):
        '''
            Returns the imaginary portion (W) of the Orientation Quaternion which
            fully describes the current sensor orientation with respect to the
            reference angle defined as the angle at which the yaw was last "zeroed".

            Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
            to 2.  This total range (4) can be associated with a unit circle, since
            each circle is comprised of 4 PI Radians.

            For more information on Quaternions and their use, please see this
            <a href=https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>definition</a>.
            :return Returns the imaginary portion (W) of the quaternion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.QUATERNION_W)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_quaternion_x(self):
        '''
            Returns the real portion (X axis) of the Orientation Quaternion which
            fully describes the current sensor orientation with respect to the
            reference angle defined as the angle at which the yaw was last "zeroed".

            Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
            to 2.  This total range (4) can be associated with a unit circle, since
            each circle is comprised of 4 PI Radians.

            For more information on Quaternions and their use, please see this
            <a href=https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>description</a>.
            :return Returns the real portion (X) of the quaternion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.QUATERNION_X)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_quaternion_y(self):
        '''
            Returns the real portion (X axis) of the Orientation Quaternion which
            fully describes the current sensor orientation with respect to the
            reference angle defined as the angle at which the yaw was last "zeroed".

            Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
            to 2.  This total range (4) can be associated with a unit circle, since
            each circle is comprised of 4 PI Radians.

            For more information on Quaternions and their use, please see:

            https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

            :return Returns the real portion (X) of the quaternion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.QUATERNION_Y)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_quaternion_z(self):
        '''
            Returns the real portion (X axis) of the Orientation Quaternion which
            fully describes the current sensor orientation with respect to the
            reference angle defined as the angle at which the yaw was last "zeroed".

            Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
            to 2.  This total range (4) can be associated with a unit circle, since
            each circle is comprised of 4 PI Radians.

            For more information on Quaternions and their use, please see:

            https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

            :return Returns the real portion (X) of the quaternion.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.QUATERNION_Z)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def reset_displacement(self):
        '''
            Zeros the displacement integration variables.   Invoke this at the moment when
            integration begins.
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.IS_CALIBRATING)
        response = self.parent_conn.recv()
        self.lock.release()
        assert response == "ok"

    def get_velocity_x(self):
        '''
            Returns the velocity (in meters/sec) of the X axis [Experimental].

            NOTE:  This feature is experimental.  Velocity measures rely on integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting velocities are not known to be very accurate.
            :return Current Velocity (in meters/squared).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.VELOCITY_X)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_velocity_y(self):
        '''
            Returns the velocity (in meters/sec) of the Y axis [Experimental].

            NOTE:  This feature is experimental.  Velocity measures rely on integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting velocities are not known to be very accurate.
            :return Current Velocity (in meters/squared).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.VELOCITY_Y)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_velocity_z(self):
        '''
            Returns the velocity (in meters/sec) of the Z axis [Experimental].

            NOTE:  This feature is experimental.  Velocity measures rely on integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting velocities are not known to be very accurate.
            :return Current Velocity (in meters/squared).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.VELOCITY_Z)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_displacement_x(self):
        '''
            Returns the displacement (in meters) of the X axis since reset_displacement()
            was last invoked [Experimental].

            NOTE:  This feature is experimental.  Displacement measures rely on double-integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting displacement are not known to be very accurate, and the amount of error
            increases quickly as time progresses.
            :return Displacement since last reset (in meters).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.DISPLACEMENT_X)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_displacement_y(self):
        '''
            Returns the displacement (in meters) of the Y axis since reset_displacement()
            was last invoked [Experimental].

            NOTE:  This feature is experimental.  Displacement measures rely on double-integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting displacement are not known to be very accurate, and the amount of error
            increases quickly as time progresses.
            :return Displacement since last reset (in meters).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.DISPLACEMENT_Y)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_displacement_z(self):
        '''
            Returns the displacement (in meters) of the Z axis since reset_displacement()
            was last invoked [Experimental].

            NOTE:  This feature is experimental.  Displacement measures rely on double-integration
            of acceleration values from MEMS accelerometers which yield "noisy" values.  The
            resulting displacement are not known to be very accurate, and the amount of error
            increases quickly as time progresses.
            :return Displacement since last reset (in meters).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.DISPLACEMENT_Z)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

    def get_temp(self):
        '''
            Returns the current temperature (in degrees centigrade) reported by
            the sensor's gyro/accelerometer circuit.

            This value may be useful in order to perform advanced temperature-
            correction of raw gyroscope and accelerometer values.

            :return The current temperature (in degrees centigrade).
        '''
        self.lock.acquire()
        self.parent_conn.send(ProcessCommands.TEMP_C)
        response = self.parent_conn.recv()
        self.lock.release()
        return response

if __name__ == "__main__":
    with get_navx(10) as navx:
        time.sleep(1)
        while True:
            time.sleep(1)
            print("Time: {}, Yaw: {}, Pitch: {}, Roll: {}".format(navx.get_last_time_stamp().to(Time.s),
                                                                  navx.get_yaw().to(Angle.degree),
                                                                  navx.get_pitch().to(Angle.degree),
                                                                  navx.get_roll().to(Angle.degree)))